From fe85da4e788fb846cece5601c74b5cfd0395924f Mon Sep 17 00:00:00 2001
From: Henry Moore <henry.moore@picknik.ai>
Date: Wed, 22 May 2024 11:18:34 -0600
Subject: [PATCH] JTC trajectory end time validation fix (#1090)

(cherry picked from commit b51e4c2436814124bd30a215d949c6062ceb1237)
---
 .../src/joint_trajectory_controller.cpp       | 19 ++++++++-----------
 .../test/test_trajectory_controller.cpp       | 11 +++++++++++
 2 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 561fd903ac..132a443ecb 100644
--- 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
index 92f0084e08..1959c92f4b 100644
--- 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));
 }
 
 /**