AMRNAV-6169 Extend use of composition: ros2 control

controller_interface/src/controller_interface_base.cpp

@@ -29,7 +29,9 @@ return_type ControllerInterfaceBase::init(
   const rclcpp::NodeOptions & node_options)
 {
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
-    controller_name, namespace_, node_options, false);  // disable LifecycleNode service interfaces
+    controller_name, namespace_, rclcpp::NodeOptions().enable_logger_service(true).arguments({
+    "--ros-args",
+    "--remap", ("__node:=" + controller_name).c_str()}), false);  // disable LifecycleNode service interfaces
 
   try
   {

controller_manager/CMakeLists.txt

@@ -15,6 +15,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   realtime_tools
   std_msgs
+  rclcpp_components
+  rclcpp_lifecycle
 )
 
 find_package(ament_cmake REQUIRED)
@@ -44,6 +46,8 @@ target_link_libraries(ros2_control_node PRIVATE
   controller_manager
 )
 
+rclcpp_components_register_nodes(controller_manager "controller_manager::ControllerManager")
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
@@ -222,5 +226,6 @@ ament_python_install_package(controller_manager
   SCRIPTS_DESTINATION lib/controller_manager
 )
 ament_export_targets(export_controller_manager HAS_LIBRARY_TARGET)
+ament_export_libraries(controller_manager) 
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

controller_manager/include/controller_manager/controller_manager.hpp

@@ -21,6 +21,7 @@
 #include <tuple>
 #include <unordered_map>
 #include <utility>
+#include <thread>
 #include <vector>
 
 #include "controller_interface/chainable_controller_interface.hpp"
