Variants ex14

example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp

@@ -40,10 +40,6 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface
   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;
 
@@ -58,12 +54,9 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface
 
 private:
   // Parameters for the RRBot simulation
-
-  // Store the command and state interfaces for the simulated robot
-  std::vector<double> hw_commands_;
-  std::vector<double> hw_states_;
-  std::vector<double> hw_gpio_in_;
-  std::vector<double> hw_gpio_out_;
+  double hw_start_sec_;
+  double hw_stop_sec_;
+  double hw_slowdown_;
 };
 
 }  // namespace ros2_control_demo_example_10

example_10/hardware/rrbot.cpp

@@ -37,9 +37,6 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
     return hardware_interface::CallbackReturn::ERROR;
   }
 
-  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)
   {
     // RRBotSystemPositionOnly has exactly one state and command interface on each joint
@@ -123,67 +120,25 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure(
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
   // reset values always when configuring hardware
-  std::fill(hw_states_.begin(), hw_states_.end(), 0);
-  std::fill(hw_commands_.begin(), hw_commands_.end(), 0);
-  std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0);
-  std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0);
-
-  RCLCPP_INFO(get_logger(), "Successfully configured!");
-
-  return hardware_interface::CallbackReturn::SUCCESS;
-}
-
-std::vector<hardware_interface::StateInterface>
-RRBotSystemWithGPIOHardware::export_state_interfaces()
-{
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
+  for (const auto & [name, descr] : joint_state_interfaces_)
   {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
+    set_state(name, 0.0);
   }
-
-  RCLCPP_INFO(get_logger(), "State interfaces:");
-  hw_gpio_in_.resize(4);
-  size_t ct = 0;
-  for (size_t i = 0; i < info_.gpios.size(); i++)
+  for (const auto & [name, descr] : joint_command_interfaces_)
   {
-    for (auto state_if : info_.gpios.at(i).state_interfaces)
-    {
-      state_interfaces.emplace_back(hardware_interface::StateInterface(
-        info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++]));
-      RCLCPP_INFO(
-        get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), state_if.name.c_str());
-    }
+    set_command(name, 0.0);
   }
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface>
-RRBotSystemWithGPIOHardware::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
+  for (const auto & [name, descr] : gpio_state_interfaces_)
   {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
+    set_state(name, 0.0);
   }
-  RCLCPP_INFO(get_logger(), "Command interfaces:");
-  hw_gpio_out_.resize(2);
-  size_t ct = 0;
-  for (size_t i = 0; i < info_.gpios.size(); i++)
+  for (const auto & [name, descr] : gpio_command_interfaces_)
   {
-    for (auto command_if : info_.gpios.at(i).command_interfaces)
-    {
-      command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++]));
-      RCLCPP_INFO(
-        get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), command_if.name.c_str());
-    }
+    set_command(name, 0.0);
   }
+  RCLCPP_INFO(get_logger(), "Successfully configured!");
 
-  return command_interfaces;
+  return hardware_interface::CallbackReturn::SUCCESS;
 }
 
 hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate(
@@ -194,9 +149,13 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::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_)
+  {
+    set_command(name, get_state(name));
+  }
+  for (const auto & [name, descr] : gpio_command_interfaces_)
   {
-    hw_commands_[i] = hw_states_[i];
+    set_command(name, get_state(name));
   }
 
   RCLCPP_INFO(get_logger(), "Successfully activated!");
@@ -221,25 +180,33 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::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]);
+    auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_;
+    set_state(name, new_value);
+  }
+
+  for (const auto & [name, descr] : gpio_command_interfaces_)
+  {
+    // mirror GPIOs back
+    set_state(name, get_command(name));
   }
 
-  // mirror GPIOs back
-  hw_gpio_in_[0] = hw_gpio_out_[0];
-  hw_gpio_in_[3] = hw_gpio_out_[1];
-  // random inputs
+  // random inputs analog_input1 and analog_input2
   unsigned int seed = time(NULL) + 1;
