Merge remote-tracking branch 'origin/main' into trajectory_until

Author: urfeex

.pre-commit-config.yaml

@@ -39,7 +39,7 @@ repos:
         args: [--py36-plus]
 
   - repo: https://github.com/psf/black
-    rev: 25.1.0
+    rev: 25.9.0
     hooks:
       - id: black
         args: ["--line-length=100"]
@@ -87,7 +87,7 @@ repos:
         args: ["--linelength=120"]
 
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: 'v21.1.0'
+    rev: 'v21.1.2'
     hooks:
       - id: clang-format
 

ur/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package ur
 ^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.2.0 (2025-09-16)
+------------------
+
 4.1.0 (2025-07-29)
 ------------------
 

ur/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>ur</name>
-  <version>4.1.0</version>
+  <version>4.2.0</version>
   <description>Metapackage for universal robots</description>
   <maintainer email="[email protected]">Felix Exner</maintainer>
   <maintainer email="[email protected]">Rune Søe-Knudsen</maintainer>

ur_calibration/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package ur_calibration
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.2.0 (2025-09-16)
+------------------
+
 4.1.0 (2025-07-29)
 ------------------
 * Use hpp headers from geometry2 (`#1467 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1467>`_)

ur_calibration/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ur_calibration</name>
-  <version>4.1.0</version>
+  <version>4.2.0</version>
   <description>Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF</description>
 
   <maintainer email="[email protected]">Felix Exner</maintainer>

ur_controllers/CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package ur_controllers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.2.0 (2025-09-16)
+------------------
+* Realtime tools migration (`#1474 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1474>`_)
+  Use RealtimeThreadSafeBox instead of RealTimeBuffer.
+* Replace SJTC implementation with upstream (`#1485 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1485>`_)
+* Contributors: Felix Exner, URJala
+
 4.1.0 (2025-07-29)
 ------------------
 * ur_configuration_controller: use try_set on RTBox (`#1470 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1470>`_)

ur_controllers/include/ur_controllers/force_mode_controller.hpp

@@ -41,7 +41,7 @@
 #include <controller_interface/controller_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #if __has_include(<tf2_ros/buffer.hpp>)
 #include <tf2_ros/buffer.hpp>
@@ -98,7 +98,7 @@ struct ForceModeParameters
   std::array<double, 6> task_frame;
   std::array<double, 6> selection_vec;
   std::array<double, 6> limits;
-  geometry_msgs::msg::Wrench wrench;
+  geometry_msgs::msg::Wrench wrench{};
   double type;
   double damping_factor;
   double gain_scaling;
@@ -137,7 +137,7 @@ class ForceModeController : public controller_interface::ControllerInterface
   std::shared_ptr<force_mode_controller::ParamListener> param_listener_;
   force_mode_controller::Params params_;
 
-  realtime_tools::RealtimeBuffer<ForceModeParameters> force_mode_params_buffer_;
+  realtime_tools::RealtimeThreadSafeBox<ForceModeParameters> force_mode_params_buffer_;
   std::atomic<bool> force_mode_active_;
   std::atomic<bool> change_requested_;
   std::atomic<double> async_state_;

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -51,7 +51,7 @@
 #include <vector>
 
 #include <controller_interface/controller_interface.hpp>
-#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 #include <realtime_tools/realtime_server_goal_handle.hpp>
 #include <rclcpp_action/server.hpp>
 #include <rclcpp_action/create_server.hpp>
@@ -114,11 +114,11 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
   using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowJTrajAction>;
   using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
-  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeThreadSafeBox<RealtimeGoalHandlePtr>;
 
   RealtimeGoalHandleBuffer rt_active_goal_;         ///< Currently active action goal, if any.
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;  ///< Timer to frequently check on the running goal
-  realtime_tools::RealtimeBuffer<std::unordered_map<std::string, size_t>> joint_trajectory_mapping_;
+  realtime_tools::RealtimeThreadSafeBox<std::unordered_map<std::string, size_t>> joint_trajectory_mapping_;
 
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
 
@@ -147,7 +147,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   void goal_accepted_callback(
       std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle);
 
-  realtime_tools::RealtimeBuffer<std::vector<std::string>> joint_names_;
+  realtime_tools::RealtimeThreadSafeBox<std::vector<std::string>> joint_names_;
   std::vector<std::string> state_interface_types_;
 
   std::vector<std::string> joint_state_interface_names_;
@@ -159,11 +159,13 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   bool check_goal_positions(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
   bool check_goal_velocities(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
   bool check_goal_accelerations(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  std::optional<RealtimeGoalHandlePtr> get_rt_goal_from_non_rt();
+  bool set_rt_goal_from_non_rt(RealtimeGoalHandlePtr& rt_goal);
 
   trajectory_msgs::msg::JointTrajectory active_joint_traj_;
   // std::vector<control_msgs::msg::JointTolerance> path_tolerance_;
-  realtime_tools::RealtimeBuffer<std::vector<control_msgs::msg::JointTolerance>> goal_tolerance_;
-  realtime_tools::RealtimeBuffer<rclcpp::Duration> goal_time_tolerance_{ rclcpp::Duration(0, 0) };
+  realtime_tools::RealtimeThreadSafeBox<std::vector<control_msgs::msg::JointTolerance>> goal_tolerance_;
+  realtime_tools::RealtimeThreadSafeBox<rclcpp::Duration> goal_time_tolerance_{ rclcpp::Duration(0, 0) };
 
   std::atomic<size_t> current_index_;
   std::atomic<bool> trajectory_active_;

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -53,8 +53,9 @@
 #include <rclcpp_action/server_goal_handle.hpp>
 #include <rclcpp/duration.hpp>
 
-#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 #include <realtime_tools/realtime_server_goal_handle.hpp>
+#include <realtime_tools/lock_free_queue.hpp>
 
 #include <ur_msgs/action/tool_contact.hpp>
 #include "ur_controllers/tool_contact_controller_parameters.hpp"
@@ -86,11 +87,13 @@ class ToolContactController : public controller_interface::ControllerInterface
 private:
   using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<ur_msgs::action::ToolContact>;
   using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
-  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeThreadSafeBox<RealtimeGoalHandlePtr>;
 
   RealtimeGoalHandleBuffer rt_active_goal_;                     ///< Currently active action goal, if any.
   ur_msgs::action::ToolContact::Feedback::SharedPtr feedback_;  ///< preallocated feedback
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;              ///< Timer to frequently check on the running goal
+  std::optional<RealtimeGoalHandlePtr> get_rt_goal_from_non_rt();
+  bool set_rt_goal_from_non_rt(const RealtimeGoalHandlePtr& goal_handle);
 
   // non-rt function that will be called with action_monitor_period to monitor the rt action
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration::from_seconds(0.05);

ur_controllers/include/ur_controllers/ur_configuration_controller.hpp

@@ -44,7 +44,7 @@
 #include <memory>
 
 #include <controller_interface/controller_interface.hpp>
-#include <realtime_tools/realtime_box.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 
 #include "ur_msgs/srv/get_robot_software_version.hpp"
 #include "ur_controllers/ur_configuration_controller_parameters.hpp"
@@ -85,7 +85,7 @@ class URConfigurationController : public controller_interface::ControllerInterfa
   CallbackReturn on_init() override;
 
 private:
-  realtime_tools::RealtimeBox<std::shared_ptr<VersionInformation>> robot_software_version_{
+  realtime_tools::RealtimeThreadSafeBox<std::shared_ptr<VersionInformation>> robot_software_version_{
     std::make_shared<VersionInformation>()
   };
   std::atomic<bool> robot_software_version_set_{ false };