@@ -53,6 +54,7 @@
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "realtime_tools/thread_priority.hpp"
 
 namespace controller_manager
 {
@@ -80,9 +82,19 @@ class ControllerManager : public rclcpp::Node
     const std::string & manager_node_name = "controller_manager",
     const std::string & namespace_ = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
+
+
+  /**
+   * Contstuctor for launching ControllerManager as a component.
+   * Creates an executor for controllers and spins it.
+   * Also runs the update loop in a thread.
+  */
+  CONTROLLER_MANAGER_PUBLIC
+  explicit ControllerManager(rclcpp::NodeOptions options);
 
   CONTROLLER_MANAGER_PUBLIC
-  virtual ~ControllerManager() = default;
+  ~ControllerManager();
+
 
   CONTROLLER_MANAGER_PUBLIC
   void robot_description_callback(const std_msgs::msg::String & msg);
@@ -172,6 +184,9 @@ class ControllerManager : public rclcpp::Node
   controller_interface::return_type update(
     const rclcpp::Time & time, const rclcpp::Duration & period);
 
+  CONTROLLER_MANAGER_PUBLIC
+  void update_loop();
+
   /// Write values from command interfaces.
   /**
    * Write values from command interface into hardware.
@@ -417,6 +432,9 @@ class ControllerManager : public rclcpp::Node
   diagnostic_updater::Updater diagnostics_updater_;
 
   std::shared_ptr<rclcpp::Executor> executor_;
+  std::thread spin_executor_thread_;
+  rclcpp::TimerBase::SharedPtr init_timer_;
+  std::thread cm_thread_;
 
   std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
   std::shared_ptr<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>

controller_manager/src/controller_manager.cpp

@@ -27,6 +27,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
+using namespace std::chrono_literals;
 namespace  // utility
 {
 static constexpr const char * kControllerInterfaceNamespace = "controller_interface";
@@ -248,6 +249,55 @@ rclcpp::NodeOptions get_cm_node_options()
   return node_options;
 }
 
+ControllerManager::ControllerManager(rclcpp::NodeOptions options)
+: rclcpp::Node("controller_manager", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)),
+  diagnostics_updater_(this),
+  loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
+    kControllerInterfaceNamespace, kControllerInterfaceClassName)),
+  chainable_loader_(
+    std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
+  resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
+    update_rate_, this->get_node_clock_interface()))
+{
+  if (!get_parameter("update_rate", update_rate_))
+  {
+    RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
+  }
+  auto update_period = std::chrono::duration<double>(1.0 / update_rate_);
+
+  std::string robot_description = "";
+  // TODO(destogl): remove support at the end of 2023
+  get_parameter("robot_description", robot_description);
+  if (robot_description.empty())
+  {
+    subscribe_to_robot_description_topic();
+  }
+  else
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "[Deprecated] Passing the robot description parameter directly to the control_manager node "
+      "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
+    init_resource_manager(robot_description);
+  }
+
+  diagnostics_updater_.setHardwareID("ros2_control");
+  diagnostics_updater_.add(
+    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+  executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+  spin_executor_thread_ = std::thread([this]() {
+    executor_->spin();
+  });
+  init_services();
+  init_timer_ = this->create_wall_timer(
+    1s,
+    [this]() -> void {
+      init_timer_->cancel();
+      update_loop();
+    });
+};
+
 ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
   const std::string & namespace_, const rclcpp::NodeOptions & options)
@@ -281,12 +331,12 @@ ControllerManager::ControllerManager(
       "[Deprecated] Passing the robot description parameter directly to the control_manager node "
       "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
     init_resource_manager(robot_description);
-    init_services();
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+  init_services();
 }
 
 ControllerManager::ControllerManager(
@@ -308,15 +358,12 @@ ControllerManager::ControllerManager(
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
-  if (resource_manager_->is_urdf_already_loaded())
-  {
-    init_services();
-  }
   subscribe_to_robot_description_topic();
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+  init_services();
 }
 
 void ControllerManager::subscribe_to_robot_description_topic()
@@ -349,7 +396,6 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       return;
     }
     init_resource_manager(robot_description.data.c_str());
-    init_services();
   }
   catch (std::runtime_error & e)
   {
@@ -465,6 +511,52 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   }
 }
 
+
+void ControllerManager::update_loop(){
+
+  cm_thread_ = std::thread(
+    [this]()
+    {
+      if (realtime_tools::has_realtime_kernel())
+      {
+        if (!realtime_tools::configure_sched_fifo(50))
+        {
+          RCLCPP_WARN(this->get_logger(), "Could not enable FIFO RT scheduling policy");
+        }
+      }
+      else
+      {
+        RCLCPP_INFO(this->get_logger(), "RT kernel is recommended for better performance");
+      }
+
+      // for calculating sleep time
+      auto const period = std::chrono::nanoseconds(1'000'000'000 / this->get_update_rate());
+      auto const cm_now = std::chrono::nanoseconds(this->now().nanoseconds());
+      std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds>
+        next_iteration_time{cm_now};
+
+      // for calculating the measured period of the loop
+      rclcpp::Time previous_time = this->now();
+
+      while (rclcpp::ok())
+      {
+        // calculate measured period
+        auto const current_time = this->now();
+        auto const measured_period = current_time - previous_time;
+        previous_time = current_time;
+
+        // execute update loop
+        this->read(this->now(), measured_period);
+        this->update(this->now(), measured_period);
+        this->write(this->now(), measured_period);
+
+        // wait until we hit the end of the period
+        next_iteration_time += period;
+        std::this_thread::sleep_until(next_iteration_time);
+      }
+    });
+}
+
 void ControllerManager::init_services()
 {
   // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on
@@ -1332,6 +1424,10 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
 
   return to.back().c;
 }
+ControllerManager::~ControllerManager()
+{
+  cm_thread_.join();
+}
 
 void ControllerManager::manage_switch()
 {
@@ -2581,3 +2677,6 @@ void ControllerManager::controller_activity_diagnostic_callback(
 }
 
 }  // namespace controller_manager
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(controller_manager::ControllerManager)

controller_manager/test/test_hardware_management_srvs.cpp

@@ -84,9 +84,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    auto msg = std_msgs::msg::String();
-    msg.data = robot_description_;
-    cm_->robot_description_callback(msg);
+    cm_->init_resource_manager(robot_description);
 
     SetUpSrvsCMExecutor();
   }
@@ -385,9 +383,7 @@ class TestControllerManagerHWManagementSrvsWithoutParams
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    auto msg = std_msgs::msg::String();
-    msg.data = robot_description_;
-    cm_->robot_description_callback(msg);
+    cm_->init_resource_manager(robot_description);
 
     SetUpSrvsCMExecutor();
   }
@@ -444,9 +440,7 @@ class TestControllerManagerHWManagementSrvsOldParameters
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    auto msg = std_msgs::msg::String();
-    msg.data = robot_description_;
-    cm_->robot_description_callback(msg);
+    cm_->init_resource_manager(robot_description);
 
     SetUpSrvsCMExecutor();
   }

hardware_interface/CMakeLists.txt

@@ -41,6 +41,7 @@ target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDI
 
 add_library(mock_components SHARED
   src/mock_components/generic_system.cpp
+  src/lexical_casts.cpp
 )
 target_compile_features(mock_components PUBLIC cxx_std_17)
 target_include_directories(mock_components PUBLIC