Merge branch 'master' into gpio_controllers

ackermann_steering_controller/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package ackermann_steering_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+
 4.15.0 (2024-10-07)
 -------------------
 * Adapt test to new way of exporting reference interfaces (Related to `#1240 <https://github.com/ros-controls/ros2_controllers/issues/1240>`_ in ros2_control) (`#1103 <https://github.com/ros-controls/ros2_controllers/issues/1103>`_)

ackermann_steering_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ackermann_steering_controller</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it.</description>
   <license>Apache License 2.0</license>
   <maintainer email="[email protected]">Bence Magyar</maintainer>

admittance_controller/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package admittance_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+* Adding use of robot description parameter in the Admittance Controller (`#1247 <https://github.com/ros-controls/ros2_controllers/issues/1247>`_)
+* Contributors: Dr. Denis, Kevin DeMarco, Nikola Banovic, Bence Magyar, Christoph Fröhlich
+
 4.15.0 (2024-10-07)
 -------------------
 

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -102,7 +102,8 @@ class AdmittanceRule
 
   /// Configure admittance rule memory using number of joints.
   controller_interface::return_type configure(
-    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint);
+    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint,
+    const std::string & robot_description);
 
   /// Reset all values back to default
   controller_interface::return_type reset(const size_t num_joints);

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -20,6 +20,7 @@
 #include "admittance_controller/admittance_rule.hpp"
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include <control_toolbox/filters.hpp>
@@ -34,7 +35,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6;  // (3 translation + 3 rotation)
 
 /// Configure admittance rule memory for num joints and load kinematics interface
 controller_interface::return_type AdmittanceRule::configure(
-  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints)
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints,
+  const std::string & robot_description)
 {
   num_joints_ = num_joints;
 
@@ -58,7 +60,7 @@ controller_interface::return_type AdmittanceRule::configure(
         kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));
 
       if (!kinematics_->initialize(
-            node->get_node_parameters_interface(), parameters_.kinematics.tip))
+            robot_description, node->get_node_parameters_interface(), "kinematics"))
       {
         return controller_interface::return_type::ERROR;
       }

admittance_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>admittance_controller</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Implementation of admittance controllers for different input and output interface.</description>
   <maintainer email="[email protected]">Denis Štogl</maintainer>
   <maintainer email="[email protected]">Bence Magyar</maintainer>

admittance_controller/src/admittance_controller.cpp

@@ -280,7 +280,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
     semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name));
 
   // configure admittance rule
-  if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR)
+  if (
+    admittance_->configure(get_node(), num_joints_, this->get_robot_description()) ==
+    controller_interface::return_type::ERROR)
   {
     return controller_interface::CallbackReturn::ERROR;
   }

admittance_controller/test/test_admittance_controller.hpp

@@ -102,18 +102,14 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
     }
   }
 
