Merge branch 'master' into feature/admittance_wrench_target_rolling

ros-controls · Dec 9, 2024 · 9509e53 · 9509e53
2 parents 7aa9691 + 2c7047e
commit 9509e53
Show file tree

Hide file tree

Showing 75 changed files with 697 additions and 96 deletions.
diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package ackermann_steering_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-12-07)
+-------------------
+* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 <https://github.com/ros-controls/ros2_controllers/issues/1370>`_)
+* Update maintainers and add url tags (`#1363 <https://github.com/ros-controls/ros2_controllers/issues/1363>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
 4.16.0 (2024-11-08)
 -------------------
 

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.16.0</version>
+  <version>4.17.0</version>
   <description>Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package admittance_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-12-07)
+-------------------
+* Use the .hpp headers from `realtime_tools` package (`#1406 <https://github.com/ros-controls/ros2_controllers/issues/1406>`_)
+* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 <https://github.com/ros-controls/ros2_controllers/issues/1370>`_)
+* Update maintainers and add url tags (`#1363 <https://github.com/ros-controls/ros2_controllers/issues/1363>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
 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>`_)

diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp
@@ -32,8 +32,8 @@
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.h"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
 
 namespace admittance_controller

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.16.0</version>
+  <version>4.17.0</version>
   <description>Implementation of admittance controllers for different input and output interface.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package bicycle_steering_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-12-07)
