Ex9

example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp

@@ -40,10 +40,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
   hardware_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
-
-  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
-
   hardware_interface::CallbackReturn on_activate(
     const rclcpp_lifecycle::State & previous_state) override;
 
@@ -61,10 +57,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
   double hw_start_sec_;
   double hw_stop_sec_;
   double hw_slowdown_;
-
-  // Store the command for the simulated robot
-  std::vector<double> hw_commands_;
-  std::vector<double> hw_states_;
 };
 
 }  // namespace ros2_control_demo_example_9

example_9/hardware/rrbot.cpp

@@ -43,8 +43,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init(
   hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
   hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);
   // END: This part here is for exemplary purposes - Please do not copy to your production code
-  hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
-  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
 
   for (const hardware_interface::ComponentInfo & joint : info_.joints)
   {
@@ -100,41 +98,17 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
   // reset values always when configuring hardware
-  for (uint i = 0; i < hw_states_.size(); i++)
+  for (const auto & [name, descr] : joint_state_interfaces_)
   {
-    hw_states_[i] = 0;
-    hw_commands_[i] = 0;
+    set_state(name, 0.0);
   }
-
-  RCLCPP_INFO(get_logger(), "Successfully configured!");
-
-  return hardware_interface::CallbackReturn::SUCCESS;
-}
-
-std::vector<hardware_interface::StateInterface>
-RRBotSystemPositionOnlyHardware::export_state_interfaces()
-{
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
-  {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
-  }
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface>
-RRBotSystemPositionOnlyHardware::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
+  for (const auto & [name, descr] : joint_command_interfaces_)
   {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
+    set_command(name, 0.0);
   }
+  RCLCPP_INFO(get_logger(), "Successfully configured!");
 
-  return command_interfaces;
+  return hardware_interface::CallbackReturn::SUCCESS;
 }
 
 hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate(
@@ -151,9 +125,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate(
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
   // command and state should be equal when starting
-  for (uint i = 0; i < hw_states_.size(); i++)
+  for (const auto & [name, descr] : joint_state_interfaces_)
   {
-    hw_commands_[i] = hw_states_[i];
+    set_command(name, get_state(name));
   }
 
   RCLCPP_INFO(get_logger(), "Successfully activated!");
@@ -186,13 +160,13 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read(
   std::stringstream ss;
   ss << "Reading states:";
 
-  for (uint i = 0; i < hw_states_.size(); i++)
+  for (const auto & [name, descr] : joint_state_interfaces_)
   {
     // Simulate RRBot's movement
-    hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
-
+    auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_;
+    set_state(name, new_value);
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'";
+       << "\t" << get_state(name) << " for joint '" << name << "'";
   }
   RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
   // END: This part here is for exemplary purposes - Please do not copy to your production code
@@ -207,11 +181,11 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write(
   std::stringstream ss;
   ss << "Writing commands:";
 
-  for (uint i = 0; i < hw_commands_.size(); i++)
+  for (const auto & [name, descr] : joint_command_interfaces_)
   {
     // Simulate sending commands to the hardware
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name.c_str() << "'";
+       << "\t" << get_command(name) << " for joint '" << name << "'";
   }
   RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
   // END: This part here is for exemplary purposes - Please do not copy to your production code