Merge branch 'master' into fix/exclusive_hw_interface_switching

Author: saikishor

controller_interface/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.24.0 (2025-01-13)
+-------------------
+* Trigger shutdown transition in destructor (`#1979 <https://github.com/ros-controls/ros2_control/issues/1979>`_)
+* Contributors: Christoph Fröhlich
+
 4.23.0 (2024-12-29)
 -------------------
 * Remove boilerplate visibility macros (`#1972 <https://github.com/ros-controls/ros2_control/issues/1972>`_)

controller_interface/CMakeLists.txt

@@ -45,11 +45,12 @@ if(BUILD_TESTING)
   )
 
   ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp)
-  install(FILES test/test_controller_node_options.yaml
-      DESTINATION test)
   target_link_libraries(test_controller_with_options
     controller_interface
   )
+  target_compile_definitions(
+    test_controller_with_options
+    PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/")
 
   ament_add_gmock(test_chainable_controller_interface test/test_chainable_controller_interface.cpp)
   target_link_libraries(test_chainable_controller_interface
@@ -86,6 +87,12 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_pose_sensor
     geometry_msgs
   )
+
+  ament_add_gmock(test_gps_sensor test/test_gps_sensor.cpp)
+  target_link_libraries(test_gps_sensor
+    controller_interface
+    hardware_interface::hardware_interface
+  )
 endif()
 
 install(

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -99,7 +99,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 public:
   ControllerInterfaceBase() = default;
 
-  virtual ~ControllerInterfaceBase() = default;
+  virtual ~ControllerInterfaceBase();
 
   /// Get configuration for controller's required command interfaces.
   /**

controller_interface/include/semantic_components/gps_sensor.hpp

@@ -0,0 +1,136 @@
+// Copyright 2025 ros2_control development team
+//
+// 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__GPS_SENSOR_HPP_
+#define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
+
+#include <array>
+#include <string>
+
+#include "semantic_components/semantic_component_interface.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+
+namespace semantic_components
+{
+
+enum class GPSSensorOption
+{
+  WithCovariance,
+  WithoutCovariance
+};
+
+template <GPSSensorOption sensor_option>
+class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
+{
+public:
+  static_assert(
+    sensor_option == GPSSensorOption::WithCovariance ||
+      sensor_option == GPSSensorOption::WithoutCovariance,
+    "Invalid GPSSensorOption");
+  explicit GPSSensor(const std::string & name)
+  : SemanticComponentInterface(
+      name, {{name + "/" + "status"},
+             {name + "/" + "service"},
+             {name + "/" + "latitude"},
+             {name + "/" + "longitude"},
+             {name + "/" + "altitude"}})
+  {
+    if constexpr (sensor_option == GPSSensorOption::WithCovariance)
+    {
+      interface_names_.emplace_back(name + "/" + "latitude_covariance");
+      interface_names_.emplace_back(name + "/" + "longitude_covariance");
+      interface_names_.emplace_back(name + "/" + "altitude_covariance");
+    }
+  }
+
+  /**
+   * Return GPS's status e.g. fix/no_fix
+   *
+   * \return Status
+   */
+  int8_t get_status() const { return static_cast<int8_t>(state_interfaces_[0].get().get_value()); }
+
+  /**
+   * Return service used by GPS e.g. fix/no_fix
+   *
+   * \return Service
+   */
+  uint16_t get_service() const
+  {
+    return static_cast<uint16_t>(state_interfaces_[1].get().get_value());
+  }
+
+  /**
+   * Return latitude reported by a GPS
+   *
+   * \return Latitude.
+   */
+  double get_latitude() const { return state_interfaces_[2].get().get_value(); }
+
+  /**
+   * Return longitude reported by a GPS
+   *
+   * \return Longitude.
+   */
+  double get_longitude() const { return state_interfaces_[3].get().get_value(); }
+
+  /**
+   * Return altitude reported by a GPS
+   *
+   * \return Altitude.
+   */
+  double get_altitude() const { return state_interfaces_[4].get().get_value(); }
+
+  /**
+   * Return covariance reported by a GPS.
+   *
+   * \return Covariance array.
+   */
+  template <
+    typename U = void,
+    typename = std::enable_if_t<sensor_option == GPSSensorOption::WithCovariance, U>>
+  const std::array<double, 9> & get_covariance()
+  {
+    covariance_[0] = state_interfaces_[5].get().get_value();
+    covariance_[4] = state_interfaces_[6].get().get_value();
+    covariance_[8] = state_interfaces_[7].get().get_value();
+    return covariance_;
+  }
+
+  /**
+   * Fills a NavSatFix message from the current values.
+   */
+  bool get_values_as_message(sensor_msgs::msg::NavSatFix & message)
+  {
+    message.status.status = get_status();
+    message.status.service = get_service();
+    message.latitude = get_latitude();
+    message.longitude = get_longitude();
+    message.altitude = get_altitude();
+
+    if constexpr (sensor_option == GPSSensorOption::WithCovariance)
+    {
+      message.position_covariance = get_covariance();
+    }
+
+    return true;
+  }
+
+private:
+  std::array<double, 9> covariance_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+};
+
+}  // namespace semantic_components
+
+#endif  // SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_

