diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a4b63cb75..b250b25236 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1370,17 +1370,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(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( @@ -1405,12 +1408,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 0b00bd5380..611c1d4c1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1384,6 +1384,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)); } /**