Add Debug

Author: urfeex

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -797,9 +797,9 @@ bool PassthroughTrajectoryController::check_goal_tolerance()
       return false;
     }
     if (std::abs(joint_pos.value() - setpoint) > joint_tol.position) {
-      // RCLCPP_ERROR(
-      // get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f, where tolerance is %f",
-      // joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos, joint_tol.position);
+      RCLCPP_ERROR(
+          get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f, where tolerance is %f",
+          joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos.value(), joint_tol.position);
       return false;
     }
 

ur_robot_driver/test/integration_test_passthrough_controller.py

@@ -197,7 +197,9 @@ def test_quintic_trajectory(self, tf_prefix):
             result = self._passthrough_forward_joint_trajectory.get_result(
                 goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
             )
-            self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+            self.assertEqual(
+                result.error_code, FollowJointTrajectory.Result.SUCCESSFUL, result.error_string
+            )
 
     def test_impossible_goal_tolerance_fails(self, tf_prefix):
         # Test impossible goal tolerance, should fail.
@@ -265,7 +267,9 @@ def test_impossible_goal_time_tolerance_fails(self, tf_prefix):
                 goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
             )
             self.assertEqual(
-                result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
+                result.error_code,
+                FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED,
+                result.error_string,
             )
         self.assertTrue(
             self._controller_manager_interface.switch_controller(