+-------------------
+* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 <https://github.com/ros-controls/ros2_controllers/issues/1370>`_)
+* Update maintainers and add url tags (`#1363 <https://github.com/ros-controls/ros2_controllers/issues/1363>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
 4.16.0 (2024-11-08)
 -------------------
 

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.16.0</version>
+  <version>4.17.0</version>
   <description>Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst
@@ -2,6 +2,15 @@
 Changelog for package diff_drive_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-12-07)
+-------------------
+* Use the .hpp headers from `realtime_tools` package (`#1406 <https://github.com/ros-controls/ros2_controllers/issues/1406>`_)
+* Fix RealtimeBox API changes (`#1385 <https://github.com/ros-controls/ros2_controllers/issues/1385>`_)
+* Rename Twist->TwistStamped (`#1393 <https://github.com/ros-controls/ros2_controllers/issues/1393>`_)
+* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 <https://github.com/ros-controls/ros2_controllers/issues/1370>`_)
+* Update maintainers and add url tags (`#1363 <https://github.com/ros-controls/ros2_controllers/issues/1363>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
 4.16.0 (2024-11-08)
 -------------------
 

diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
@@ -34,8 +34,8 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "odometry.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_box.h"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_box.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
 // auto-generated by generate_parameter_library
@@ -131,6 +131,7 @@ class DiffDriveController : public controller_interface::ControllerInterface
   rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
 
   realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
+  std::shared_ptr<TwistStamped> last_command_msg_;
 
   std::queue<TwistStamped> previous_commands_;  // last two commands
 

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.16.0</version>
+  <version>4.17.0</version>
   <description>Controller for a differential-drive mobile base.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -111,26 +111,27 @@ controller_interface::return_type DiffDriveController::update(
     return controller_interface::return_type::OK;
   }
 
-  std::shared_ptr<TwistStamped> last_command_msg;
-  received_velocity_msg_ptr_.get(last_command_msg);
+  // if the mutex is unable to lock, last_command_msg_ won't be updated
+  received_velocity_msg_ptr_.try_get([this](const std::shared_ptr<TwistStamped> & msg)
+                                     { last_command_msg_ = msg; });
 
-  if (last_command_msg == nullptr)
+  if (last_command_msg_ == nullptr)
   {
     RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
     return controller_interface::return_type::ERROR;
   }
 
-  const auto age_of_last_command = time - last_command_msg->header.stamp;
+  const auto age_of_last_command = time - last_command_msg_->header.stamp;
   // Brake if cmd_vel has timeout, override the stored command
   if (age_of_last_command > cmd_vel_timeout_)
   {
-    last_command_msg->twist.linear.x = 0.0;
-    last_command_msg->twist.angular.z = 0.0;
+    last_command_msg_->twist.linear.x = 0.0;
+    last_command_msg_->twist.angular.z = 0.0;
   }
 
   // command may be limited further by SpeedLimit,
   // without affecting the stored twist command
-  TwistStamped command = *last_command_msg;
+  TwistStamped command = *last_command_msg_;
   double & linear_command = command.twist.linear.x;
   double & angular_command = command.twist.angular.z;
 
@@ -325,12 +326,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
         limited_velocity_publisher_);
   }
 
-  const TwistStamped empty_twist;
-  received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>(empty_twist));
-
+  last_command_msg_ = std::make_shared<TwistStamped>();
+  received_velocity_msg_ptr_.set([this](std::shared_ptr<TwistStamped> & stored_value)
+                                 { stored_value = last_command_msg_; });
   // Fill last two commands with default constructed commands
-  previous_commands_.emplace(empty_twist);
-  previous_commands_.emplace(empty_twist);
+  previous_commands_.emplace(*last_command_msg_);
+  previous_commands_.emplace(*last_command_msg_);
 
   // initialize command subscriber
   velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
@@ -350,7 +351,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
           "time, this message will only be shown once");
         msg->header.stamp = get_node()->get_clock()->now();
       }
-      received_velocity_msg_ptr_.set(std::move(msg));
+      received_velocity_msg_ptr_.set([msg](std::shared_ptr<TwistStamped> & stored_value)
+                                     { stored_value = std::move(msg); });
     });
 
   // initialize odometry publisher and message
@@ -476,7 +478,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>());
   return controller_interface::CallbackReturn::SUCCESS;
 }
 

diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp
@@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
   std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
   {
     std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
-    received_velocity_msg_ptr_.get(ret);
+    received_velocity_msg_ptr_.get(
+      [&ret](const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg) { ret = msg; });
     return ret;
   }
 

diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package effort_controllers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-12-07)
+-------------------
+* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 <https://github.com/ros-controls/ros2_controllers/issues/1370>`_)
+* Update maintainers and add url tags (`#1363 <https://github.com/ros-controls/ros2_controllers/issues/1363>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
 4.16.0 (2024-11-08)
 -------------------
 

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.16.0</version>
+  <version>4.17.0</version>
   <description>Generic controller for forwarding commands.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package force_torque_sensor_broadcaster
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-12-07)
+-------------------
+* Use the .hpp headers from `realtime_tools` package (`#1406 <https://github.com/ros-controls/ros2_controllers/issues/1406>`_)
+* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 <https://github.com/ros-controls/ros2_controllers/issues/1370>`_)
+* Update maintainers and add url tags (`#1363 <https://github.com/ros-controls/ros2_controllers/issues/1363>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
 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>`_)

diff --git a/...r_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/...r_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp
@@ -28,7 +28,7 @@
 #include "force_torque_sensor_broadcaster_parameters.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
 
 namespace force_torque_sensor_broadcaster

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.16.0</version>
+  <version>4.17.0</version>
   <description>Controller to publish state of force-torque sensors.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package forward_command_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.17.0 (2024-12-07)
+-------------------
+* Use the .hpp headers from `realtime_tools` package (`#1406 <https://github.com/ros-controls/ros2_controllers/issues/1406>`_)
+* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 <https://github.com/ros-controls/ros2_controllers/issues/1370>`_)
+* Update maintainers and add url tags (`#1363 <https://github.com/ros-controls/ros2_controllers/issues/1363>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
 4.16.0 (2024-11-08)
 -------------------
 

diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp
@@ -23,7 +23,7 @@
 #include "forward_command_controller/visibility_control.h"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_buffer.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
 
 namespace forward_command_controller

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.16.0</version>
+  <version>4.17.0</version>
   <description>Generic controller for forwarding commands.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>