Rewrite resource storage and add mimic test (#44)

Author: christophfroehlich

joint_state_topic_hardware_interface/CMakeLists.txt

@@ -81,12 +81,19 @@ if(BUILD_TESTING)
     rclcpp::rclcpp
     rclcpp_action::rclcpp_action
   )
+  add_executable(test_gripper test/gripper/test_gripper.cpp)
+  target_link_libraries(test_gripper PUBLIC
+    ${std_msgs_TARGETS}
+    rclcpp::rclcpp
+  )
   function(add_ros_isolated_launch_test path)
     set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py")
     add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN})
   endfunction()
   add_ros_isolated_launch_test(
     test/rrr/position.test.py)
+  add_ros_isolated_launch_test(
+    test/gripper/position.test.py)
 endif()
 
 pluginlib_export_plugin_description_file(hardware_interface joint_state_topic_hardware_interface_plugin_description.xml)

joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp

@@ -38,10 +38,6 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
 public:
   CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;
 
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
-
-  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
-
   hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
   hardware_interface::return_type write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override;
@@ -52,31 +48,9 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
   sensor_msgs::msg::JointState latest_joint_state_;
   bool sum_wrapped_joint_states_{ false };
 
-  /// Use standard interfaces for joints because they are relevant for dynamic behavior
-  std::array<std::string, 4> standard_interfaces_ = {
-    { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION,
-      hardware_interface::HW_IF_EFFORT }
-  };
-
-  struct MimicJoint
-  {
-    std::size_t joint_index;
-    std::size_t mimicked_joint_index;
-    double multiplier = 1.0;
-  };
-  std::vector<MimicJoint> mimic_joints_;
-
-  /// The size of this vector is (standard_interfaces_.size() x nr_joints)
-  std::vector<std::vector<double>> joint_command_values_;
-  std::vector<std::vector<double>> joint_state_values_;
-
   // If the difference between the current joint state and joint command is less than this value,
   // the joint command will not be published.
   double trigger_joint_command_threshold_ = 1e-5;
-
-  template <typename HandleType>
-  bool getInterface(const std::string& name, const std::string& interface_name, const size_t vector_index,
-                    std::vector<std::vector<double>>& values, std::vector<HandleType>& interfaces);
 };
 
 }  // namespace joint_state_topic_hardware_interface

joint_state_topic_hardware_interface/package.xml

@@ -30,6 +30,7 @@
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>control_msgs</test_depend>
   <test_depend>controller_manager</test_depend>
+  <test_depend>forward_command_controller</test_depend>
   <test_depend>joint_state_broadcaster</test_depend>
   <test_depend>joint_trajectory_controller</test_depend>
   <test_depend>launch_ros</test_depend>

joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp

@@ -29,13 +29,14 @@ namespace
 {
 /** @brief Sums the total rotation for joint states that wrap from 2*pi to -2*pi
 when rotating in the positive direction */
-void sumRotationFromMinus2PiTo2Pi(const double current_wrapped_rad, double& total_rotation)
+double sumRotationFromMinus2PiTo2Pi(const double current_wrapped_rad, double total_rotation_in)
 {
   double delta = 0;
-  angles::shortest_angular_distance_with_large_limits(total_rotation, current_wrapped_rad, 2 * M_PI, -2 * M_PI, delta);
+  angles::shortest_angular_distance_with_large_limits(total_rotation_in, current_wrapped_rad, 2 * M_PI, -2 * M_PI,
+                                                      delta);
 
   // Add the corrected delta to the total rotation
-  total_rotation += delta;
+  return total_rotation_in + delta;
 }
 }  // namespace
 
@@ -54,64 +55,6 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
     return CallbackReturn::ERROR;
   }
 
