|
| 1 | +// Copyright (c) 2025, b-robotized |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +// |
| 16 | +// Source of this file are templates in |
| 17 | +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. |
| 18 | +// |
| 19 | + |
| 20 | +#ifndef VDA5050_SAFETY_STATE_BROADCASTER__VDA5050_SAFETY_STATE_BROADCASTER_HPP_ |
| 21 | +#define VDA5050_SAFETY_STATE_BROADCASTER__VDA5050_SAFETY_STATE_BROADCASTER_HPP_ |
| 22 | + |
| 23 | +#include <memory> |
| 24 | +#include <string> |
| 25 | +#include <unordered_map> |
| 26 | +#include <vector> |
| 27 | + |
| 28 | +#include "controller_interface/controller_interface.hpp" |
| 29 | +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" |
| 30 | +#include "rclcpp_lifecycle/state.hpp" |
| 31 | +#include "realtime_tools/realtime_buffer.hpp" |
| 32 | +#include "realtime_tools/realtime_publisher.hpp" |
| 33 | + |
| 34 | +#include <vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster_parameters.hpp> |
| 35 | +#include "control_msgs/msg/vda5050_safety_state.hpp" |
| 36 | + |
| 37 | +namespace vda5050_safety_state_broadcaster |
| 38 | +{ |
| 39 | + |
| 40 | +/** |
| 41 | + * \brief VDA5050 safety state broadcaster for all or some state in a ros2_control system. |
| 42 | + * |
| 43 | + * Vda5050SafetyStateBroadcaster publishes state interfaces from ros2_control as ROS messages. |
| 44 | + * The state interfaces published can be configured via parameters: |
| 45 | + * |
| 46 | + * \param fieldViolation_interfaces that are used to acknowledge field violation events by setting |
| 47 | + * the interface to 1.0. |
| 48 | + * \param eStop_manual_interfaces that are used to manually acknowledge eStop events by setting the |
| 49 | + * interface to 1.0. |
| 50 | + * \param eStop_remote_interfaces that are used to remotely acknowledge eStop events by setting the |
| 51 | + * interface to 1.0. |
| 52 | + * \param eStop_autoack_interfaces that are used to autoacknowledge eStop events by setting the |
| 53 | + * interface to 1.0. |
| 54 | + * |
| 55 | + * Publishes to: |
| 56 | + * |
| 57 | + * - \b vda5050_safety_state (control_msgs::msg::VDA5050SafetyState): safety state of the combined |
| 58 | + * safety interfaces according the priority: eStop_manual > eStop_remote > eStop_autoack. |
| 59 | + * |
| 60 | + */ |
| 61 | +class Vda5050SafetyStateBroadcaster : public controller_interface::ControllerInterface |
| 62 | +{ |
| 63 | +public: |
| 64 | + Vda5050SafetyStateBroadcaster(); |
| 65 | + |
| 66 | + controller_interface::InterfaceConfiguration command_interface_configuration() const override; |
| 67 | + |
| 68 | + controller_interface::InterfaceConfiguration state_interface_configuration() const override; |
| 69 | + |
| 70 | + controller_interface::CallbackReturn on_init() override; |
| 71 | + |
| 72 | + controller_interface::CallbackReturn on_configure( |
| 73 | + const rclcpp_lifecycle::State & previous_state) override; |
| 74 | + |
| 75 | + controller_interface::CallbackReturn on_activate( |
| 76 | + const rclcpp_lifecycle::State & previous_state) override; |
| 77 | + |
| 78 | + controller_interface::CallbackReturn on_deactivate( |
| 79 | + const rclcpp_lifecycle::State & previous_state) override; |
| 80 | + |
| 81 | + controller_interface::return_type update( |
| 82 | + const rclcpp::Time & time, const rclcpp::Duration & period) override; |
| 83 | + |
| 84 | +protected: |
| 85 | + vda5050_safety_state_broadcaster::Params params_; |
| 86 | + |
| 87 | + std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::VDA5050SafetyState>> |
| 88 | + realtime_vda5050_safety_state_publisher_; |
| 89 | + |
| 90 | +private: |
| 91 | + std::shared_ptr<vda5050_safety_state_broadcaster::ParamListener> param_listener_; |
| 92 | + std::shared_ptr<rclcpp::Publisher<control_msgs::msg::VDA5050SafetyState>> |
| 93 | + vda5050_safety_state_publisher_; |
| 94 | + |
| 95 | + /** |
| 96 | + * @brief Determines the current E-stop state based on the state interfaces. |
| 97 | + * @return The E-stop type as defined in control_msgs::msg::VDA5050SafetyState. |
| 98 | + */ |
| 99 | + control_msgs::msg::VDA5050SafetyState::_e_stop_type determineEstopState(); |
| 100 | + |
| 101 | + struct InterfaceIds |
| 102 | + { |
| 103 | + int manual_start = 0; |
| 104 | + int remote_start = 0; |
| 105 | + int autoack_start = 0; |
| 106 | + int total_interfaces = 0; |
| 107 | + }; |
| 108 | + |
| 109 | + InterfaceIds itfs_ids_; |
| 110 | + bool fieldViolation_value = false; |
| 111 | + std::string estop_msg = control_msgs::msg::VDA5050SafetyState::NONE; |
| 112 | + |
| 113 | + /** |
| 114 | + * @brief Safely converts a double value to bool, treating NaN as false. |
| 115 | + * @param value The double value to convert. |
| 116 | + * @return true if value is not NaN and not zero, false otherwise. |
| 117 | + */ |
| 118 | + bool safe_double_to_bool(double value) const |
| 119 | + { |
| 120 | + if (std::isnan(value)) |
| 121 | + { |
| 122 | + return false; |
| 123 | + } |
| 124 | + return value != 0.0; |
| 125 | + } |
| 126 | +}; |
| 127 | + |
| 128 | +} // namespace vda5050_safety_state_broadcaster |
| 129 | + |
| 130 | +#endif // VDA5050_SAFETY_STATE_BROADCASTER__VDA5050_SAFETY_STATE_BROADCASTER_HPP_ |
0 commit comments