created cascaded structure

Author: soham2560

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -126,41 +126,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
   {
-    actuator_clock_ = clock;
-    actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name);
-    info_ = hardware_info;
-    if (info_.is_async)
-    {
-      RCLCPP_INFO_STREAM(
-        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
-      async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
-      async_handler_->init(
-        [this](const rclcpp::Time & time, const rclcpp::Duration & period)
-        {
-          const auto read_start_time = std::chrono::steady_clock::now();
-          const auto ret_read = read(time, period);
-          const auto read_end_time = std::chrono::steady_clock::now();
-          read_return_info_.store(ret_read, std::memory_order_release);
-          read_execution_time_.store(
-            std::chrono::duration_cast<std::chrono::nanoseconds>(read_end_time - read_start_time),
-            std::memory_order_release);
-          if (ret_read != return_type::OK)
-          {
-            return ret_read;
-          }
-          const auto write_start_time = std::chrono::steady_clock::now();
-          const auto ret_write = write(time, period);
-          const auto write_end_time = std::chrono::steady_clock::now();
-          write_return_info_.store(ret_write, std::memory_order_release);
-          write_execution_time_.store(
-            std::chrono::duration_cast<std::chrono::nanoseconds>(write_end_time - write_start_time),
-            std::memory_order_release);
-          return ret_write;
-        },
-        info_.thread_priority);
-      async_handler_->start_thread();
-    }
-    return on_init(hardware_info);
+    hardware_interface::HardwareComponentParams params;
+    params.hardware_info = hardware_info;
+    params.clock = clock;
+    params.logger = logger;
+    return init(params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
@@ -215,7 +185,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
     }
     hardware_interface::HardwareComponentInterfaceParams interface_params;
     interface_params.hardware_info = info_;
-    interface_params.executor = params.executor;
+    // interface_params.executor = params.executor;
     return on_init(interface_params);
   };
 
@@ -227,10 +197,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
-    info_ = hardware_info;
-    parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
-    parse_command_interface_descriptions(info_.joints, joint_command_interfaces_);
-    return CallbackReturn::SUCCESS;
+    hardware_interface::HardwareComponentInterfaceParams params;
+    params.hardware_info = hardware_info;
+    return on_init(params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF.

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -107,8 +107,11 @@ class ResourceManager
    * interfaces.
    *
    * \param[in] params ResourceManagerParams containing the parameters for the ResourceManager.
+   * \param[in] load boolean argument indicating if the components should be loaded and
+   * initialized. If false, the ResourceManager will not load any components and will only
+   * initialize the ResourceManager with the given parameters.
    */
-  explicit ResourceManager(const hardware_interface::ResourceManagerParams & params);
+  explicit ResourceManager(const hardware_interface::ResourceManagerParams & params, bool load = true);
 
   ResourceManager(const ResourceManager &) = delete;
 
@@ -595,8 +598,9 @@ class ResourceManager
   rclcpp::Logger logger,
   const std::string & urdf = std::string(),
   bool activate_all = false,
-  unsigned int update_rate = 100,
-  rclcpp::Executor::SharedPtr executor = nullptr);
+  unsigned int update_rate = 100
+  //,rclcpp::Executor::SharedPtr executor = nullptr
+  );
 
   std::unordered_map<std::string, bool> claimed_command_interface_map_;
 

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -110,20 +110,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
   {
-    sensor_clock_ = clock;
-    sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name);
-    info_ = hardware_info;
-    if (info_.is_async)
-    {
-      RCLCPP_INFO_STREAM(
-        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
-      read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
-      read_async_handler_->init(
-        std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2),
-        info_.thread_priority);
-      read_async_handler_->start_thread();
-    }
-    return on_init(hardware_info);
+    hardware_interface::HardwareComponentParams params;
+    params.hardware_info = hardware_info;
+    params.clock = clock;
+    params.logger = logger;
+    return init(params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
@@ -157,7 +148,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     }
     hardware_interface::HardwareComponentInterfaceParams interface_params;
     interface_params.hardware_info = info_;
