From 603f8d9f338dee6e4de5e8d4b72aa3ab459d566b Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 27 Mar 2024 17:04:58 -0600 Subject: [PATCH 1/4] fix JTC end time validation --- .../src/joint_trajectory_controller.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a4b63cb75..f818e16572 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1376,11 +1376,8 @@ bool JointTrajectoryController::validate_trajectory_msg( // 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( From 8952c2908ae2f0361840b791bbbb30eda3ae9040 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 27 Mar 2024 17:10:48 -0600 Subject: [PATCH 2/4] move trajectory check to before length validation --- .../src/joint_trajectory_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f818e16572..b250b25236 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1370,6 +1370,12 @@ 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, @@ -1402,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) From 7d6598fa48dd9d80a8aa321ae5db3ec019582f2c Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 11 Apr 2024 19:00:59 +0000 Subject: [PATCH 3/4] add test case --- .../test/test_trajectory_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f5e9cb7260..f7b9ff2c8a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1388,6 +1388,11 @@ 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)); } /** From 6959507f5878fc80c0f744bbe6837b961d0446c2 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 12 Apr 2024 19:11:37 +0000 Subject: [PATCH 4/4] added successful test case --- .../test/test_trajectory_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8c7293d69a..611c1d4c1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1389,6 +1389,12 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) 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)); } /**