-private:
   const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
   const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf;
 };
 
 class AdmittanceControllerTest : public ::testing::Test
 {
 public:
-  static void SetUpTestCase()
-  {
-    //    rclcpp::init(0, nullptr);
-  }
+  static void SetUpTestCase() {}
 
   void SetUp()
   {
@@ -163,7 +159,8 @@ class AdmittanceControllerTest : public ::testing::Test
   controller_interface::return_type SetUpControllerCommon(
     const std::string & controller_name, const rclcpp::NodeOptions & options)
   {
-    auto result = controller_->init(controller_name, "", 0, "", options);
+    auto result =
+      controller_->init(controller_name, controller_->robot_description_, 0, "", options);
 
     controller_->export_reference_interfaces();
     assign_interfaces();

bicycle_steering_controller/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package bicycle_steering_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+
 4.15.0 (2024-10-07)
 -------------------
 * Adapt test to new way of exporting reference interfaces (Related to `#1240 <https://github.com/ros-controls/ros2_controllers/issues/1240>`_ in ros2_control) (`#1103 <https://github.com/ros-controls/ros2_controllers/issues/1103>`_)

bicycle_steering_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>bicycle_steering_controller</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering.</description>
   <license>Apache License 2.0</license>
   <maintainer email="[email protected]">Bence Magyar</maintainer>

diff_drive_controller/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package diff_drive_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+
 4.15.0 (2024-10-07)
 -------------------
 

diff_drive_controller/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>diff_drive_controller</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Controller for a differential drive mobile base.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Jordan Palacios</maintainer>

effort_controllers/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package effort_controllers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+
 4.15.0 (2024-10-07)
 -------------------
 

effort_controllers/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>effort_controllers</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Generic controller for forwarding commands.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Jordan Palacios</maintainer>

force_torque_sensor_broadcaster/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package force_torque_sensor_broadcaster
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+* [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 <https://github.com/ros-controls/ros2_controllers/issues/1215>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.15.0 (2024-10-07)
 -------------------
 

force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml

@@ -1,6 +1,6 @@
 <library path="force_torque_sensor_broadcaster">
   <class name="force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"
-         type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ControllerInterface">
+         type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface">
   <description>
     This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message.
   </description>

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

@@ -20,8 +20,9 @@
 #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
 
 #include <memory>
+#include <vector>
 
-#include "controller_interface/controller_interface.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
 #include "force_torque_sensor_broadcaster/visibility_control.h"
 // auto-generated by generate_parameter_library
 #include "force_torque_sensor_broadcaster_parameters.hpp"
@@ -32,7 +33,7 @@
 
 namespace force_torque_sensor_broadcaster
 {
-class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface
+class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface
 {
 public:
   FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
@@ -59,10 +60,19 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte
     const rclcpp_lifecycle::State & previous_state) override;
 
   FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
-  controller_interface::return_type update(
+  controller_interface::return_type update_and_write_commands(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
+  std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
+
 protected:
+  void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg);
+
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
 

force_torque_sensor_broadcaster/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>force_torque_sensor_broadcaster</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Controller to publish state of force-torque sensors.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -24,7 +24,7 @@
 namespace force_torque_sensor_broadcaster
 {
 ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster()
-: controller_interface::ControllerInterface()
+: controller_interface::ChainableControllerInterface()
 {
 }
 
@@ -141,23 +141,97 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type ForceTorqueSensorBroadcaster::update(
+controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
+  if (param_listener_->is_old(params_))
+  {
+    params_ = param_listener_->get_params();
+  }
   if (realtime_publisher_ && realtime_publisher_->trylock())
   {
     realtime_publisher_->msg_.header.stamp = time;
     force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench);
+    this->apply_sensor_offset(params_, realtime_publisher_->msg_);
     realtime_publisher_->unlockAndPublish();
   }
 
   return controller_interface::return_type::OK;
 }
 
+controller_interface::return_type ForceTorqueSensorBroadcaster::update_reference_from_subscribers(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  return controller_interface::return_type::OK;
+}
+
+std::vector<hardware_interface::StateInterface>
+ForceTorqueSensorBroadcaster::on_export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> exported_state_interfaces;
+
+  std::vector<std::string> force_names(
+    {params_.interface_names.force.x, params_.interface_names.force.y,
+     params_.interface_names.force.z});
+  std::vector<std::string> torque_names(
+    {params_.interface_names.torque.x, params_.interface_names.torque.y,
+     params_.interface_names.torque.z});
+  if (!params_.sensor_name.empty())
+  {
+    const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names();
+    std::copy(
+      semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin());
+    std::copy(
+      semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin());
+  }
+  const std::string controller_name = get_node()->get_name();
+  if (!force_names[0].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x));
+  }
+  if (!force_names[1].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y));
+  }
+  if (!force_names[2].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z));
+  }
+  if (!torque_names[0].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x));
+  }
+  if (!torque_names[1].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y));
+  }
+  if (!torque_names[2].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z));
+  }
+  return exported_state_interfaces;
+}
+
+void ForceTorqueSensorBroadcaster::apply_sensor_offset(
+  const Params & params, geometry_msgs::msg::WrenchStamped & msg)
+{
+  msg.wrench.force.x += params.offset.force.x;
+  msg.wrench.force.y += params.offset.force.y;
+  msg.wrench.force.z += params.offset.force.z;
+  msg.wrench.torque.x += params.offset.torque.x;
+  msg.wrench.torque.y += params.offset.torque.y;
+  msg.wrench.torque.z += params.offset.torque.z;
+}
 }  // namespace force_torque_sensor_broadcaster
 
 #include "pluginlib/class_list_macros.hpp"
 
 PLUGINLIB_EXPORT_CLASS(
   force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster,
-  controller_interface::ControllerInterface)
+  controller_interface::ChainableControllerInterface)

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml

@@ -46,3 +46,36 @@ force_torque_sensor_broadcaster:
         default_value: "",
         description: "Name of the state interface with torque values around 'z' axis.",
       }
+  offset:
+    force:
+      x: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of force values on 'x' axis.",
+      }
+      y: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of force values on 'y' axis.",
+      }
+      z: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of force values on 'z' axis.",
+      }
+    torque:
+      x: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of torque values around 'x' axis.",
+      }
+      y: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of torque values around 'y' axis.",
+      }
+      z: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of torque values around 'z' axis.",
+      }