controller_interface/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>4.23.0</version>
+  <version>4.24.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_interface/src/controller_interface_base.cpp

@@ -22,6 +22,22 @@
 
 namespace controller_interface
 {
+ControllerInterfaceBase::~ControllerInterfaceBase()
+{
+  // check if node is initialized and we still have a valid context
+  if (
+    node_.get() &&
+    get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED &&
+    rclcpp::ok())
+  {
+    RCLCPP_DEBUG(
+      get_node()->get_logger(),
+      "Calling shutdown transition of controller node '%s' due to destruction.",
+      get_node()->get_name());
+    node_->shutdown();
+  }
+}
+
 return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
   const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
@@ -52,6 +68,11 @@ return_type ControllerInterfaceBase::init(
       break;
     case LifecycleNodeInterface::CallbackReturn::ERROR:
     case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      RCLCPP_DEBUG(
+        get_node()->get_logger(),
+        "Calling shutdown transition of controller node '%s' due to init failure.",
+        get_node()->get_name());
+      node_->shutdown();
       return return_type::ERROR;
   }
 
@@ -158,6 +179,10 @@ void ControllerInterfaceBase::release_interfaces()
 
 const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
 {
+  if (!node_.get())
+  {
+    throw std::runtime_error("Lifecycle node hasn't been initialized yet!");
+  }
   return node_->get_current_state();
 }
 

controller_interface/test/test_controller_interface.cpp

@@ -36,16 +36,21 @@ TEST(TestableControllerInterface, init)
   rclcpp::init(argc, argv);
 
   TestableControllerInterface controller;
+  const TestableControllerInterface & const_controller = controller;
 
   // try to get node when not initialized
   ASSERT_THROW(controller.get_node(), std::runtime_error);
+  ASSERT_THROW(const_controller.get_node(), std::runtime_error);
+  ASSERT_THROW(controller.get_lifecycle_state(), std::runtime_error);
 
   // initialize, create node
   const auto node_options = controller.define_custom_node_options();
   ASSERT_EQ(
     controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options),
     controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
+  ASSERT_NO_THROW(const_controller.get_node());
+  ASSERT_NO_THROW(controller.get_lifecycle_state());
 
   // update_rate is set to controller_manager's rate
   ASSERT_EQ(controller.get_update_rate(), 10u);
@@ -54,6 +59,7 @@ TEST(TestableControllerInterface, init)
   controller.configure();
   ASSERT_EQ(controller.get_update_rate(), 10u);
 
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -80,6 +86,8 @@ TEST(TestableControllerInterface, setting_negative_update_rate_in_configure)
   // The configure should fail and the update rate should stay the same
   ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
   ASSERT_EQ(controller.get_update_rate(), 1000u);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -149,6 +157,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   ASSERT_EQ(controller.get_update_rate(), 623u);
 
   executor->cancel();
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -166,6 +175,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
     controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options),
     controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(
+    controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 
@@ -183,6 +194,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
     controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
     controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(
+    controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }