Updated scaled JTC to latest JTC updates (#1067)

Author: Felix Exner (fexner)

ur_controllers/src/scaled_joint_trajectory_controller.cpp

@@ -86,10 +86,11 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
   if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
     return controller_interface::return_type::OK;
   }
+  auto logger = this->get_node()->get_logger();
   // update dynamic parameters
   if (param_listener_->is_old(params_)) {
     params_ = param_listener_->get_params();
-    default_tolerances_ = get_segment_tolerances(params_);
+    default_tolerances_ = get_segment_tolerances(logger, params_);
   }
 
   auto compute_error_for_joint = [&](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current,
@@ -138,6 +139,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
 
   // currently carrying out a trajectory
   if (has_active_trajectory()) {
+    // Main Speed scaling difference...
     // Adjust time with scaling factor
     TimeData time_data;
     time_data.time = time;
@@ -153,9 +155,10 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
     if (!traj_external_point_ptr_->is_sampled_already()) {
       first_sample = true;
       if (params_.open_loop_control) {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_);
+        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_,
+                                                                  joints_angle_wraparound_);
       } else {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_);
+        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_, joints_angle_wraparound_);
       }
     }
 
@@ -178,12 +181,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;
       const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end();
+      auto active_tol = active_tolerances_.readFromRT();
 
       // 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_) {
-        RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout");
+        RCLCPP_WARN(logger, "Aborted due to command timeout");
 
         traj_msg_external_point_ptr_.reset();
         traj_msg_external_point_ptr_.initRT(set_hold_position());
@@ -195,21 +199,25 @@ 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
-        if ((before_last_point || first_sample) &&
-            !check_state_tolerance_per_joint(state_error_, index, default_tolerances_.state_tolerance[index], false) &&
-            *(rt_is_holding_.readFromRT()) == false) {
+        // print output per default, goal will be aborted afterwards
+        if ((before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false &&
+            !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 &&
-            !check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index],
-                                             false) &&
-            *(rt_is_holding_.readFromRT()) == false) {
+        if (!before_last_point && *(rt_is_holding_.readFromRT()) == false &&
+            !check_state_tolerance_per_joint(state_error_, index, active_tol->goal_state_tolerance[index],
+                                             false /* show_errors */)) {
           outside_goal_tolerance = true;
 
-          if (default_tolerances_.goal_time_tolerance != 0.0) {
-            if (time_difference > default_tolerances_.goal_time_tolerance) {
+          if (active_tol->goal_time_tolerance != 0.0) {
+            // if we exceed goal_time_tolerance set it to aborted
+            if (time_difference > active_tol->goal_time_tolerance) {
               within_goal_time = false;
+              // print once, goal will be aborted afterwards
+              check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index],
+                                              true /* show_errors */);
             }
           }
         }
@@ -263,54 +271,60 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
         if (tolerance_violated_while_moving) {
           auto result = std::make_shared<FollowJTrajAction::Result>();
           result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
+          result->set__error_string("Aborted due to path tolerance violation");
           active_goal->setAborted(result);
           // 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);
 
-          RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation");
+          RCLCPP_WARN(logger, "Aborted due to state tolerance violation");
 
           traj_msg_external_point_ptr_.reset();
           traj_msg_external_point_ptr_.initRT(set_hold_position());
         } else if (!before_last_point) {
+          // check goal tolerance
           if (!outside_goal_tolerance) {
-            auto res = std::make_shared<FollowJTrajAction::Result>();
-            res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
-            active_goal->setSucceeded(res);
+            auto result = std::make_shared<FollowJTrajAction::Result>();
+            result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
+            result->set__error_string("Goal successfully reached!");
+            active_goal->setSucceeded(result);
             // 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);
 
-            RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");
+            RCLCPP_INFO(logger, "Goal reached, success!");
 
             traj_msg_external_point_ptr_.reset();
             traj_msg_external_point_ptr_.initRT(set_success_trajectory_point());
           } else if (!within_goal_time) {
+            const std::string error_string =
+                "Aborted due to goal_time_tolerance exceeding by " + std::to_string(time_difference) + " seconds";
+
             auto result = std::make_shared<FollowJTrajAction::Result>();
             result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
+            result->set__error_string(error_string);
             active_goal->setAborted(result);
             // 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);
 
-            RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds",
-                        time_difference);
+            RCLCPP_WARN(logger, error_string.c_str());
 
             traj_msg_external_point_ptr_.reset();
             traj_msg_external_point_ptr_.initRT(set_hold_position());
           }
         }
       } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) {
         // we need to ensure that there is no pending goal -> we get a race condition otherwise
-        RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation");
+        RCLCPP_ERROR(logger, "Holding position due to state tolerance violation");
 
         traj_msg_external_point_ptr_.reset();
         traj_msg_external_point_ptr_.initRT(set_hold_position());
       } else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) {
-        RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position...");
+        RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position...");
 
         traj_msg_external_point_ptr_.reset();
         traj_msg_external_point_ptr_.initRT(set_hold_position());