-  // Initialize storage for all joints' standard interfaces, regardless of their existence and set all values to nan
-  joint_command_values_.resize(standard_interfaces_.size());
-  joint_state_values_.resize(standard_interfaces_.size());
-  for (auto i = 0u; i < standard_interfaces_.size(); i++)
-  {
-    joint_command_values_[i].resize(get_hardware_info().joints.size(), 0.0);
-    joint_state_values_[i].resize(get_hardware_info().joints.size(), 0.0);
-  }
-
-  // Initial command values
-  for (auto i = 0u; i < get_hardware_info().joints.size(); i++)
-  {
-    const auto& component = get_hardware_info().joints[i];
-    for (const auto& interface : component.state_interfaces)
-    {
-      auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name);
-      // If interface name is found in the interfaces list
-      if (it != standard_interfaces_.end())
-      {
-        auto index = static_cast<std::size_t>(std::distance(standard_interfaces_.begin(), it));
-        // Check the initial_value param is used
-        if (!interface.initial_value.empty())
-        {
-          joint_state_values_[index][i] = std::stod(interface.initial_value);
-          joint_command_values_[index][i] = std::stod(interface.initial_value);
-        }
-      }
-    }
-  }
-
-  // Search for mimic joints
-  for (auto i = 0u; i < get_hardware_info().joints.size(); ++i)
-  {
-    const auto& joint = get_hardware_info().joints.at(i);
-    if (joint.parameters.find("mimic") != joint.parameters.cend())
-    {
-      const auto mimicked_joint_it = std::find_if(
-          get_hardware_info().joints.begin(), get_hardware_info().joints.end(),
-          [&mimicked_joint = joint.parameters.at("mimic")](const hardware_interface::ComponentInfo& joint_info) {
-            return joint_info.name == mimicked_joint;
-          });
-      if (mimicked_joint_it == get_hardware_info().joints.cend())
-      {
-        throw std::runtime_error(std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found");
-      }
-      MimicJoint mimic_joint;
-      mimic_joint.joint_index = i;
-      mimic_joint.mimicked_joint_index =
-          static_cast<std::size_t>(std::distance(get_hardware_info().joints.begin(), mimicked_joint_it));
-      auto param_it = joint.parameters.find("multiplier");
-      if (param_it != joint.parameters.end())
-      {
-        mimic_joint.multiplier = std::stod(joint.parameters.at("multiplier"));
-      }
-      mimic_joints_.push_back(mimic_joint);
-    }
-  }
-
   const auto get_hardware_parameter = [this](const std::string& parameter_name, const std::string& default_value) {
     if (auto it = get_hardware_info().hardware_parameters.find(parameter_name);
         it != get_hardware_info().hardware_parameters.end())
@@ -143,164 +86,130 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
   return CallbackReturn::SUCCESS;
 }
 
-std::vector<hardware_interface::StateInterface> JointStateTopicSystem::export_state_interfaces()
-{
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-
-  // Joints' state interfaces
-  for (auto i = 0u; i < get_hardware_info().joints.size(); i++)
-  {
-    const auto& joint = get_hardware_info().joints[i];
-    for (const auto& interface : joint.state_interfaces)
-    {
-      // Add interface: if not in the standard list then use "other" interface list
-      if (!getInterface(joint.name, interface.name, i, joint_state_values_, state_interfaces))
-      {
-        throw std::runtime_error("Interface is not found in the standard list.");
-      }
-    }
-  }
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface> JointStateTopicSystem::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-
-  // Joints' state interfaces
-  for (auto i = 0u; i < get_hardware_info().joints.size(); i++)
-  {
-    const auto& joint = get_hardware_info().joints[i];
-    for (const auto& interface : joint.command_interfaces)
-    {
-      if (!getInterface(joint.name, interface.name, i, joint_command_values_, command_interfaces))
-      {
-        throw std::runtime_error("Interface is not found in the standard list.");
-      }
-    }
-  }
-
-  return command_interfaces;
-}
-
 hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& /*time*/,
                                                             const rclcpp::Duration& /*period*/)
 {
+  const auto& joints = get_hardware_info().joints;
   for (std::size_t i = 0; i < latest_joint_state_.name.size(); ++i)
   {
-    const auto& joints = get_hardware_info().joints;
-    auto it = std::find_if(joints.begin(), joints.end(),
-                           [&joint_name = std::as_const(latest_joint_state_.name[i])](
-                               const hardware_interface::ComponentInfo& info) { return joint_name == info.name; });
+    const auto it = std::find_if(joints.begin(), joints.end(),
+                                 [name = latest_joint_state_.name[i]](const hardware_interface::ComponentInfo& joint) {
+                                   return joint.name == name;
+                                 });
     if (it != joints.end())
     {
-      auto j = static_cast<std::size_t>(std::distance(joints.begin(), it));
-      if (sum_wrapped_joint_states_)
+      if (std::find_if(get_hardware_info().mimic_joints.begin(), get_hardware_info().mimic_joints.end(),
+                       [idx = static_cast<std::size_t>(std::distance(joints.begin(), it))](
+                           const hardware_interface::MimicJoint& mimic_joint) {
+                         return idx == mimic_joint.joint_index;
+                       }) != get_hardware_info().mimic_joints.end())
       {
-        sumRotationFromMinus2PiTo2Pi(latest_joint_state_.position[i], joint_state_values_[POSITION_INTERFACE_INDEX][j]);
+        // mimic joints are updated at the end of this function
+        continue;
       }
-      else
+
+      if (!latest_joint_state_.position.empty() && std::isfinite(latest_joint_state_.position.at(i)))
       {
-        joint_state_values_[POSITION_INTERFACE_INDEX][j] = latest_joint_state_.position[i];
+        if (sum_wrapped_joint_states_)
+        {
+          auto name = latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_POSITION;
+
+          set_state(name, sumRotationFromMinus2PiTo2Pi(latest_joint_state_.position.at(i), get_state(name)));
+        }
+        else
+        {
+          set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_POSITION,
+                    latest_joint_state_.position.at(i));
+        }
       }
-      if (!latest_joint_state_.velocity.empty())
+      if (!latest_joint_state_.velocity.empty() && std::isfinite(latest_joint_state_.velocity.at(i)))
       {
-        joint_state_values_[VELOCITY_INTERFACE_INDEX][j] = latest_joint_state_.velocity[i];
+        set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_VELOCITY,
+                  latest_joint_state_.velocity.at(i));
       }
-      if (!latest_joint_state_.effort.empty())
+      if (!latest_joint_state_.effort.empty() && std::isfinite(latest_joint_state_.effort.at(i)))
       {
-        joint_state_values_[EFFORT_INTERFACE_INDEX][j] = latest_joint_state_.effort[i];
+        set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_EFFORT,
+                  latest_joint_state_.effort.at(i));
       }
     }
   }
 
