diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 3dc3bc4d0a..cad5810ee5 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -34,6 +34,7 @@ target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BU if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) ament_add_gmock(test_controller_interface test/test_controller_interface.cpp) @@ -74,6 +75,15 @@ if(BUILD_TESTING) ament_target_dependencies(test_imu_sensor sensor_msgs ) + + ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp) + target_link_libraries(test_pose_sensor + controller_interface + hardware_interface::hardware_interface + ) + ament_target_dependencies(test_pose_sensor + geometry_msgs + ) endif() install( diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index e4e4ec662c..6775724bfa 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -145,9 +145,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; // END - std::vector ordered_reference_interfaces_; + std::vector + ordered_exported_reference_interfaces_; std::unordered_map - reference_interfaces_ptrs_; + exported_reference_interfaces_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index b571751f55..dfd29841bf 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -78,6 +78,16 @@ inline bool interface_list_contains_interface_type( interface_type_list.end(); } +template +void add_element_to_list(std::vector & list, const T & element) +{ + if (std::find(list.begin(), list.end(), element) == list.end()) + { + // Only add to the list if it doesn't exist + list.push_back(element); + } +} + } // namespace controller_interface #endif // CONTROLLER_INTERFACE__HELPERS_HPP_ diff --git a/controller_interface/include/semantic_components/pose_sensor.hpp b/controller_interface/include/semantic_components/pose_sensor.hpp new file mode 100644 index 0000000000..60dbecd718 --- /dev/null +++ b/controller_interface/include/semantic_components/pose_sensor.hpp @@ -0,0 +1,110 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ +#define SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "semantic_components/semantic_component_interface.hpp" + +namespace semantic_components +{ + +class PoseSensor : public SemanticComponentInterface +{ +public: + /// Constructor for a standard pose sensor with interface names set based on sensor name. + explicit PoseSensor(const std::string & name) : SemanticComponentInterface{name, 7} + { + // Use standard interface names + interface_names_.emplace_back(name_ + '/' + "position.x"); + interface_names_.emplace_back(name_ + '/' + "position.y"); + interface_names_.emplace_back(name_ + '/' + "position.z"); + interface_names_.emplace_back(name_ + '/' + "orientation.x"); + interface_names_.emplace_back(name_ + '/' + "orientation.y"); + interface_names_.emplace_back(name_ + '/' + "orientation.z"); + interface_names_.emplace_back(name_ + '/' + "orientation.w"); + + // Set all sensor values to default value NaN + std::fill(position_.begin(), position_.end(), std::numeric_limits::quiet_NaN()); + std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits::quiet_NaN()); + } + + virtual ~PoseSensor() = default; + + /// Update and return position. + /*! + * Update and return current pose position from state interfaces. + * + * \return Array of position coordinates. + */ + std::array get_position() + { + for (size_t i = 0; i < 3; ++i) + { + position_[i] = state_interfaces_[i].get().get_value(); + } + + return position_; + } + + /// Update and return orientation + /*! + * Update and return current pose orientation from state interfaces. + * + * \return Array of orientation coordinates in xyzw convention. + */ + std::array get_orientation() + { + for (size_t i = 3; i < 7; ++i) + { + orientation_[i - 3] = state_interfaces_[i].get().get_value(); + } + + return orientation_; + } + + /// Fill pose message with current values. + /** + * Fill a pose message with current position and orientation from the state interfaces. + */ + bool get_values_as_message(geometry_msgs::msg::Pose & message) + { + // Update state from state interfaces + get_position(); + get_orientation(); + + // Set message values from current state + message.position.x = position_[0]; + message.position.y = position_[1]; + message.position.z = position_[2]; + message.orientation.x = orientation_[0]; + message.orientation.y = orientation_[1]; + message.orientation.z = orientation_[2]; + message.orientation.w = orientation_[3]; + + return true; + } + +protected: + std::array position_; + std::array orientation_; +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 55ad0c5916..dce1d79f86 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -19,6 +19,7 @@ rclcpp_lifecycle ament_cmake_gmock + geometry_msgs sensor_msgs diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index a6d427fe2b..409578be9b 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -16,6 +16,7 @@ #include +#include "controller_interface/helpers.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -67,7 +68,7 @@ ChainableControllerInterface::export_state_interfaces() throw std::runtime_error(error_msg); } auto state_interface = std::make_shared(interface); - const auto interface_name = state_interface->get_name(); + const auto interface_name = state_interface->get_interface_name(); auto [it, succ] = exported_state_interfaces_.insert({interface_name, state_interface}); // either we have name duplicate which we want to avoid under all circumstances since interfaces // need to be uniquely identify able or something else really went wrong. In any case abort and @@ -87,11 +88,24 @@ ChainableControllerInterface::export_state_interfaces() throw std::runtime_error(error_msg); } ordered_exported_state_interfaces_.push_back(state_interface); - exported_state_interface_names_.push_back(interface_name); + add_element_to_list(exported_state_interface_names_, interface_name); state_interfaces_ptrs_vec.push_back( std::const_pointer_cast(state_interface)); } + if (exported_state_interfaces_.size() != state_interfaces.size()) + { + std::string error_msg = + "The internal storage for state interface ptrs 'exported_state_interfaces_' variable has " + "size '" + + std::to_string(exported_state_interfaces_.size()) + + "', but it is expected to have the size '" + std::to_string(state_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + return state_interfaces_ptrs_vec; } @@ -102,7 +116,7 @@ ChainableControllerInterface::export_reference_interfaces() std::vector reference_interfaces_ptrs_vec; reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); exported_reference_interface_names_.reserve(reference_interfaces.size()); - ordered_reference_interfaces_.reserve(reference_interfaces.size()); + ordered_exported_reference_interfaces_.reserve(reference_interfaces.size()); // BEGIN (Handle export change): for backward compatibility // check if the "reference_interfaces_" variable is resized to number of interfaces @@ -135,16 +149,16 @@ ChainableControllerInterface::export_reference_interfaces() hardware_interface::CommandInterface::SharedPtr reference_interface = std::make_shared(std::move(interface)); - const auto inteface_name = reference_interface->get_name(); + const auto interface_name = reference_interface->get_interface_name(); // check the exported interface name is unique - auto [it, succ] = reference_interfaces_ptrs_.insert({inteface_name, reference_interface}); + auto [it, succ] = exported_reference_interfaces_.insert({interface_name, reference_interface}); // either we have name duplicate which we want to avoid under all circumstances since interfaces // need to be uniquely identify able or something else really went wrong. In any case abort and // inform cm by throwing exception if (!succ) { std::string error_msg = - "Could not insert Reference interface<" + inteface_name + + "Could not insert Reference interface<" + interface_name + "> into reference_interfaces_ map. Check if you export duplicates. The " "map returned iterator with interface_name<" + it->second->get_name() + @@ -155,16 +169,17 @@ ChainableControllerInterface::export_reference_interfaces() reference_interfaces_ptrs_vec.clear(); throw std::runtime_error(error_msg); } - ordered_reference_interfaces_.push_back(reference_interface); - exported_reference_interface_names_.push_back(inteface_name); + ordered_exported_reference_interfaces_.push_back(reference_interface); + add_element_to_list(exported_reference_interface_names_, interface_name); reference_interfaces_ptrs_vec.push_back(reference_interface); } - if (reference_interfaces_ptrs_.size() != ref_interface_size) + if (exported_reference_interfaces_.size() != ref_interface_size) { std::string error_msg = - "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + - std::to_string(reference_interfaces_ptrs_.size()) + + "The internal storage for exported reference ptrs 'exported_reference_interfaces_' variable " + "has size '" + + std::to_string(exported_reference_interfaces_.size()) + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + "' equal to the number of exported reference interfaces. Please correct and recompile the " "controller with name '" + diff --git a/controller_interface/test/test_pose_sensor.cpp b/controller_interface/test/test_pose_sensor.cpp new file mode 100644 index 0000000000..1ceb7c32a6 --- /dev/null +++ b/controller_interface/test/test_pose_sensor.cpp @@ -0,0 +1,98 @@ +// Copyright (c) 2024, FZI Forschungszentrum Informatik +// +// 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 "test_pose_sensor.hpp" + +void PoseSensorTest::SetUp() +{ + full_interface_names_.reserve(size_); + for (const auto & interface_name : interface_names_) + { + full_interface_names_.emplace_back(sensor_name_ + '/' + interface_name); + } +} + +void PoseSensorTest::TearDown() { pose_sensor_.reset(nullptr); } + +TEST_F(PoseSensorTest, validate_all) +{ + // Create sensor + pose_sensor_ = std::make_unique(sensor_name_); + EXPECT_EQ(pose_sensor_->name_, sensor_name_); + + // Validate reserved space for interface_names_ and state_interfaces_ + // As state_interfaces_ are not defined yet, use capacity() + ASSERT_EQ(pose_sensor_->interface_names_.size(), size_); + ASSERT_EQ(pose_sensor_->state_interfaces_.capacity(), size_); + + // Validate default interface_names_ + EXPECT_TRUE(std::equal( + pose_sensor_->interface_names_.cbegin(), pose_sensor_->interface_names_.cend(), + full_interface_names_.cbegin(), full_interface_names_.cend())); + + // Get interface names + std::vector interface_names = pose_sensor_->get_state_interface_names(); + + // Assign values to position + hardware_interface::StateInterface position_x{ + sensor_name_, interface_names_[0], &position_values_[0]}; + hardware_interface::StateInterface position_y{ + sensor_name_, interface_names_[1], &position_values_[1]}; + hardware_interface::StateInterface position_z{ + sensor_name_, interface_names_[2], &position_values_[2]}; + + // Assign values to orientation + hardware_interface::StateInterface orientation_x{ + sensor_name_, interface_names_[3], &orientation_values_[0]}; + hardware_interface::StateInterface orientation_y{ + sensor_name_, interface_names_[4], &orientation_values_[1]}; + hardware_interface::StateInterface orientation_z{ + sensor_name_, interface_names_[5], &orientation_values_[2]}; + hardware_interface::StateInterface orientation_w{ + sensor_name_, interface_names_[6], &orientation_values_[3]}; + + // Create state interface vector in jumbled order + std::vector temp_state_interfaces; + temp_state_interfaces.reserve(7); + + temp_state_interfaces.emplace_back(position_z); + temp_state_interfaces.emplace_back(orientation_y); + temp_state_interfaces.emplace_back(orientation_x); + temp_state_interfaces.emplace_back(position_x); + temp_state_interfaces.emplace_back(orientation_w); + temp_state_interfaces.emplace_back(position_y); + temp_state_interfaces.emplace_back(orientation_z); + + // Assign interfaces + pose_sensor_->assign_loaned_state_interfaces(temp_state_interfaces); + EXPECT_EQ(pose_sensor_->state_interfaces_.size(), size_); + + // Validate correct position and orientation + EXPECT_EQ(pose_sensor_->get_position(), position_values_); + EXPECT_EQ(pose_sensor_->get_orientation(), orientation_values_); + + // Validate generated message + geometry_msgs::msg::Pose temp_message; + ASSERT_TRUE(pose_sensor_->get_values_as_message(temp_message)); + EXPECT_EQ(temp_message.position.x, position_values_[0]); + EXPECT_EQ(temp_message.position.y, position_values_[1]); + EXPECT_EQ(temp_message.position.z, position_values_[2]); + EXPECT_EQ(temp_message.orientation.x, orientation_values_[0]); + EXPECT_EQ(temp_message.orientation.y, orientation_values_[1]); + EXPECT_EQ(temp_message.orientation.z, orientation_values_[2]); + EXPECT_EQ(temp_message.orientation.w, orientation_values_[3]); + + // Release state interfaces + pose_sensor_->release_interfaces(); + ASSERT_EQ(pose_sensor_->state_interfaces_.size(), 0); +} diff --git a/controller_interface/test/test_pose_sensor.hpp b/controller_interface/test/test_pose_sensor.hpp new file mode 100644 index 0000000000..c2344caaa2 --- /dev/null +++ b/controller_interface/test/test_pose_sensor.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2024, FZI Forschungszentrum Informatik +// +// 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_POSE_SENSOR_HPP_ +#define TEST_POSE_SENSOR_HPP_ + +#include + +#include +#include +#include +#include + +#include "semantic_components/pose_sensor.hpp" + +class TestablePoseSensor : public semantic_components::PoseSensor +{ + FRIEND_TEST(PoseSensorTest, validate_all); + +public: + // Use default interface names + explicit TestablePoseSensor(const std::string & name) : PoseSensor{name} {} + + virtual ~TestablePoseSensor() = default; +}; + +class PoseSensorTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + +protected: + const size_t size_ = 7; + const std::string sensor_name_ = "test_pose_sensor"; + + std::vector full_interface_names_; + const std::vector interface_names_ = { + "position.x", "position.y", "position.z", "orientation.x", + "orientation.y", "orientation.z", "orientation.w"}; + + std::array position_values_ = {{1.1, 2.2, 3.3}}; + std::array orientation_values_ = {{4.4, 5.5, 6.6, 7.7}}; + + std::unique_ptr pose_sensor_; +}; + +#endif // TEST_POSE_SENSOR_HPP_ diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 3bbb050433..c64b893156 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -19,8 +19,8 @@ from launch_ros.actions import Node -def generate_load_controller_launch_description( - controller_name, controller_params_file=None, extra_spawner_args=[] +def generate_controllers_spawner_launch_description( + controller_names: list, controller_params_file=None, extra_spawner_args=[] ): """ Generate launch description for loading a controller using spawner. @@ -32,11 +32,11 @@ def generate_load_controller_launch_description( Examples -------- # Assuming the controller parameters are known to the controller_manager - generate_load_controller_launch_description('joint_state_broadcaster') + generate_controllers_spawner_launch_description(['joint_state_broadcaster']) # Passing controller parameter file to load the controller (Controller type is retrieved from config file) - generate_load_controller_launch_description( - 'joint_state_broadcaster', + generate_controllers_spawner_launch_description( + ['joint_state_broadcaster'], controller_params_file=os.path.join(get_package_share_directory('my_pkg'), 'config', 'controller_params.yaml'), extra_spawner_args=[--load-only] @@ -54,11 +54,13 @@ def generate_load_controller_launch_description( description="Wait until the node is interrupted and then unload controller", ) - spawner_arguments = [ - controller_name, - "--controller-manager", - LaunchConfiguration("controller_manager_name"), - ] + spawner_arguments = controller_names + spawner_arguments.extend( + [ + "--controller-manager", + LaunchConfiguration("controller_manager_name"), + ] + ) if controller_params_file: spawner_arguments += ["--param-file", controller_params_file] @@ -94,3 +96,13 @@ def generate_load_controller_launch_description( spawner, ] ) + + +def generate_load_controller_launch_description( + controller_name: str, controller_params_file=None, extra_spawner_args=[] +): + return generate_controllers_spawner_launch_description( + controller_names=[controller_name], + controller_params_file=controller_params_file, + extra_spawner_args=extra_spawner_args, + ) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e2f4e8d0af..980de16ea6 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -443,6 +443,10 @@ class ControllerManager : public rclcpp::Node void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + void hardware_components_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + + void controller_manager_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + /** * @brief determine_controller_node_options - A method that retrieves the controller defined node * options and adapts them, based on if there is a params file to be loaded or the use_sim_time diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index de17356855..a22dd4d0b2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -281,6 +281,12 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + diagnostics_updater_.add( + "Hardware Components Activity", this, + &ControllerManager::hardware_components_diagnostic_callback); + diagnostics_updater_.add( + "Controller Manager Activity", this, + &ControllerManager::controller_manager_diagnostic_callback); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) @@ -796,16 +802,16 @@ controller_interface::return_type ControllerManager::configure_controller( // TODO(destogl): Add test for this! RCLCPP_ERROR( get_logger(), - "Controller '%s' is chainable, but does not export any reference interfaces. Did you " - "override the on_export_method() correctly?", + "Controller '%s' is chainable, but does not export any state or reference interfaces. " + "Did you override the on_export_method() correctly?", controller_name.c_str()); return controller_interface::return_type::ERROR; } } - catch (const std::runtime_error & e) + catch (const std::exception & e) { RCLCPP_FATAL( - get_logger(), "Creation of the reference interfaces failed with following error: %s", + get_logger(), "Export of the state or reference interfaces failed with following error: %s", e.what()); return controller_interface::return_type::ERROR; } @@ -2770,6 +2776,16 @@ controller_interface::return_type ControllerManager::check_preceeding_controller void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + atleast_one_hw_active = true; + break; + } + } // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); @@ -2783,13 +2799,95 @@ void ControllerManager::controller_activity_diagnostic_callback( stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); } - if (all_active) + if (!atleast_one_hw_active) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "All controllers are active"); + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "No hardware components are currently active to activate controllers"); } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not all controllers are active"); + if (controllers.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + } + } +} + +void ControllerManager::hardware_components_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + bool all_active = true; + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + stat.add(component_name, component_info.state.label()); + if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + all_active = false; + } + else + { + atleast_one_hw_active = true; + } + } + if (!is_resource_manager_initialized()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + } + else if (hw_components_info.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + } + else + { + if (!atleast_one_hw_active) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + "No hardware components are currently active"); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, all_active + ? "All hardware components are active" + : "Not all hardware components are active"); + } + } +} + +void ControllerManager::controller_manager_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("update_rate", std::to_string(get_update_rate())); + if (is_resource_manager_initialized()) + { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); + } + else + { + if (robot_description_.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Waiting for robot description...."); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "Resource Manager is not initialized properly!"); + } } } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 8320a81705..7d28b4390d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -25,6 +25,7 @@ For details see the controller_manager section. * The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) +* The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) controller_manager ******************