add patch

Signed-off-by: wep21 <[email protected]>

Author: wep21

patch/ros-jazzy-ur-controllers.patch

@@ -0,0 +1,82 @@
+diff --git a/src/scaled_joint_trajectory_controller.cpp b/src/scaled_joint_trajectory_controller.cpp
+index 4a6320fa..fd039fb9 100644
+--- a/src/scaled_joint_trajectory_controller.cpp
++++ b/src/scaled_joint_trajectory_controller.cpp
+@@ -115,7 +115,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
+   const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg();
+   auto new_external_msg = new_trajectory_msg_.readFromRT();
+   // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
+-  if (current_trajectory_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) {
++  if (current_trajectory_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false) {
+     fill_partial_goal(*new_external_msg);
+     sort_to_local_joint_order(*new_external_msg);
+     // TODO(denis): Add here integration of position and velocity
+@@ -175,8 +175,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
+ 
+       // have we reached the end, are not holding position, and is a timeout configured?
+       // Check independently of other tolerances
+-      if (!before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 &&
+-          time_difference > cmd_timeout_) {
++      if (!before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) {
+         RCLCPP_WARN(logger, "Aborted due to command timeout");
+ 
+         new_trajectory_msg_.reset();
+@@ -190,13 +189,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
+         // Always check the state tolerance on the first sample in case the first sample
+         // is the last point
+         // print output per default, goal will be aborted afterwards
+-        if ((before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false &&
++        if ((before_last_point || first_sample) && !rt_is_holding_ &&
+             !check_state_tolerance_per_joint(state_error_, index, active_tol->state_tolerance[index],
+                                              true /* show_errors */)) {
+           tolerance_violated_while_moving = true;
+         }
+         // past the final point, check that we end up inside goal tolerance
+-        if (!before_last_point && *(rt_is_holding_.readFromRT()) == false &&
++        if (!before_last_point && !rt_is_holding_ &&
+             !check_state_tolerance_per_joint(state_error_, index, active_tol->goal_state_tolerance[index],
+                                              false /* show_errors */)) {
+           outside_goal_tolerance = true;
+@@ -277,7 +276,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
+           // TODO(matthew-reynolds): Need a lock-free write here
+           // See https://github.com/ros-controls/ros2_controllers/issues/168
+           rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+-          rt_has_pending_goal_.writeFromNonRT(false);
++          rt_has_pending_goal_ = false;
+ 
+           RCLCPP_WARN(logger, "Aborted due to state tolerance violation");
+ 
+@@ -293,7 +292,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
+             // TODO(matthew-reynolds): Need a lock-free write here
+             // See https://github.com/ros-controls/ros2_controllers/issues/168
+             rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+-            rt_has_pending_goal_.writeFromNonRT(false);
++            rt_has_pending_goal_ = false;
+ 
+             RCLCPP_INFO(logger, "Goal reached, success!");
+ 
+@@ -310,7 +309,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
+             // TODO(matthew-reynolds): Need a lock-free write here
+             // See https://github.com/ros-controls/ros2_controllers/issues/168
+             rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+-            rt_has_pending_goal_.writeFromNonRT(false);
++            rt_has_pending_goal_ = false;
+ 
+             RCLCPP_WARN(logger, "%s", error_string.c_str());
+ 
+@@ -318,13 +317,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
+             new_trajectory_msg_.initRT(set_hold_position());
+           }
+         }
+-      } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) {
++      } else if (tolerance_violated_while_moving && !rt_has_pending_goal_) {
+         // we need to ensure that there is no pending goal -> we get a race condition otherwise
+         RCLCPP_ERROR(logger, "Holding position due to state tolerance violation");
+ 
+         new_trajectory_msg_.reset();
+         new_trajectory_msg_.initRT(set_hold_position());
+-      } else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) {
++      } else if (!before_last_point && !within_goal_time && !rt_has_pending_goal_) {
+         RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position...");
+ 
+         new_trajectory_msg_.reset();