diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index d1ac6ae3f1..0b32fc82dd 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -41,6 +41,7 @@ steering_controllers_library tricycle_controller tricycle_steering_controller + vda5050_safety_state_broadcaster velocity_controllers diff --git a/vda5050_safety_state_broadcaster/CMakeLists.txt b/vda5050_safety_state_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..7f59b36a10 --- /dev/null +++ b/vda5050_safety_state_broadcaster/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.8) +project(vda5050_safety_state_broadcaster) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +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 + $ + $ +) +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() diff --git a/vda5050_safety_state_broadcaster/doc/userdoc.rst b/vda5050_safety_state_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..e26ebf6450 --- /dev/null +++ b/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 `_ to manage parameters. +The parameter `definition file `_ 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 `_: + +.. literalinclude:: ../test/vda5050_safety_state_broadcaster_params.yaml + :language: yaml diff --git a/vda5050_safety_state_broadcaster/include/vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster.hpp b/vda5050_safety_state_broadcaster/include/vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster.hpp new file mode 100644 index 0000000000..fba4af08b4 --- /dev/null +++ b/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 +#include +#include +#include + +#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 +#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_vda5050_safety_state_publisher_; + +private: + std::shared_ptr param_listener_; + std::shared_ptr> + 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_ diff --git a/vda5050_safety_state_broadcaster/package.xml b/vda5050_safety_state_broadcaster/package.xml new file mode 100644 index 0000000000..c293f7c871 --- /dev/null +++ b/vda5050_safety_state_broadcaster/package.xml @@ -0,0 +1,40 @@ + + + + vda5050_safety_state_broadcaster + 0.1.0 + ros2 control VDA5050 safety state broadcaster + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Yara Shahin + + ament_cmake + + ros2_control_cmake + rosidl_default_generators + + rclcpp + controller_interface + control_msgs + + rosidl_default_runtime + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/vda5050_safety_state_broadcaster/src/vda5050_safety_state_broadcaster.cpp b/vda5050_safety_state_broadcaster/src/vda5050_safety_state_broadcaster.cpp new file mode 100644 index 0000000000..eb3bdff84c --- /dev/null +++ b/vda5050_safety_state_broadcaster/src/vda5050_safety_state_broadcaster.cpp @@ -0,0 +1,238 @@ +// 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. +// + +#include + +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" + +namespace vda5050_safety_state_broadcaster +{ +const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); +const size_t MAX_LENGTH = 64; // maximum length of strings to reserve + +Vda5050SafetyStateBroadcaster::Vda5050SafetyStateBroadcaster() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::CallbackReturn Vda5050SafetyStateBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn Vda5050SafetyStateBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + try + { + vda5050_safety_state_publisher_ = + get_node()->create_publisher( + "~/vda5050_safety_state", rclcpp::SystemDefaultsQoS()); + + realtime_vda5050_safety_state_publisher_ = + std::make_shared>( + vda5050_safety_state_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + if (!realtime_vda5050_safety_state_publisher_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Realtime publisher not initialized"); + return controller_interface::CallbackReturn::ERROR; + } + realtime_vda5050_safety_state_publisher_->lock(); + realtime_vda5050_safety_state_publisher_->msg_.e_stop.reserve(MAX_LENGTH); + realtime_vda5050_safety_state_publisher_->unlock(); + + // Initialize the indices for different interface types. + itfs_ids_ = {}; + itfs_ids_.manual_start = static_cast(params_.fieldViolation_interfaces.size()); + itfs_ids_.remote_start = + itfs_ids_.manual_start + static_cast(params_.eStop_manual_interfaces.size()); + itfs_ids_.autoack_start = + itfs_ids_.remote_start + static_cast(params_.eStop_remote_interfaces.size()); + itfs_ids_.total_interfaces = + itfs_ids_.autoack_start + static_cast(params_.eStop_autoack_interfaces.size()); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +Vda5050SafetyStateBroadcaster::command_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration +Vda5050SafetyStateBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(itfs_ids_.total_interfaces); + for (auto const & fieldViolation_interface : params_.fieldViolation_interfaces) + { + state_interfaces_config.names.push_back(fieldViolation_interface); + } + for (auto const & eStop_manual_interface : params_.eStop_manual_interfaces) + { + state_interfaces_config.names.push_back(eStop_manual_interface); + } + for (auto const & eStop_remote_interface : params_.eStop_remote_interfaces) + { + state_interfaces_config.names.push_back(eStop_remote_interface); + } + for (auto const & eStop_autoack_interface : params_.eStop_autoack_interfaces) + { + state_interfaces_config.names.push_back(eStop_autoack_interface); + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn Vda5050SafetyStateBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (state_interfaces_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No state interfaces found to publish."); + return controller_interface::CallbackReturn::FAILURE; + } + if (static_cast(itfs_ids_.total_interfaces) != state_interfaces_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Number of configured interfaces (%d) does not match number of provided state interfaces " + "(%zu).", + itfs_ids_.total_interfaces, state_interfaces_.size()); + return controller_interface::CallbackReturn::FAILURE; + } + + if (!realtime_vda5050_safety_state_publisher_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Realtime publisher not initialized"); + return controller_interface::CallbackReturn::FAILURE; + } + auto & safety_state_msg = realtime_vda5050_safety_state_publisher_->msg_; + + safety_state_msg.e_stop = control_msgs::msg::VDA5050SafetyState::NONE; + safety_state_msg.field_violation = false; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn Vda5050SafetyStateBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type Vda5050SafetyStateBroadcaster::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + fieldViolation_value = false; + for (int itf_idx = 0; itf_idx < itfs_ids_.manual_start; ++itf_idx) + { + if (safe_double_to_bool( + state_interfaces_[itf_idx].get_optional().value_or(kUninitializedValue))) + { + fieldViolation_value = true; + break; + } + } + + estop_msg = determineEstopState(); + + if ( + realtime_vda5050_safety_state_publisher_ && realtime_vda5050_safety_state_publisher_->trylock()) + { + auto & safety_state_msg = realtime_vda5050_safety_state_publisher_->msg_; + + safety_state_msg.field_violation = fieldViolation_value; + safety_state_msg.e_stop = estop_msg; + realtime_vda5050_safety_state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +control_msgs::msg::VDA5050SafetyState::_e_stop_type +Vda5050SafetyStateBroadcaster::determineEstopState() +{ + // Scan all e-stop interfaces and return the type of the first active one + for (int itf_idx = itfs_ids_.manual_start; itf_idx < itfs_ids_.total_interfaces; ++itf_idx) + { + if (safe_double_to_bool( + state_interfaces_[itf_idx].get_optional().value_or(kUninitializedValue))) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "E-stop triggered by interface %s", + state_interfaces_[itf_idx].get_name().c_str()); + if (itf_idx < itfs_ids_.remote_start) + { + return control_msgs::msg::VDA5050SafetyState::MANUAL; + } + else if (itf_idx < itfs_ids_.autoack_start) + { + return control_msgs::msg::VDA5050SafetyState::REMOTE; + } + else + { + return control_msgs::msg::VDA5050SafetyState::AUTO_ACK; + } + } + } + + return control_msgs::msg::VDA5050SafetyState::NONE; +} + +} // namespace vda5050_safety_state_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + vda5050_safety_state_broadcaster::Vda5050SafetyStateBroadcaster, + controller_interface::ControllerInterface) diff --git a/vda5050_safety_state_broadcaster/src/vda5050_safety_state_broadcaster.yaml b/vda5050_safety_state_broadcaster/src/vda5050_safety_state_broadcaster.yaml new file mode 100644 index 0000000000..8c2436de35 --- /dev/null +++ b/vda5050_safety_state_broadcaster/src/vda5050_safety_state_broadcaster.yaml @@ -0,0 +1,21 @@ +vda5050_safety_state_broadcaster: + fieldViolation_interfaces: { + type: string_array, + default_value: [], + description: "names of interfaces that are used to acknowledge field violation events by setting the interface to 1.0.", + } + eStop_manual_interfaces: { + type: string_array, + default_value: [], + description: "names of interfaces that are used to manually acknowledge eStop events by setting the interface to 1.0.", + } + eStop_remote_interfaces: { + type: string_array, + default_value: [], + description: "names of interfaces that are used to remotely acknowledge eStop events by setting the interface to 1.0.", + } + eStop_autoack_interfaces: { + type: string_array, + default_value: [], + description: "names of interfaces that are used to autoacknowledge eStop events by setting the interface to 1.0.", + } diff --git a/vda5050_safety_state_broadcaster/test/test_load_vda5050_safety_state_broadcaster.cpp b/vda5050_safety_state_broadcaster/test/test_load_vda5050_safety_state_broadcaster.cpp new file mode 100644 index 0000000000..0bb5eb1959 --- /dev/null +++ b/vda5050_safety_state_broadcaster/test/test_load_vda5050_safety_state_broadcaster.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2025, b-robotized Group +// +// 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. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadVDA5050SafetyStateBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/vda5050_safety_state_broadcaster_params.yaml"; + + cm.set_parameter({"test_vda5050_safety_state_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_vda5050_safety_state_broadcaster.type", + "vda5050_safety_state_broadcaster/VDA5050SafetyStateBroadcaster"}); + + ASSERT_NO_THROW(cm.load_controller( + "test_vda5050_safety_state_broadcaster", + "vda5050_safety_state_broadcaster/VDA5050SafetyStateBroadcaster")); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/vda5050_safety_state_broadcaster/test/test_vda5050_safety_state_broadcaster.cpp b/vda5050_safety_state_broadcaster/test/test_vda5050_safety_state_broadcaster.cpp new file mode 100644 index 0000000000..a5d780905d --- /dev/null +++ b/vda5050_safety_state_broadcaster/test/test_vda5050_safety_state_broadcaster.cpp @@ -0,0 +1,191 @@ + +// 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. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_vda5050_safety_state_broadcaster.hpp" + +// Test correct broadcaster initialization +TEST_F(VDA5050SafetyStateBroadcasterTest, init_success) { SetUpVDA5050SafetyStateBroadcaster(); } + +// Test that VDA5050SafetyStateBroadcaster parses parameters correctly and sets up state interfaces +TEST_F(VDA5050SafetyStateBroadcasterTest, all_parameters_set_configure_success) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Check interface configuration + auto cmd_if_conf = vda5050_safety_state_broadcaster_->command_interface_configuration(); + ASSERT_TRUE(cmd_if_conf.names.empty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = vda5050_safety_state_broadcaster_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_if_conf.names.size(), itfs_values_.size()); +} + +// Test fails when no defined interfaces +TEST_F(VDA5050SafetyStateBroadcasterTest, no_interfaces_set_activate_fail) +{ + ASSERT_EQ( + vda5050_safety_state_broadcaster_->init( + "test_vda5050_safety_state_broadcaster", "", 0, "", + vda5050_safety_state_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_FAILURE); +} + +// Test all message initial values +TEST_F(VDA5050SafetyStateBroadcasterTest, activate_success) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Check that the message is reset + auto msg = vda5050_safety_state_broadcaster_->realtime_vda5050_safety_state_publisher_->msg_; + EXPECT_EQ(msg.e_stop, control_msgs::msg::VDA5050SafetyState::NONE); + EXPECT_FALSE(msg.field_violation); +} + +TEST_F(VDA5050SafetyStateBroadcasterTest, deactivate_success) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(VDA5050SafetyStateBroadcasterTest, check_exported_interfaces) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = vda5050_safety_state_broadcaster_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), static_cast(0)); + + auto state_interfaces = vda5050_safety_state_broadcaster_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), itfs_values_.size()); + EXPECT_EQ(state_interfaces.names[0], "PLC_sensor1/fieldViolation"); + EXPECT_EQ(state_interfaces.names[1], "PLC_sensor2/fieldViolation"); + EXPECT_EQ(state_interfaces.names[2], "PLC_sensor1/eStopManual"); + EXPECT_EQ(state_interfaces.names[3], "PLC_sensor2/eStopManual"); + EXPECT_EQ(state_interfaces.names[4], "PLC_sensor1/eStopRemote"); + EXPECT_EQ(state_interfaces.names[5], "PLC_sensor2/eStopRemote"); + EXPECT_EQ(state_interfaces.names[6], "PLC_sensor1/eStopAutoack"); +} + +TEST_F(VDA5050SafetyStateBroadcasterTest, update_success) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->update( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +// Test correct values published for field violation and e-stop +TEST_F(VDA5050SafetyStateBroadcasterTest, publish_status_success) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->update( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + Vda5050SafetyStateMsg vda5050_safety_state_msg; + subscribe_and_get_messages(vda5050_safety_state_msg); + + EXPECT_TRUE(vda5050_safety_state_msg.field_violation); + EXPECT_EQ(vda5050_safety_state_msg.e_stop, control_msgs::msg::VDA5050SafetyState::REMOTE); +} + +// Test update logic for field violation and e-stop +TEST_F(VDA5050SafetyStateBroadcasterTest, update_broadcasted_success) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(fieldViolation2_itf_.set_value(0.0)); + ASSERT_TRUE(eStopManual1_itf_.set_value(1.0)); + + Vda5050SafetyStateMsg vda5050_safety_state_msg; + subscribe_and_get_messages(vda5050_safety_state_msg); + + EXPECT_FALSE(vda5050_safety_state_msg.field_violation); + EXPECT_EQ(vda5050_safety_state_msg.e_stop, control_msgs::msg::VDA5050SafetyState::MANUAL); +} + +TEST_F(VDA5050SafetyStateBroadcasterTest, publish_nan_voltage) +{ + SetUpVDA5050SafetyStateBroadcaster(); + + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + vda5050_safety_state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(fieldViolation2_itf_.set_value(std::numeric_limits::quiet_NaN())); + ASSERT_TRUE(eStopRemote2_itf_.set_value(std::numeric_limits::quiet_NaN())); + + Vda5050SafetyStateMsg vda5050_safety_state_msg; + subscribe_and_get_messages(vda5050_safety_state_msg); + + EXPECT_FALSE(vda5050_safety_state_msg.field_violation); + EXPECT_EQ(vda5050_safety_state_msg.e_stop, control_msgs::msg::VDA5050SafetyState::AUTO_ACK); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/vda5050_safety_state_broadcaster/test/test_vda5050_safety_state_broadcaster.hpp b/vda5050_safety_state_broadcaster/test/test_vda5050_safety_state_broadcaster.hpp new file mode 100644 index 0000000000..d0a9a6d7a0 --- /dev/null +++ b/vda5050_safety_state_broadcaster/test/test_vda5050_safety_state_broadcaster.hpp @@ -0,0 +1,168 @@ +// 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. + +#ifndef TEST_VDA5050_SAFETY_STATE_BROADCASTER_HPP_ +#define TEST_VDA5050_SAFETY_STATE_BROADCASTER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "control_msgs/msg/vda5050_safety_state.hpp" +#include "vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster.hpp" + +using Vda5050SafetyStateMsg = control_msgs::msg::VDA5050SafetyState; +using testing::IsEmpty; +using testing::SizeIs; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +constexpr auto NODE_FAILURE = controller_interface::CallbackReturn::FAILURE; +} // namespace + +class FriendVDA5050SafetyStateBroadcaster +: public vda5050_safety_state_broadcaster::Vda5050SafetyStateBroadcaster +{ + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, init_success); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, all_parameters_set_configure_success); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, no_interfaces_set_activate_fail); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, activate_success); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, deactivate_success); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, check_exported_interfaces); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, update_success); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, publish_status_success); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, update_broadcasted_success); + FRIEND_TEST(VDA5050SafetyStateBroadcasterTest, publish_nan_voltage); +}; + +class VDA5050SafetyStateBroadcasterTest : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + static void TearDownTestCase() {} + + void SetUp() + { + // initialize controller + vda5050_safety_state_broadcaster_ = std::make_unique(); + } + void TearDown() { vda5050_safety_state_broadcaster_.reset(nullptr); } + + void SetUpVDA5050SafetyStateBroadcaster( + const std::string controller_name = "test_vda5050_safety_state_broadcaster") + { + ASSERT_EQ( + vda5050_safety_state_broadcaster_->init( + controller_name, "", 0, "", + vda5050_safety_state_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector state_ifs; + + state_ifs.emplace_back(fieldViolation1_itf_); + state_ifs.emplace_back(fieldViolation2_itf_); + state_ifs.emplace_back(eStopManual1_itf_); + state_ifs.emplace_back(eStopManual2_itf_); + state_ifs.emplace_back(eStopRemote1_itf_); + state_ifs.emplace_back(eStopRemote2_itf_); + state_ifs.emplace_back(eStopAutoack_itf_); + + vda5050_safety_state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); + } + +protected: + std::array itfs_values_ = {{ + 0.0, // 0 fieldViolation1 + 1.0, // 1 fieldViolation2 + 0.0, // 2 eStopManual1 + 0.0, // 3 eStopManual2 + 0.0, // 4 eStopRemote1 + 1.0, // 5 eStopRemote2 + 1.0, // 6 eStopAutoack + }}; + hardware_interface::StateInterface fieldViolation1_itf_{ + "PLC_sensor1", "fieldViolation", &itfs_values_[0]}; + hardware_interface::StateInterface fieldViolation2_itf_{ + "PLC_sensor2", "fieldViolation", &itfs_values_[1]}; + hardware_interface::StateInterface eStopManual1_itf_{ + "PLC_sensor1", "eStopManual", &itfs_values_[2]}; + hardware_interface::StateInterface eStopManual2_itf_{ + "PLC_sensor2", "eStopManual", &itfs_values_[3]}; + hardware_interface::StateInterface eStopRemote1_itf_{ + "PLC_sensor1", "eStopRemote", &itfs_values_[4]}; + hardware_interface::StateInterface eStopRemote2_itf_{ + "PLC_sensor2", "eStopRemote", &itfs_values_[5]}; + hardware_interface::StateInterface eStopAutoack_itf_{ + "PLC_sensor1", "eStopAutoack", &itfs_values_[6]}; + + // Test related parameters + std::unique_ptr vda5050_safety_state_broadcaster_; + + void subscribe_and_get_messages(Vda5050SafetyStateMsg & vda5050_safety_state_msg) + { + // create a new subscriber + Vda5050SafetyStateMsg::SharedPtr received_vda5050_safety_state_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto vda5050_safety_state_callback = [&](const Vda5050SafetyStateMsg::SharedPtr msg) + { received_vda5050_safety_state_msg = msg; }; + auto vda5050_safety_state_subscription = + test_subscription_node.create_subscription( + "/test_vda5050_safety_state_broadcaster/vda5050_safety_state", 10, + vda5050_safety_state_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + vda5050_safety_state_broadcaster_->update( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while ((!received_vda5050_safety_state_msg) && + test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_vda5050_safety_state_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_vda5050_safety_state_msg); + + // take message from subscription + vda5050_safety_state_msg = *received_vda5050_safety_state_msg; + } +}; + +#endif // TEST_VDA5050_SAFETY_STATE_BROADCASTER_HPP_ diff --git a/vda5050_safety_state_broadcaster/test/vda5050_safety_state_broadcaster_params.yaml b/vda5050_safety_state_broadcaster/test/vda5050_safety_state_broadcaster_params.yaml new file mode 100644 index 0000000000..26fda7f548 --- /dev/null +++ b/vda5050_safety_state_broadcaster/test/vda5050_safety_state_broadcaster_params.yaml @@ -0,0 +1,13 @@ +test_vda5050_safety_state_broadcaster: + ros__parameters: + fieldViolation_interfaces: + - PLC_sensor1/fieldViolation + - PLC_sensor2/fieldViolation + eStop_manual_interfaces: + - PLC_sensor1/eStopManual + - PLC_sensor2/eStopManual + eStop_remote_interfaces: + - PLC_sensor1/eStopRemote + - PLC_sensor2/eStopRemote + eStop_autoack_interfaces: + - PLC_sensor1/eStopAutoack diff --git a/vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster.xml b/vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster.xml new file mode 100644 index 0000000000..5e15c78918 --- /dev/null +++ b/vda5050_safety_state_broadcaster/vda5050_safety_state_broadcaster.xml @@ -0,0 +1,28 @@ + + + + + + vda5050_safety_state_broadcaster publishes the safety states as defined in the VDA 5050 standard as a control_msgs/VDA5050. + + +