-  hw_gpio_in_[1] = static_cast<float>(rand_r(&seed));
+  set_state(
+    info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[1].name,
+    static_cast<float>(rand_r(&seed)));
   seed = time(NULL) + 2;
-  hw_gpio_in_[2] = static_cast<float>(rand_r(&seed));
+  set_state(
+    info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[2].name,
+    static_cast<float>(rand_r(&seed)));
 
-  for (uint i = 0; i < hw_gpio_in_.size(); i++)
+  for (const auto & [name, descr] : gpio_state_interfaces_)
   {
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_gpio_in_[i] << " from GPIO input '" << static_cast<int>(i) << "'";
+       << "\t" << get_state(name) << " from GPIO input '" << 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
@@ -254,10 +221,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write(
   std::stringstream ss;
   ss << "Writing commands:";
 
-  for (uint i = 0; i < hw_gpio_out_.size(); i++)
+  for (const auto & [name, descr] : gpio_command_interfaces_)
   {
+    // Simulate sending commands to the hardware
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_gpio_out_[i] << " for GPIO output '" << static_cast<int>(i) << "'";
+       << "\t" << get_command(name) << " for GPIO output '" << 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

example_11/hardware/carlikebot_system.cpp

@@ -56,6 +56,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
     // Steering joints have a position command interface and a position state interface
     if (joint_is_steering)
     {
+      steering_joint_ = joint.name;
       RCLCPP_INFO(get_logger(), "Joint '%s' is a steering joint.", joint.name.c_str());
 
       if (joint.command_interfaces.size() != 1)
@@ -93,6 +94,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
     else
     {
       RCLCPP_INFO(get_logger(), "Joint '%s' is a drive joint.", joint.name.c_str());
+      traction_joint_ = joint.name;
 
       // Drive joints have a velocity command interface and a velocity state interface
       if (joint.command_interfaces.size() != 1)
@@ -143,69 +145,33 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
   hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
   // // END: This part here is for exemplary purposes - Please do not copy to your production code
 
-  hw_interfaces_["steering"] = Joint("virtual_front_wheel_joint");
-
-  hw_interfaces_["traction"] = Joint("virtual_rear_wheel_joint");
-
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
-std::vector<hardware_interface::StateInterface> CarlikeBotSystemHardware::export_state_interfaces()
+hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  std::vector<hardware_interface::StateInterface> state_interfaces;
+  RCLCPP_INFO(get_logger(), "Configuring ...please wait...");
 
-  for (auto & joint : hw_interfaces_)
+  for (auto i = 0; i < hw_start_sec_; i++)
   {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position));
-
-    if (joint.first == "traction")
-    {
-      state_interfaces.emplace_back(hardware_interface::StateInterface(
-        joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, &joint.second.state.velocity));
-    }
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i);
   }
 
-  RCLCPP_INFO(get_logger(), "Exported %zu state interfaces.", state_interfaces.size());
-
-  for (auto s : state_interfaces)
+  // reset values always when configuring hardware
+  for (const auto & [name, descr] : joint_state_interfaces_)
   {
-    RCLCPP_INFO(get_logger(), "Exported state interface '%s'.", s.get_name().c_str());
+    set_state(name, 0.0);
   }
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface>
-CarlikeBotSystemHardware::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-
-  for (auto & joint : hw_interfaces_)
+  for (const auto & [name, descr] : joint_command_interfaces_)
   {
-    if (joint.first == "steering")
-    {
-      command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        joint.second.joint_name, hardware_interface::HW_IF_POSITION,
-        &joint.second.command.position));
-    }
-    else if (joint.first == "traction")
-    {
-      command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        joint.second.joint_name, hardware_interface::HW_IF_VELOCITY,
-        &joint.second.command.velocity));
-    }
+    set_command(name, 0.0);
   }
 
-  RCLCPP_INFO(get_logger(), "Exported %zu command interfaces.", command_interfaces.size());
-
-  for (auto i = 0u; i < command_interfaces.size(); i++)
-  {
-    RCLCPP_INFO(
-      get_logger(), "Exported command interface '%s'.", command_interfaces[i].get_name().c_str());
-  }
+  RCLCPP_INFO(get_logger(), "Successfully configured!");
 