-  for (const auto& mimic_joint : mimic_joints_)
+  // Update mimic joints
+  for (const auto& mimic_joint : get_hardware_info().mimic_joints)
   {
-    for (auto& joint_state : joint_state_values_)
+    const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name;
+    const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name;
+    if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION))
     {
-      joint_state[mimic_joint.joint_index] = mimic_joint.multiplier * joint_state[mimic_joint.mimicked_joint_index];
+      set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION,
+                mimic_joint.offset +
+                    mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION));
+    }
+    if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY))
+    {
+      set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY,
+                mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY));
+    }
+    if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION))
+    {
+      set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION,
+                mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION));
     }
   }
 
   return hardware_interface::return_type::OK;
 }
 
-template <typename HandleType>
-bool JointStateTopicSystem::getInterface(const std::string& name, const std::string& interface_name,
-                                         const size_t vector_index, std::vector<std::vector<double>>& values,
-                                         std::vector<HandleType>& interfaces)
-{
-  auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface_name);
-  if (it != standard_interfaces_.end())
-  {
-    auto j = static_cast<std::size_t>(std::distance(standard_interfaces_.begin(), it));
-    interfaces.emplace_back(name, *it, &values[j][vector_index]);
-    return true;
-  }
-  return false;
-}
-
 hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time& /*time*/,
                                                              const rclcpp::Duration& /*period*/)
 {
+  const auto& joints = get_hardware_info().joints;
   // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and
   // the current joint commands, if it's smaller than a threshold we don't publish it.
-  const auto diff = std::transform_reduce(
-      joint_state_values_[POSITION_INTERFACE_INDEX].cbegin(), joint_state_values_[POSITION_INTERFACE_INDEX].cend(),
-      joint_command_values_[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
-      [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
+  auto diff = 0.0;
+  for (std::size_t i = 0; i < joints.size(); ++i)
+  {
+    for (const auto& interface : joints[i].command_interfaces)
+    {
+      if (interface.name != hardware_interface::HW_IF_POSITION)
+      {
+        continue;
+      }
+      // sum the absolute difference for all joints
+      diff += std::abs(get_state(joints[i].name + "/" + interface.name) -
+                       get_command(joints[i].name + "/" + interface.name));
+    }
+  }
   if (diff <= trigger_joint_command_threshold_)
   {
     return hardware_interface::return_type::OK;
   }
 
   sensor_msgs::msg::JointState joint_state;
-  for (std::size_t i = 0; i < get_hardware_info().joints.size(); ++i)
+  for (std::size_t i = 0; i < joints.size(); ++i)
   {
-    joint_state.name.push_back(get_hardware_info().joints[i].name);
+    joint_state.name.push_back(joints[i].name);
     joint_state.header.stamp = get_node()->now();
     // only send commands to the interfaces that are defined for this joint
-    for (const auto& interface : get_hardware_info().joints[i].command_interfaces)
+    for (const auto& interface : joints[i].command_interfaces)
     {
       if (interface.name == hardware_interface::HW_IF_POSITION)
       {
-        joint_state.position.push_back(joint_command_values_[POSITION_INTERFACE_INDEX][i]);
+        joint_state.position.push_back(get_command(joints[i].name + "/" + interface.name));
       }
       else if (interface.name == hardware_interface::HW_IF_VELOCITY)
       {
-        joint_state.velocity.push_back(joint_command_values_[VELOCITY_INTERFACE_INDEX][i]);
+        joint_state.velocity.push_back(get_command(joints[i].name + "/" + interface.name));
       }
       else if (interface.name == hardware_interface::HW_IF_EFFORT)
       {
-        joint_state.effort.push_back(joint_command_values_[EFFORT_INTERFACE_INDEX][i]);
+        joint_state.effort.push_back(get_command(joints[i].name + "/" + interface.name));
       }
       else
       {
         RCLCPP_WARN_ONCE(get_node()->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.",
-                         get_hardware_info().joints[i].name.c_str(), interface.name.c_str());
-      }
-    }
-  }
-
-  for (const auto& mimic_joint : mimic_joints_)
-  {
-    for (const auto& interface : get_hardware_info().joints[mimic_joint.mimicked_joint_index].command_interfaces)
-    {
-      if (interface.name == hardware_interface::HW_IF_POSITION)
-      {
-        joint_state.position[mimic_joint.joint_index] =
-            mimic_joint.multiplier * joint_state.position[mimic_joint.mimicked_joint_index];
-      }
-      else if (interface.name == hardware_interface::HW_IF_VELOCITY)
-      {
-        joint_state.velocity[mimic_joint.joint_index] =
-            mimic_joint.multiplier * joint_state.velocity[mimic_joint.mimicked_joint_index];
-      }
-      else if (interface.name == hardware_interface::HW_IF_EFFORT)
-      {
-        joint_state.effort[mimic_joint.joint_index] =
-            mimic_joint.multiplier * joint_state.effort[mimic_joint.mimicked_joint_index];
+                         joints[i].name.c_str(), interface.name.c_str());
       }
     }
   }