-    interface_params.executor = params.executor;
+    // interface_params.executor = params.executor;
     return on_init(interface_params);
   };
 
@@ -169,10 +160,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
-    info_ = hardware_info;
-    parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
-    parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_);
-    return CallbackReturn::SUCCESS;
+    hardware_interface::HardwareComponentInterfaceParams params;
+    params.hardware_info = hardware_info;
+    return on_init(params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF.

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -130,41 +130,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
   {
-    system_clock_ = clock;
-    system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name);
-    info_ = hardware_info;
-    if (info_.is_async)
-    {
-      RCLCPP_INFO_STREAM(
-        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
-      async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
-      async_handler_->init(
-        [this](const rclcpp::Time & time, const rclcpp::Duration & period)
-        {
-          const auto read_start_time = std::chrono::steady_clock::now();
-          const auto ret_read = read(time, period);
-          const auto read_end_time = std::chrono::steady_clock::now();
-          read_return_info_.store(ret_read, std::memory_order_release);
-          read_execution_time_.store(
-            std::chrono::duration_cast<std::chrono::nanoseconds>(read_end_time - read_start_time),
-            std::memory_order_release);
-          if (ret_read != return_type::OK)
-          {
-            return ret_read;
-          }
-          const auto write_start_time = std::chrono::steady_clock::now();
-          const auto ret_write = write(time, period);
-          const auto write_end_time = std::chrono::steady_clock::now();
-          write_return_info_.store(ret_write, std::memory_order_release);
-          write_execution_time_.store(
-            std::chrono::duration_cast<std::chrono::nanoseconds>(write_end_time - write_start_time),
-            std::memory_order_release);
-          return ret_write;
-        },
-        info_.thread_priority);
-      async_handler_->start_thread();
-    }
-    return on_init(hardware_info);
+    hardware_interface::HardwareComponentParams params;
+    params.hardware_info = hardware_info;
+    params.clock = clock;
+    params.logger = logger;
+    return init(params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
@@ -219,7 +189,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     }
     hardware_interface::HardwareComponentInterfaceParams interface_params;
     interface_params.hardware_info = info_;
-    interface_params.executor = params.executor;
+    // interface_params.executor = params.executor;
     return on_init(interface_params);
   };
 
@@ -231,13 +201,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
-    info_ = hardware_info;
-    parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
-    parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_);
-    parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_);
-    parse_command_interface_descriptions(info_.joints, joint_command_interfaces_);
-    parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_);
-    return CallbackReturn::SUCCESS;
+    hardware_interface::HardwareComponentInterfaceParams params;
+    params.hardware_info = hardware_info;
+    return on_init(params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF.

hardware_interface/src/actuator.cpp

@@ -55,26 +55,11 @@ const rclcpp_lifecycle::State & Actuator::initialize(
 const rclcpp_lifecycle::State & Actuator::initialize(
   const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
 {
-  std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
-  if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
-  {
-    switch (impl_->init(actuator_info, logger, clock))
-    {
-      case CallbackReturn::SUCCESS:
-        impl_->set_lifecycle_state(
-          rclcpp_lifecycle::State(
-            lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-            lifecycle_state_names::UNCONFIGURED));
-        break;
-      case CallbackReturn::FAILURE:
-      case CallbackReturn::ERROR:
-        impl_->set_lifecycle_state(
-          rclcpp_lifecycle::State(
-            lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED));
-        break;
-    }
-  }
-  return impl_->get_lifecycle_state();
+  hardware_interface::HardwareComponentParams params;
+  params.hardware_info = actuator_info;
+  params.logger = logger;
+  params.clock = clock;
+  return initialize(params);
 }
 
 const rclcpp_lifecycle::State & Actuator::initialize(