Merge branch 'master' into add-mecanum-drive-controller

ros-controls · Nov 12, 2024 · be7aa01 · be7aa01
2 parents dca40d7 + f519170
commit be7aa01
Show file tree

Hide file tree

Showing 57 changed files with 482 additions and 40 deletions.
diff --git a/ackermann_steering_controller/CHANGELOG.rst b/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>`_)

diff --git a/ackermann_steering_controller/package.xml b/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>

diff --git a/admittance_controller/CHANGELOG.rst b/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)
 -------------------
 

diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/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);

diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/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;
       }

diff --git a/admittance_controller/package.xml b/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>

diff --git a/admittance_controller/src/admittance_controller.cpp b/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;
   }

diff --git a/admittance_controller/test/test_admittance_controller.hpp b/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();

diff --git a/bicycle_steering_controller/CHANGELOG.rst b/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>`_)

diff --git a/bicycle_steering_controller/package.xml b/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 --git a/diff_drive_controller/CHANGELOG.rst b/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 --git a/diff_drive_controller/package.xml b/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>

diff --git a/effort_controllers/CHANGELOG.rst b/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)
 -------------------
 

diff --git a/effort_controllers/package.xml b/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>

diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/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)
 -------------------
 

diff --git a/force_torque_sensor_broadcaster/package.xml b/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>

diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package forward_command_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+
 4.15.0 (2024-10-07)
 -------------------
 

diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>forward_command_controller</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>

diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package gripper_controllers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+
 4.15.0 (2024-10-07)
 -------------------
 

diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml
@@ -4,7 +4,7 @@
   schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>gripper_controllers</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>The gripper_controllers package</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
 

diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package imu_sensor_broadcaster
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+
 4.15.0 (2024-10-07)
 -------------------
 

diff --git a/imu_sensor_broadcaster/package.xml b/imu_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>imu_sensor_broadcaster</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Controller to publish readings of IMU sensors.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package joint_state_broadcaster
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+* [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 <https://github.com/ros-controls/ros2_controllers/issues/1331>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.15.0 (2024-10-07)
 -------------------
 

diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>joint_state_broadcaster</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Broadcaster to publish joint state</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Stogl</maintainer>

diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst
@@ -2,6 +2,14 @@
 Changelog for package joint_trajectory_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.16.0 (2024-11-08)
+-------------------
+* [JTC] Fix the JTC length_error exceptions in the tests (`#1360 <https://github.com/ros-controls/ros2_controllers/issues/1360>`_)
+* [jtc] Improve trajectory sampling efficiency (`#1297 <https://github.com/ros-controls/ros2_controllers/issues/1297>`_)
+* fixes for windows compilation (`#1330 <https://github.com/ros-controls/ros2_controllers/issues/1330>`_)
+* [JTC] Add Parameter to Toggle State Setting on Activation (`#1231 <https://github.com/ros-controls/ros2_controllers/issues/1231>`_)
+* Contributors: Gilmar Correia, Kenta Kato, RobertWilbrandt, Sai Kishor Kothakota
+
 4.15.0 (2024-10-07)
 -------------------
 

diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
@@ -69,6 +69,9 @@ class Trajectory
    * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to
    * be reached at the time defined in the segment.
    *
+   * This function assumes that sampling is only done at monotonically increasing \p sample_time
+   * for any trajectory.
+   *
    * Specific case returns for start_segment_itr and end_segment_itr:
    * - Sampling before the trajectory start:
    *   start_segment_itr = begin(), end_segment_itr = begin()
@@ -85,9 +88,12 @@ class Trajectory
    *
    * \param[in] sample_time Time at which trajectory will be sampled.
    * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at
-   * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr
-   * Iterator to the start segment for given \p sample_time. See description above. \param[out]
-   * end_segment_itr Iterator to the end segment for given \p sample_time. See description above.
+   *      all.
+   * \param[out] output_state Calculated new at \p sample_time.
+   * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See
+   *      description above.
+   * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See
+   *      description above.
    */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool sample(
@@ -147,6 +153,14 @@ class Trajectory
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool is_sampled_already() const { return sampled_already_; }
 
+  /// Get the index of the segment start returned by the last \p sample() operation.
+  /**
+   * As the trajectory is only accessed at monotonically increasing sampling times, this index is
+   * used to speed up the selection of relevant trajectory points.
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  size_t last_sample_index() const { return last_sample_idx_; }
+
 private:
   void deduce_from_derivatives(
     trajectory_msgs::msg::JointTrajectoryPoint & first_state,
@@ -160,6 +174,7 @@ class Trajectory
   trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
 
   bool sampled_already_ = false;
+  size_t last_sample_idx_ = 0;
 };
 
 /**

diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>joint_trajectory_controller</name>
-  <version>4.15.0</version>
+  <version>4.16.0</version>
   <description>Controller for executing joint-space trajectories on a group of joints</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Dr. Denis Štogl</maintainer>

diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp
@@ -84,6 +84,7 @@ void Trajectory::update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> j
   trajectory_msg_ = joint_trajectory;
   trajectory_start_time_ = static_cast<rclcpp::Time>(joint_trajectory->header.stamp);
   sampled_already_ = false;
+  last_sample_idx_ = 0;
 }
 
 bool Trajectory::sample(
@@ -149,7 +150,7 @@ bool Trajectory::sample(
 
   // time_from_start + trajectory time is the expected arrival time of trajectory
   const auto last_idx = trajectory_msg_->points.size() - 1;
-  for (size_t i = 0; i < last_idx; ++i)
+  for (size_t i = last_sample_idx_; i < last_idx; ++i)
   {
     auto & point = trajectory_msg_->points[i];
     auto & next_point = trajectory_msg_->points[i + 1];
@@ -175,13 +176,15 @@ bool Trajectory::sample(
       }
       start_segment_itr = begin() + i;
       end_segment_itr = begin() + (i + 1);
+      last_sample_idx_ = i;
       return true;
     }
   }
 
   // whole animation has played out
   start_segment_itr = --end();
   end_segment_itr = end();
+  last_sample_idx_ = last_idx;
   output_state = (*start_segment_itr);
   // the trajectories in msg may have empty velocities/accel, so resize them
   if (output_state.velocities.empty())