Ex7

example_7/doc/userdoc.rst

@@ -193,13 +193,14 @@ In ros2_control, hardware system components are integrated via user defined driv
 
 The following code blocks will explain the requirements for writing a new hardware interface.
 
-The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from  ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement five public methods.
+The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from  ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement the following public methods.
 
 1. ``on_init``
-2. ``export_state_interfaces``
-3. ``export_command_interfaces``
-4. ``read``
-5. ``write``
+2. ``on_configure``
+3. ``read``
+4. ``write``
+
+There are more methods that can be implemented for lifecycle changes, but they are not required for the tutorial robot.
 
 .. code-block:: c++
 
@@ -209,15 +210,14 @@ The hardware plugin for the tutorial robot is a class called ``RobotSystem`` tha
   class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface {
       public:
       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;
+      CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
       return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
       return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
       // private members
       // ...
   }
 
-The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established. Instead, vectors will be initialized that represent the state all the hardware, e.g. a vector of doubles describing joint angles, etc.
+The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established.
 
 .. code-block:: c++
 
@@ -232,26 +232,15 @@ The ``on_init`` method is called once during ros2_control initialization if the
 
 Notably, the behavior of ``on_init`` is expected to vary depending on the URDF file. The ``SystemInterface::on_init(info)`` call fills out the ``info`` object with specifics from the URDF. For example, the ``info`` object has fields for joints, sensors, gpios, and more. Suppose the sensor field has a name value of ``tcp_force_torque_sensor``. Then the ``on_init`` must try to establish communication with that sensor. If it fails, then an error value is returned.
 
-Next, ``export_state_interfaces`` and ``export_command_interfaces`` methods are called in succession. The ``export_state_interfaces`` method returns a vector of ``StateInterface``, describing the ``state_interfaces`` for each joint. The ``StateInterface`` objects are read only data handles. Their constructors require an interface name, interface type, and a pointer to a double data value. For the ``RobotSystem``, the data pointers reference class member variables. This way, the data can be access from every method.
-
-.. code-block:: c++
-
-  std::vector<hardware_interface::StateInterface> RobotSystem::export_state_interfaces() {
-      std::vector<hardware_interface::StateInterface> state_interfaces;
-      // add state interfaces to ``state_interfaces`` for each joint, e.g. `info_.joints[0].state_interfaces_`, `info_.joints[1].state_interfaces_`, `info_.joints[2].state_interfaces_` ...
-      // ...
-      return state_interfaces;
-    }
-
-The ``export_command_interfaces`` method is nearly identical to the previous one. The difference is that a vector of ``CommandInterface`` is returned. The vector contains objects describing the ``command_interfaces`` for each joint.
+The ``on_configure`` method is called once during ros2_control lifecycle of the ``RobotSystem``. Initial values are set for state and command interfaces that represent the state all the hardware, e.g. doubles describing joint angles, etc.
 
 .. code-block:: c++
 
-  std::vector<hardware_interface::CommandInterface> RobotSystem::export_command_interfaces() {
-      std::vector<hardware_interface::CommandInterface> command_interfaces;
-      // add command interfaces to ``command_interfaces`` for each joint, e.g. `info_.joints[0].command_interfaces_`, `info_.joints[1].command_interfaces_`, `info_.joints[2].command_interfaces_` ...
+  CallbackReturn RobotSystem::on_configure(
+    const rclcpp_lifecycle::State & /*previous_state*/)
+      // setup communication with robot hardware
       // ...
-      return command_interfaces;
+      return CallbackReturn::SUCCESS;
   }
 
 The ``read`` method is core method in the ros2_control loop. During the main loop, ros2_control loops over all hardware components and calls the ``read`` method. It is executed on the realtime thread, hence the method must obey by realtime constraints. The ``read`` method is responsible for updating the data values of the ``state_interfaces``. Since the data value point to class member variables, those values can be filled with their corresponding sensor values, which will in turn update the values of each exported ``StateInterface`` object.

example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp

@@ -36,25 +36,13 @@ class RobotSystem : public hardware_interface::SystemInterface
 public:
   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;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
   return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
 
 protected:
-  /// The size of this vector is (standard_interfaces_.size() x nr_joints)
-  std::vector<double> joint_position_command_;
-  std::vector<double> joint_velocities_command_;
-  std::vector<double> joint_position_;
-  std::vector<double> joint_velocities_;
-  std::vector<double> ft_states_;
-  std::vector<double> ft_command_;
-
-  std::unordered_map<std::string, std::vector<std::string>> joint_interfaces = {
-    {"position", {}}, {"velocity", {}}};
 };
 
 }  // namespace ros2_control_demo_example_7

