From 39ec556a7af37f49c93b48e0f467dacaa2ef884a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 10 Dec 2023 18:34:51 +0000 Subject: [PATCH 1/4] Invalidate empty trajectory messages --- .../src/joint_trajectory_controller.cpp | 6 +++ .../test/test_trajectory_controller.cpp | 42 ++++++++++++++++++- 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c6a5169855..98565b7c11 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( } } + 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 05010c562c..5356f00d9a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1197,6 +1197,11 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg.joint_names = {"bad_name"}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // empty message + traj_msg = good_traj_msg; + traj_msg.points.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // No position data traj_msg = good_traj_msg; traj_msg.points[0].positions.clear(); @@ -1233,8 +1238,41 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); } -/// With allow_integration_in_goal_trajectories parameter trajectory missing position or -/// velocities are accepted +/** + * @brief invalid_message_nonzero_vel Test invalid velocity at trajectory end + */ +TEST_P(TrajectoryControllerTestParameterized, invalid_message_nonzero_vel) +{ + rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + // Nonzero velocity at trajectory end! + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // empty message (no throw!) + traj_msg = good_traj_msg; + traj_msg.points.clear(); + ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg)); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes + * + * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or + * velocities are accepted + */ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) { rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); From 2a74722574cc9ebec30fb8e44372da65d320d025 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 11 Dec 2023 06:33:12 +0000 Subject: [PATCH 2/4] Rename variable --- .../test/test_trajectory_controller.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 5356f00d9a..f5a7903007 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1247,24 +1247,22 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message_nonzero_vel) rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters}); - trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - - good_traj_msg.joint_names = joint_names_; - good_traj_msg.header.stamp = rclcpp::Time(0); - good_traj_msg.points.resize(1); - good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); - good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); - good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - // Nonzero velocity at trajectory end! - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + trajectory_msgs::msg::JointTrajectory traj_msg; + traj_msg.joint_names = joint_names_; + traj_msg.header.stamp = rclcpp::Time(0); // empty message (no throw!) - traj_msg = good_traj_msg; - traj_msg.points.clear(); ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg)); EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Nonzero velocity at trajectory end! + traj_msg.points.resize(1); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(1); + traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + traj_msg.points[0].velocities.resize(1); + traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); } /** From df699c0c4b94b96ad135c59ae0d02912adf7d74b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 20 Dec 2023 20:03:22 +0100 Subject: [PATCH 3/4] Update joint_trajectory_controller/test/test_trajectory_controller.cpp --- .../test/test_trajectory_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f5a7903007..f9403fdac3 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1239,9 +1239,9 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) } /** - * @brief invalid_message_nonzero_vel Test invalid velocity at trajectory end + * @brief Test invalid velocity at trajectory end with parameter set to false */ -TEST_P(TrajectoryControllerTestParameterized, invalid_message_nonzero_vel) +TEST_P(TrajectoryControllerTestParameterized, expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) { rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); rclcpp::executors::SingleThreadedExecutor executor; From fba33ddcbd693fef2f3fbbc91f8f981614339423 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Dec 2023 11:38:19 +0000 Subject: [PATCH 4/4] FIx format --- .../test/test_trajectory_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f9403fdac3..1aba6c941a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1241,7 +1241,9 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) /** * @brief Test invalid velocity at trajectory end with parameter set to false */ -TEST_P(TrajectoryControllerTestParameterized, expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) +TEST_P( + TrajectoryControllerTestParameterized, + expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) { rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); rclcpp::executors::SingleThreadedExecutor executor;