Add VDA5050 Safety State Broadcaster

ros2_controllers/package.xml

@@ -41,6 +41,7 @@
   <exec_depend>steering_controllers_library</exec_depend>
   <exec_depend>tricycle_controller</exec_depend>
   <exec_depend>tricycle_steering_controller</exec_depend>
+  <exec_depend>vda5050_safety_state_broadcaster</exec_depend>
   <exec_depend>velocity_controllers</exec_depend>
 
   <export>

vda5050_safety_state_broadcaster/CMakeLists.txt

@@ -0,0 +1,91 @@
+cmake_minimum_required(VERSION 3.8)
+project(vda5050_safety_state_broadcaster)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  builtin_interfaces
+  control_msgs
+  controller_interface
+  generate_parameter_library
+  pluginlib
+  rclcpp_lifecycle
+  realtime_tools
+  urdf
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(vda5050_safety_state_broadcaster_parameters
+  src/vda5050_safety_state_broadcaster.yaml
+)
+
+add_library(vda5050_safety_state_broadcaster SHARED
+  src/vda5050_safety_state_broadcaster.cpp
+)
+
+target_compile_features(vda5050_safety_state_broadcaster PUBLIC cxx_std_17)
+target_include_directories(vda5050_safety_state_broadcaster
+  PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/vda5050_safety_state_broadcaster>
+)
+target_link_libraries(vda5050_safety_state_broadcaster PUBLIC
+                      vda5050_safety_state_broadcaster_parameters
+                      controller_interface::controller_interface
+                      pluginlib::pluginlib
+                      rclcpp::rclcpp
+                      rclcpp_lifecycle::rclcpp_lifecycle
+                      realtime_tools::realtime_tools
+                      ${control_msgs_TARGETS}
+                      ${builtin_interfaces_TARGETS})
+
+
+pluginlib_export_plugin_description_file(controller_interface vda5050_safety_state_broadcaster.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_vda5050_safety_state_broadcaster test/test_load_vda5050_safety_state_broadcaster.cpp)
+  target_include_directories(test_load_vda5050_safety_state_broadcaster PRIVATE include)
+  target_link_libraries(test_load_vda5050_safety_state_broadcaster
+    vda5050_safety_state_broadcaster
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(test_vda5050_safety_state_broadcaster
+    test/test_vda5050_safety_state_broadcaster.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/vda5050_safety_state_broadcaster_params.yaml)
+  target_include_directories(test_vda5050_safety_state_broadcaster PRIVATE include)
+  target_link_libraries(test_vda5050_safety_state_broadcaster
+    vda5050_safety_state_broadcaster
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/vda5050_safety_state_broadcaster
+)
+install(
+  TARGETS
+    vda5050_safety_state_broadcaster
+    vda5050_safety_state_broadcaster_parameters
+  EXPORT export_vda5050_safety_state_broadcaster
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_vda5050_safety_state_broadcaster HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

vda5050_safety_state_broadcaster/doc/userdoc.rst

@@ -0,0 +1,65 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/vda5050_safety_state_broadcaster/doc/userdoc.rst
+
+.. _vda5050_safety_state_broadcaster_userdoc:
+
+VDA5050 Safety State Broadcaster
+--------------------------------
+The *VDA5050 Safety State Broadcaster* publishes safety state information as ``control_msgs/msg/VDA5050SafetyState`` messages, as defined by the VDA5050 standard.
+
+It reads safety-related state interfaces from a ros2_control system and exposes them in a standard ROS 2 message format. This enables easy integration with VDA5050-compliant systems, safety monitoring, and higher-level fleet management.
+
+Interfaces
+^^^^^^^^^^
+
+The broadcaster can read the following state interfaces, configured via parameters:
+
+- ``fieldViolation_interfaces`` (string_array)
+- ``eStop_manual_interfaces`` (string_array)
+- ``eStop_remote_interfaces`` (string_array)
+- ``eStop_autoack_interfaces`` (string_array)
+
+Published Topics
+^^^^^^^^^^^^^^^^
+
+The broadcaster publishes the following topic:
+
+- ``~/vda5050_safety_state`` (``control_msgs/msg/VDA5050SafetyState``)
+  Publishes the **combined safety state** of the system, reflecting the current field violation and E-stop status according to the configured interfaces and their priorities.
+
+Message Fields
+^^^^^^^^^^^^^^
+
+The published ``VDA5050SafetyState`` message contains:
+
++--------------------+-------------------------------------------------------------------------------------------------------+
+| Field              | Description                                                                                           |
++====================+=======================================================================================================+
+| ``field_violation``| True if any field violation interface is active                                                       |
++--------------------+-------------------------------------------------------------------------------------------------------+
+| ``e_stop``         | E-stop state, one of:                                                                                 |
+|                    |                                                                                                       |
+|                    | - ``none``: No E-stop active                                                                          |
+|                    | - ``manual``: Any Manual E-stop Interface triggered                                                   |
+|                    | - ``remote``: Any Remote E-stop Interface triggered and manual is not active                          |
+|                    | - ``autoAck``: Any Auto-acknowledged E-stop Interface triggered and manual and remote are not active  |
++--------------------+-------------------------------------------------------------------------------------------------------+
+
+The E-stop state is determined by the first active interface in the following priority:
+``manual > remote > autoAck > none``.
+
+Parameters
+^^^^^^^^^^
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to manage parameters.
+The parameter `definition file <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/vda5050_safety_state_broadcaster/src/vda5050_safety_state_broadcaster.yaml>`_ contains the full list and descriptions.
+
+List of parameters
+==================
+.. generate_parameter_library_details:: ../src/vda5050_safety_state_broadcaster.yaml
+
+Example Parameter File
+======================
+
+An example parameter file for this controller is available in the `test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/vda5050_safety_state_broadcaster/test/vda5050_safety_state_broadcaster_params.yaml>`_:
+
+.. literalinclude:: ../test/vda5050_safety_state_broadcaster_params.yaml
+   :language: yaml

vda5050_safety_state_broadcaster/include/vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster.hpp

@@ -0,0 +1,130 @@
+// Copyright (c) 2025, b-robotized
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+//
+// Source of this file are templates in
+// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
+//
+
+#ifndef VDA5050_SAFETY_STATE_BROADCASTER__VDA5050_SAFETY_STATE_BROADCASTER_HPP_
+#define VDA5050_SAFETY_STATE_BROADCASTER__VDA5050_SAFETY_STATE_BROADCASTER_HPP_
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+
+#include <vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster_parameters.hpp>
+#include "control_msgs/msg/vda5050_safety_state.hpp"
+
+namespace vda5050_safety_state_broadcaster
+{
+
+/**
+ * \brief VDA5050 safety state broadcaster for all or some state in a ros2_control system.
+ *
+ * Vda5050SafetyStateBroadcaster publishes state interfaces from ros2_control as ROS messages.
+ * The state interfaces published can be configured via parameters:
+ *
+ * \param fieldViolation_interfaces that are used to acknowledge field violation events by setting
+ * the interface to 1.0.
+ * \param eStop_manual_interfaces that are used to manually acknowledge eStop events by setting the
+ * interface to 1.0.
+ * \param eStop_remote_interfaces that are used to remotely acknowledge eStop events by setting the
+ * interface to 1.0.
+ * \param eStop_autoack_interfaces that are used to autoacknowledge eStop events by setting the
+ * interface to 1.0.
+ *
+ * Publishes to:
+ *
+ * - \b vda5050_safety_state (control_msgs::msg::VDA5050SafetyState): safety state of the combined
+ * safety interfaces according the priority: eStop_manual > eStop_remote > eStop_autoack.
+ *
+ */
+class Vda5050SafetyStateBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  Vda5050SafetyStateBroadcaster();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  vda5050_safety_state_broadcaster::Params params_;
+
+  std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::VDA5050SafetyState>>
+    realtime_vda5050_safety_state_publisher_;
+
+private:
+  std::shared_ptr<vda5050_safety_state_broadcaster::ParamListener> param_listener_;
+  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::VDA5050SafetyState>>
+    vda5050_safety_state_publisher_;
+
+  /**
+   * @brief Determines the current E-stop state based on the state interfaces.
+   * @return The E-stop type as defined in control_msgs::msg::VDA5050SafetyState.
+   */
+  control_msgs::msg::VDA5050SafetyState::_e_stop_type determineEstopState();
+
+  struct InterfaceIds
+  {
+    int manual_start = 0;
+    int remote_start = 0;
+    int autoack_start = 0;
+    int total_interfaces = 0;
+  };
+
+  InterfaceIds itfs_ids_;
+  bool fieldViolation_value = false;
+  std::string estop_msg = control_msgs::msg::VDA5050SafetyState::NONE;
+
+  /**
+   * @brief Safely converts a double value to bool, treating NaN as false.
+   * @param value The double value to convert.
+   * @return true if value is not NaN and not zero, false otherwise.
+   */
+  bool safe_double_to_bool(double value) const
+  {
+    if (std::isnan(value))
+    {
+      return false;
+    }
+    return value != 0.0;
+  }
+};
+
+}  // namespace vda5050_safety_state_broadcaster
+
+#endif  // VDA5050_SAFETY_STATE_BROADCASTER__VDA5050_SAFETY_STATE_BROADCASTER_HPP_

vda5050_safety_state_broadcaster/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>vda5050_safety_state_broadcaster</name>
+  <version>0.1.0</version>
+  <description>ros2 control VDA5050 safety state broadcaster</description>
+
+  <maintainer email="[email protected]">Bence Magyar</maintainer>
+  <maintainer email="[email protected]">Denis Štogl</maintainer>
+  <maintainer email="[email protected]">Christoph Froehlich</maintainer>
+  <maintainer email="[email protected]">Sai Kishor Kothakota</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://control.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
+
+  <author email="[email protected]">Yara Shahin</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>ros2_control_cmake</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>rclcpp</depend>
+  <depend>controller_interface</depend>
+  <depend>control_msgs</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>