JTC trajectory end time validation fix (#1090) (#1141)

(cherry picked from commit b51e4c2) Co-authored-by: Henry Moore <henry.moore@picknik.ai>
ros-controls · May 22, 2024 · cb40e0b · cb40e0b
1 parent 12a2ef2
commit cb40e0b
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 11 deletions.
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -1361,17 +1361,20 @@ bool JointTrajectoryController::validate_trajectory_msg(
     return false;
   }
 
+  if (trajectory.points.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received.");
+    return false;
+  }
+
   const auto trajectory_start_time = static_cast<rclcpp::Time>(trajectory.header.stamp);
   // If the starting time it set to 0.0, it means the controller should start it now.
   // Otherwise we check if the trajectory ends before the current time,
   // in which case it can be ignored.
   if (trajectory_start_time.seconds() != 0.0)
   {
-    auto trajectory_end_time = trajectory_start_time;
-    for (const auto & p : trajectory.points)
-    {
-      trajectory_end_time += p.time_from_start;
-    }
+    auto const trajectory_end_time =
+      trajectory_start_time + trajectory.points.back().time_from_start;
     if (trajectory_end_time < get_node()->now())
     {
       RCLCPP_ERROR(
@@ -1396,12 +1399,6 @@ bool JointTrajectoryController::validate_trajectory_msg(
     }
   }
 
-  if (trajectory.points.empty())
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received.");
-    return false;
-  }
-
   if (!params_.allow_nonzero_velocity_at_trajectory_end)
   {
     for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)

diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -1398,6 +1398,17 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message)
   traj_msg = good_traj_msg;
   traj_msg.points.push_back(traj_msg.points.front());
   EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
+
+  // End time in the past
+  traj_msg = good_traj_msg;
+  traj_msg.header.stamp = rclcpp::Time(1);
+  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
+
+  // End time in the future
+  traj_msg = good_traj_msg;
+  traj_msg.header.stamp = traj_controller_->get_node()->now();
+  traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(10);
+  EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg));
 }
 
 /**