example_7/hardware/r6bot_hardware.cpp

@@ -16,6 +16,8 @@
 #include <string>
 #include <vector>
 
+#include <iostream>
+
 namespace ros2_control_demo_example_7
 {
 CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & info)
@@ -24,95 +26,39 @@ CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & inf
   {
     return CallbackReturn::ERROR;
   }
-
-  // robot has 6 joints and 2 interfaces
-  joint_position_.assign(6, 0);
-  joint_velocities_.assign(6, 0);
-  joint_position_command_.assign(6, 0);
-  joint_velocities_command_.assign(6, 0);
-
-  // force sensor has 6 readings
-  ft_states_.assign(6, 0);
-  ft_command_.assign(6, 0);
-
-  for (const auto & joint : info_.joints)
-  {
-    for (const auto & interface : joint.state_interfaces)
-    {
-      joint_interfaces[interface.name].push_back(joint.name);
-    }
-  }
-
-  return CallbackReturn::SUCCESS;
+  return hardware_interface::CallbackReturn::SUCCESS;
 }
 
-std::vector<hardware_interface::StateInterface> RobotSystem::export_state_interfaces()
+CallbackReturn RobotSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-
-  int ind = 0;
-  for (const auto & joint_name : joint_interfaces["position"])
+  // reset values always when configuring hardware
+  for (const auto & [name, descr] : joint_state_interfaces_)
   {
-    state_interfaces.emplace_back(joint_name, "position", &joint_position_[ind++]);
+    set_state(name, 0.0);
   }
-
-  ind = 0;
-  for (const auto & joint_name : joint_interfaces["velocity"])
+  for (const auto & [name, descr] : joint_command_interfaces_)
   {
-    state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]);
+    set_command(name, 0.0);
   }
-
-  state_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_states_[0]);
-  state_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_states_[1]);
-  state_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_states_[2]);
-  state_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_states_[3]);
-  state_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_states_[4]);
-  state_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_states_[5]);
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface> RobotSystem::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-
-  int ind = 0;
-  for (const auto & joint_name : joint_interfaces["position"])
-  {
-    command_interfaces.emplace_back(joint_name, "position", &joint_position_command_[ind++]);
-  }
-
-  ind = 0;
-  for (const auto & joint_name : joint_interfaces["velocity"])
+  for (const auto & [name, descr] : sensor_state_interfaces_)
   {
-    command_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_command_[ind++]);
+    set_state(name, 0.0);
   }
 
-  command_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_command_[0]);
-  command_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_command_[1]);
-  command_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_command_[2]);
-  command_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_command_[3]);
-  command_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_command_[4]);
-  command_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_command_[5]);
-
-  return command_interfaces;
+  return CallbackReturn::SUCCESS;
 }
 
 return_type RobotSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
   // TODO(pac48) set sensor_states_ values from subscriber
 
-  for (auto i = 0ul; i < joint_velocities_command_.size(); i++)
-  {
-    joint_velocities_[i] = joint_velocities_command_[i];
-    joint_position_[i] += joint_velocities_command_[i] * period.seconds();
-  }
-
-  for (auto i = 0ul; i < joint_position_command_.size(); i++)
+  for (std::size_t i = 0; i < info_.joints.size(); i++)
   {
-    joint_position_[i] = joint_position_command_[i];
+    const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY;
+    const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION;
+    set_state(name_vel, get_command(name_vel));
+    set_state(name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds());
   }
-
   return return_type::OK;
 }
 

example_7/reference_generator/send_trajectory.cpp

@@ -96,6 +96,17 @@ int main(int argc, char ** argv)
     trajectory_msg.points.push_back(trajectory_point_msg);
   }
 
+  // send zero velocities in the end
+  std::memset(
+    trajectory_point_msg.velocities.data(), 0.0,
+    trajectory_point_msg.velocities.size() * sizeof(double));
+  trajectory_point_msg.time_from_start.sec = trajectory_len / loop_rate;
+  trajectory_point_msg.time_from_start.nanosec = static_cast<int>(
+    1E9 / loop_rate *
+    static_cast<double>(
+      trajectory_len - loop_rate * (trajectory_len / loop_rate)));  // implicit integer division
+  trajectory_msg.points.push_back(trajectory_point_msg);
+
   pub->publish(trajectory_msg);
   while (rclcpp::ok())
   {