-  return command_interfaces;
+  return hardware_interface::CallbackReturn::SUCCESS;
 }
 
 hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
@@ -219,20 +185,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
     RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i);
   }
 
-  for (auto & joint : hw_interfaces_)
+  // command and state should be equal when starting
+  for (const auto & [name, descr] : joint_command_interfaces_)
   {
-    joint.second.state.position = 0.0;
-
-    if (joint.first == "traction")
-    {
-      joint.second.state.velocity = 0.0;
-      joint.second.command.velocity = 0.0;
-    }
-
-    else if (joint.first == "steering")
-    {
-      joint.second.command.position = 0.0;
-    }
+    set_command(name, get_state(name));
   }
 
   RCLCPP_INFO(get_logger(), "Successfully activated!");
@@ -261,26 +217,32 @@ hardware_interface::return_type CarlikeBotSystemHardware::read(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
   // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
-
-  hw_interfaces_["steering"].state.position = hw_interfaces_["steering"].command.position;
-
-  hw_interfaces_["traction"].state.velocity = hw_interfaces_["traction"].command.velocity;
-  hw_interfaces_["traction"].state.position +=
-    hw_interfaces_["traction"].state.velocity * period.seconds();
+  // update states from commands and integrate velocity to position
+  set_state(
+    steering_joint_ + "/" + hardware_interface::HW_IF_POSITION,
+    get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION));
+
+  set_state(
+    traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY,
+    get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY));
+  set_state(
+    traction_joint_ + "/" + hardware_interface::HW_IF_POSITION,
+    get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION) +
+      get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) * period.seconds());
 
   std::stringstream ss;
   ss << "Reading states:";
 
   ss << std::fixed << std::setprecision(2) << std::endl
      << "\t"
-     << "position: " << hw_interfaces_["steering"].state.position << " for joint '"
-     << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl
+     << "position: " << get_state(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION)
+     << " for joint '" << steering_joint_ << "'" << std::endl
      << "\t"
-     << "position: " << hw_interfaces_["traction"].state.position << " for joint '"
-     << hw_interfaces_["traction"].joint_name.c_str() << "'" << std::endl
+     << "position: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION)
+     << " for joint '" << traction_joint_ << "'" << std::endl
      << "\t"
-     << "velocity: " << hw_interfaces_["traction"].state.velocity << " for joint '"
-     << hw_interfaces_["traction"].joint_name.c_str() << "'";
+     << "velocity: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY)
+     << " for joint '" << traction_joint_ << "'";
 
   RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
 
@@ -298,11 +260,11 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH
 
   ss << std::fixed << std::setprecision(2) << std::endl
      << "\t"
-     << "position: " << hw_interfaces_["steering"].command.position << " for joint '"
-     << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl
+     << "position: " << get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION)
+     << " for joint '" << steering_joint_ << "'" << std::endl
      << "\t"
-     << "velocity: " << hw_interfaces_["traction"].command.velocity << " for joint '"
-     << hw_interfaces_["traction"].joint_name.c_str() << "'";
+     << "velocity: " << get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY)
+     << " for joint '" << traction_joint_ << "'";
 
   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

example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp

@@ -63,9 +63,8 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
   hardware_interface::CallbackReturn on_init(
     const hardware_interface::HardwareInfo & info) override;
 
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
-
-  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+  hardware_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
 
   hardware_interface::CallbackReturn on_activate(
     const rclcpp_lifecycle::State & previous_state) override;
@@ -84,9 +83,9 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
   double hw_start_sec_;
   double hw_stop_sec_;
 
-  // std::vector<std::tuple<std::string, double, double>>
-  //   hw_interfaces_;  // name of joint, state, command
-  std::map<std::string, Joint> hw_interfaces_;
+  // joint names
+  std::string steering_joint_;
+  std::string traction_joint_;
 };
 
 }  // namespace ros2_control_demo_example_11