[JTC] Fix time sources and wrong checks in tests (#686)

* Fix time sources and wrong checks in tests
* Use time from update-method instead of node clock
* Readd test of last command in test_goal_tolerances_fail

---------

Co-authored-by: Bence Magyar <[email protected]>

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -231,7 +231,7 @@ controller_interface::return_type JointTrajectoryController::update(
             const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time();
             const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;
 
-            time_difference = get_node()->now().seconds() - traj_end.seconds();
+            time_difference = time.seconds() - traj_end.seconds();
 
             if (time_difference > default_tolerances_.goal_time_tolerance)
             {

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -66,11 +66,14 @@ class TestTrajectoryActions : public TrajectoryControllerTest
     goal_options_.feedback_callback = nullptr;
   }
 
-  void SetUpExecutor(const std::vector<rclcpp::Parameter> & parameters = {})
+  void SetUpExecutor(
+    const std::vector<rclcpp::Parameter> & parameters = {},
+    bool separate_cmd_and_state_values = false)
   {
     setup_executor_ = true;
 
-    SetUpAndActivateTrajectoryController(executor_, true, parameters);
+    SetUpAndActivateTrajectoryController(
+      executor_, true, parameters, separate_cmd_and_state_values);
 
     SetUpActionClient();
 
@@ -472,15 +475,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail)
     rclcpp::Parameter("constraints.joint3.goal", goal_tol),
     rclcpp::Parameter("constraints.goal_time", goal_time)};
 
-  SetUpExecutor(params);
+  // separate command from states -> goal won't never be reached
+  bool separate_cmd_and_state_values = true;
+  SetUpExecutor(params, separate_cmd_and_state_values);
   SetUpControllerHardware();
 
-  const double init_pos1 = joint_pos_[0];
-  const double init_pos2 = joint_pos_[1];
-  const double init_pos3 = joint_pos_[2];
-
   std::shared_future<typename GoalHandle::SharedPtr> gh_future;
-  // send goal
+  // send goal; one point only -> command is directly set to reach this goal (no interpolation)
   {
     std::vector<JointTrajectoryPoint> points;
     JointTrajectoryPoint point;
@@ -502,14 +503,14 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail)
     control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED,
     common_action_result_code_);
 
-  // run an update, it should be holding
+  // run an update, it should be holding the last received goal
   updateController(rclcpp::Duration::from_seconds(0.01));
 
   if (traj_controller_->has_position_command_interface())
   {
-    EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD);
-    EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD);
-    EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD);
+    EXPECT_NEAR(4.0, joint_pos_[0], COMMON_THRESHOLD);
+    EXPECT_NEAR(5.0, joint_pos_[1], COMMON_THRESHOLD);
+    EXPECT_NEAR(6.0, joint_pos_[2], COMMON_THRESHOLD);
   }
 }
 

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -1092,7 +1092,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
 
   RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past");
   //  New trajectory will end before current time
-  rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100);
+  rclcpp::Time new_traj_start =
+    rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100);
   expected_actual.positions = {points_old[1].begin(), points_old[1].end()};
   expected_desired = expected_actual;
   std::cout << "Sending old trajectory" << std::endl;
@@ -1108,23 +1109,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory
 
   std::vector<std::vector<double>> points_old{{{2., 3., 4.}, {4., 5., 6.}}};
   std::vector<std::vector<double>> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}};
-
+  trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired;
   const auto delay = std::chrono::milliseconds(500);
   builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)};
+
+  // send points_old and wait to reach first point
   publish(time_from_start, points_old, rclcpp::Time());
-  trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired;
   expected_actual.positions = {points_old[0].begin(), points_old[0].end()};
   expected_desired = expected_actual;
   //  Check that we reached end of points_old[0]trajectory
   waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
 
+  // send points_new before the old trajectory is finished
   RCLCPP_INFO(
     traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past");
   //  New trajectory first point is in the past, second is in the future
-  rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100);
-  expected_actual.positions = {points_new[1].begin(), points_new[1].end()};
-  expected_desired = expected_actual;
+  rclcpp::Time new_traj_start =
+    rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100);
   publish(time_from_start, points_new, new_traj_start);
+  // it should not have accepted the new goal but finish the old one
+  expected_actual.positions = {points_old[1].begin(), points_old[1].end()};
+  expected_desired.positions = {points_old[1].begin(), points_old[1].end()};
   waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
 }
 
@@ -1163,7 +1168,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
   // Send partial trajectory starting after full trajecotry is complete
   RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future");
   publish(
-    points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities);
+    points_delay, partial_traj, rclcpp::Clock(RCL_STEADY_TIME).now() + delay * 2, {},
+    partial_traj_velocities);
   // Wait until the end start and end of partial traj
 
   expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]};

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -366,13 +366,7 @@ class TrajectoryControllerTest : public ::testing::Test
     const auto end_time = start_time + wait_time;
     while (clock.now() < end_time)
     {
-      // TODO(christophfroehlich): use the node's clock here for internal comparison
-      // if using RCL_STEADY_TIME ->
-      //   C++ exception with description
-      //   "can't compare times with different time sources" thrown in the test body.
-      // traj_controller_->update(clock.now(), clock.now() - start_time);
-      // maybe we can set the node clock to use RCL_STEADY_TIME too?
-      traj_controller_->update(node_->get_clock()->now(), clock.now() - start_time);
+      traj_controller_->update(clock.now(), clock.now() - start_time);
     }
   }