From 164aab92a69f5c8a2ef19465411c0609d2a85d52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 20 Jun 2023 22:38:20 +0200 Subject: [PATCH 01/12] [JTC] Extend tests (#612) --- .../test/test_trajectory_actions.cpp | 153 +- .../test/test_trajectory_controller.cpp | 1743 ++++++++--------- .../test/test_trajectory_controller_utils.hpp | 10 +- 3 files changed, 958 insertions(+), 948 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index ef0a11d961..fc2dd42314 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -197,7 +197,24 @@ class TestTrajectoryActions : public TrajectoryControllerTest } }; -TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class TestTrajectoryActionsTestParameterized +: public TestTrajectoryActions, + public ::testing::WithParamInterface< + std::tuple, std::vector>> +{ +public: + virtual void SetUp() + { + TestTrajectoryActions::SetUp(); + command_interface_types_ = std::get<0>(GetParam()); + state_interface_types_ = std::get<1>(GetParam()); + } + + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } +}; + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { SetUpExecutor(); SetUpControllerHardware(); @@ -223,12 +240,15 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ(1.0, joint_pos_[0]); - EXPECT_EQ(2.0, joint_pos_[1]); - EXPECT_EQ(3.0, joint_pos_[2]); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_EQ(1.0, joint_pos_[0]); + EXPECT_EQ(2.0, joint_pos_[1]); + EXPECT_EQ(3.0, joint_pos_[2]); + } } -TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { SetUpExecutor(); SetUpControllerHardware(); @@ -270,11 +290,18 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + } } +/** + * Makes sense with position command interface only, + * because no integration to position state interface is implemented +*/ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { // set tolerance parameters @@ -308,11 +335,18 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + } } +/** + * Makes sense with position command interface only, + * because no integration to position state interface is implemented +*/ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { // set tolerance parameters @@ -363,12 +397,15 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_state_tolerances_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters const double state_tol = 0.0001; @@ -415,12 +452,15 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) { // set joint tolerance parameters const double goal_tol = 0.1; @@ -465,12 +505,15 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) { // set joint tolerance parameters const double state_tol = 0.0001; @@ -512,12 +555,15 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_cancel_hold_position) +TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) { SetUpExecutor(); SetUpControllerHardware(); @@ -561,7 +607,54 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_EQ(prev_pos1, joint_pos_[0]); - EXPECT_EQ(prev_pos2, joint_pos_[1]); - EXPECT_EQ(prev_pos3, joint_pos_[2]); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_EQ(prev_pos1, joint_pos_[0]); + EXPECT_EQ(prev_pos2, joint_pos_[1]); + EXPECT_EQ(prev_pos3, joint_pos_[2]); + } } + +// position controllers +INSTANTIATE_TEST_SUITE_P( + PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple(std::vector({"position"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllersAction, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only effort controller +INSTANTIATE_TEST_SUITE_P( + OnlyEffortTrajectoryControllers, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index b1188eede7..714d3c8d49 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -199,119 +199,6 @@ TEST_P(TrajectoryControllerTestParameterized, activate) executor.cancel(); } -// TEST_F(TestTrajectoryController, activation) { -// auto traj_controller = std::make_shared( -// joint_names_, op_mode_); -// auto ret = traj_controller->init(test_robot_, controller_name_); -// if (ret != controller_interface::return_type::OK) { -// FAIL(); -// } -// -// auto traj_node = traj_controller->get_node(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// -// auto state = traj_controller_->configure(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// -// // wait for the subscriber and publisher to completely setup -// std::this_thread::sleep_for(std::chrono::seconds(2)); -// -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points {{{3.3, 4.4, 5.5}}}; -// publish(time_from_start, points, rclcpp::Time()); -// // wait for msg is be published to the system -// std::this_thread::sleep_for(std::chrono::milliseconds(1000)); -// executor.spin_once(); -// -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // change in hw position -// EXPECT_EQ(3.3, joint_pos_[0]); -// EXPECT_EQ(4.4, joint_pos_[1]); -// EXPECT_EQ(5.5, joint_pos_[2]); -// -// executor.cancel(); -// } - -// TEST_F(TestTrajectoryController, reactivation) { -// auto traj_controller = std::make_shared( -// joint_names_, op_mode_); -// auto ret = traj_controller->init(test_robot_, controller_name_); -// if (ret != controller_interface::return_type::OK) { -// FAIL(); -// } -// -// auto traj_node = traj_controller->get_node(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// -// auto state = traj_controller_->configure(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// -// // wait for the subscriber and publisher to completely setup -// std::this_thread::sleep_for(std::chrono::seconds(2)); -// -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// // *INDENT-OFF* -// std::vector> points { -// {{3.3, 4.4, 5.5}}, -// {{7.7, 8.8, 9.9}}, -// {{10.10, 11.11, 12.12}} -// }; -// // *INDENT-ON* -// publish(time_from_start, points, rclcpp::Time()); -// // wait for msg is be published to the system -// std::this_thread::sleep_for(std::chrono::milliseconds(500)); -// executor.spin_once(); -// -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // deactivated -// // wait so controller process the second point when deactivated -// std::this_thread::sleep_for(std::chrono::milliseconds(500)); -// state = traj_controller_->deactivate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// resource_manager_->read(); -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // no change in hw position -// EXPECT_EQ(3.3, joint_pos_[0]); -// EXPECT_EQ(4.4, joint_pos_[1]); -// EXPECT_EQ(5.5, joint_pos_[2]); -// -// // reactivated -// // wait so controller process the third point when reactivated -// std::this_thread::sleep_for(std::chrono::milliseconds(3000)); -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// resource_manager_->read(); -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // change in hw position to 3rd point -// EXPECT_EQ(10.10, joint_pos_[0]); -// EXPECT_EQ(11.11, joint_pos_[1]); -// EXPECT_EQ(12.12, joint_pos_[2]); -// -// executor.cancel(); -// } - TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; @@ -338,7 +225,10 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + // update for 0.25 seconds + // TODO(anyone): should the controller even allow calling update() when it is not active? + // update loop receives a new msg and updates accordingly updateController(rclcpp::Duration::from_seconds(0.25)); // should be home pose again @@ -428,25 +318,28 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_LE(0.0, joint_eff_[2]); } - // cleanup - state = traj_controller_->get_node()->cleanup(); - ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + // reactivated + // wait so controller process the third point when reactivated + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + // TODO(anyone) test copied from ROS 1: it fails now! + // should the old trajectory really be processed after reactivation? +#if 0 + ActivateTrajectoryController(); + state = traj_controller_->get_state(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - // TODO(anyone): should the controller even allow calling update() when it is not active? - // update loop receives a new msg and updates accordingly - traj_controller_->update( - rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); + updateController(rclcpp::Duration::from_seconds(0.5)); + // traj_controller_->update( + // rclcpp::Time(static_cast(0.75 * 1e9)), rclcpp::Duration::from_seconds(0.01)); + // change in hw position to 3rd point if (traj_controller_->has_position_command_interface()) { - // otherwise this won't be updated - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + EXPECT_NEAR(10.10, joint_pos_[0], allowed_delta); + EXPECT_NEAR(11.11, joint_pos_[1], allowed_delta); + EXPECT_NEAR(12.12, joint_pos_[2], allowed_delta); } - - // state = traj_controller_->get_node()->configure(); - // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); +#endif executor.cancel(); } @@ -723,14 +616,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) if (traj_controller_->has_velocity_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - EXPECT_GT(0.0, joint_vel_[2]); // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], @@ -739,6 +627,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); // is error of positions[2] normalized? + EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], k_p * allowed_delta); @@ -746,819 +635,839 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) else { // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * allowed_delta); + // is error of positions[2] normalized? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * allowed_delta); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } } executor.cancel(); } -// /** -// * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from -// * internal controller order -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// { -// trajectory_msgs::msg::JointTrajectory traj_msg; -// const std::vector jumbled_joint_names{ -// joint_names_[1], joint_names_[2], joint_names_[0]}; -// traj_msg.joint_names = jumbled_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(3); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 3.0; -// traj_msg.points[0].positions[2] = 1.0; - -// if (traj_controller_->has_velocity_command_interface()) -// { -// traj_msg.points[0].velocities.resize(3); -// traj_msg.points[0].velocities[0] = -0.1; -// traj_msg.points[0].velocities[1] = -0.1; -// traj_msg.points[0].velocities[2] = -0.1; -// } - -// if (traj_controller_->has_effort_command_interface()) -// { -// traj_msg.points[0].effort.resize(3); -// traj_msg.points[0].effort[0] = -0.1; -// traj_msg.points[0].effort[1] = -0.1; -// traj_msg.points[0].effort[2] = -0.1; -// } - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.25 seconds -// // TODO(destogl): Make this time a bit shorter to increase stability on the CI? -// // Currently COMMON_THRESHOLD is adjusted. -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// if (traj_controller_->has_position_command_interface()) -// { -// EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); -// EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); -// } +/** + * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from + * internal controller order + */ +TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor); + { + trajectory_msgs::msg::JointTrajectory traj_msg; + const std::vector jumbled_joint_names{ + joint_names_[1], joint_names_[2], joint_names_[0]}; + traj_msg.joint_names = jumbled_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(3); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 3.0; + traj_msg.points[0].positions[2] = 1.0; + + if (traj_controller_->has_velocity_command_interface()) + { + traj_msg.points[0].velocities.resize(3); + traj_msg.points[0].velocities[0] = -0.1; + traj_msg.points[0].velocities[1] = -0.1; + traj_msg.points[0].velocities[2] = -0.1; + } -// if (traj_controller_->has_velocity_command_interface()) -// { -// EXPECT_GT(0.0, joint_vel_[0]); -// EXPECT_GT(0.0, joint_vel_[1]); -// EXPECT_GT(0.0, joint_vel_[2]); -// } + if (traj_controller_->has_effort_command_interface()) + { + traj_msg.points[0].effort.resize(3); + traj_msg.points[0].effort[0] = -0.1; + traj_msg.points[0].effort[1] = -0.1; + traj_msg.points[0].effort[2] = -0.1; + } -// if (traj_controller_->has_effort_command_interface()) -// { -// EXPECT_GT(0.0, joint_eff_[0]); -// EXPECT_GT(0.0, joint_eff_[1]); -// EXPECT_GT(0.0, joint_eff_[2]); -// } -// } + trajectory_publisher_->publish(traj_msg); + } -// /** -// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled -// * joints -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + traj_controller_->wait_for_trajectory(executor); + // update for 0.25 seconds + // TODO(destogl): Make this time a bit shorter to increase stability on the CI? + // Currently COMMON_THRESHOLD is adjusted. + updateController(rclcpp::Duration::from_seconds(0.25)); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + } -// const double initial_joint1_cmd = joint_pos_[0]; -// const double initial_joint2_cmd = joint_pos_[1]; -// const double initial_joint3_cmd = joint_pos_[2]; -// trajectory_msgs::msg::JointTrajectory traj_msg; + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_GT(0.0, joint_vel_[0]); + EXPECT_GT(0.0, joint_vel_[1]); + EXPECT_GT(0.0, joint_vel_[2]); + } -// { -// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; -// traj_msg.joint_names = partial_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(2); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 1.0; -// traj_msg.points[0].velocities.resize(2); -// traj_msg.points[0].velocities[0] = -// copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); -// traj_msg.points[0].velocities[1] = -// copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.5 seconds -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// double threshold = 0.001; - -// if (traj_controller_->has_position_command_interface()) -// { -// EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); -// EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); -// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) -// << "Joint 3 command should be current position"; -// } - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != -// command_interface_types_.end()) -// { -// // estimate the sign of the velocity -// // joint rotates forward -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); -// EXPECT_NEAR(0.0, joint_vel_[2], threshold) -// << "Joint 3 velocity should be 0.0 since it's not in the goal"; -// } - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != -// command_interface_types_.end()) -// { -// // estimate the sign of the velocity -// // joint rotates forward -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); -// EXPECT_NEAR(0.0, joint_eff_[2], threshold) -// << "Joint 3 effort should be 0.0 since it's not in the goal"; -// } -// // TODO(anyone): add here ckecks for acceleration commands - -// executor.cancel(); -// } + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } + // TODO(anyone): add here checks for acceleration commands +} -// /** -// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled -// * joints without allow_partial_joints_goal -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); +/** + * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled + * joints + */ +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); -// const double initial_joint1_cmd = joint_pos_[0]; -// const double initial_joint2_cmd = joint_pos_[1]; -// const double initial_joint3_cmd = joint_pos_[2]; -// const double initial_joint_vel = 0.0; -// const double initial_joint_acc = 0.0; -// trajectory_msgs::msg::JointTrajectory traj_msg; + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; + const double initial_joint3_cmd = joint_pos_[2]; + trajectory_msgs::msg::JointTrajectory traj_msg; -// { -// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; -// traj_msg.joint_names = partial_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(2); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 1.0; -// traj_msg.points[0].velocities.resize(2); -// traj_msg.points[0].velocities[0] = 2.0; -// traj_msg.points[0].velocities[1] = 1.0; - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.5 seconds -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// double threshold = 0.001; -// EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) -// << "All joints command should be current position because goal was rejected"; -// EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) -// << "All joints command should be current position because goal was rejected"; -// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) -// << "All joints command should be current position because goal was rejected"; - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != -// command_interface_types_.end()) -// { -// EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// } - -// if ( -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != -// command_interface_types_.end()) -// { -// EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// } - -// executor.cancel(); -// } + { + std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + traj_msg.joint_names = partial_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(2); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 1.0; + traj_msg.points[0].velocities.resize(2); + traj_msg.points[0].velocities[0] = + copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); + traj_msg.points[0].velocities[1] = + copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + + trajectory_publisher_->publish(traj_msg); + } -// /** -// * @brief invalid_message Test mismatched joint and reference vector sizes -// */ -// TEST_P(TrajectoryControllerTestParameterized, invalid_message) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); -// rclcpp::Parameter allow_integration_parameters( -// "allow_integration_in_goal_trajectories", false); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController( -// true, {partial_joints_parameters, allow_integration_parameters}, &executor); - -// 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}; -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - -// // Incompatible joint names -// traj_msg = good_traj_msg; -// traj_msg.joint_names = {"bad_name"}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // No position data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too many positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few velocities -// traj_msg = good_traj_msg; -// traj_msg.points[0].velocities = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few accelerations -// traj_msg = good_traj_msg; -// traj_msg.points[0].accelerations = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few efforts -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].effort = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Non-strictly increasing waypoint times -// traj_msg = good_traj_msg; -// traj_msg.points.push_back(traj_msg.points.front()); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -// } + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(0.25)); -// /// 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); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); - -// 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}; -// good_traj_msg.points[0].accelerations.resize(1); -// good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - -// // No position data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // No position and velocity data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].velocities.clear(); -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // All empty -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].velocities.clear(); -// traj_msg.points[0].accelerations.clear(); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too many positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few velocities -// traj_msg = good_traj_msg; -// traj_msg.points[0].velocities = {1.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few accelerations -// traj_msg = good_traj_msg; -// traj_msg.points[0].accelerations = {2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -// } + double threshold = 0.001; -// /** -// * @brief test_trajectory_replace Test replacing an existing trajectory -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - -// subscribeToState(); - -// std::vector> points_old{{{2., 3., 4.}}}; -// std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; -// std::vector> points_partial_new{{1.5}}; -// std::vector> points_partial_new_velocities{{0.15}}; - -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_actual.velocities = { -// points_old_velocities[0].begin(), points_old_velocities[0].end()}; -// expected_desired.velocities = { -// points_old_velocities[0].begin(), points_old_velocities[0].end()}; -// // Check that we reached end of points_old trajectory -// // Denis: delta was 0.1 with 0.2 works for me -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); - -// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); -// points_partial_new_velocities[0][0] = -// copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); -// publish( -// time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); - -// // Replaced trajectory is a mix of previous and current goal -// expected_desired.positions[0] = points_partial_new[0][0]; -// expected_desired.positions[1] = points_old[0][1]; -// expected_desired.positions[2] = points_old[0][2]; -// expected_desired.velocities[0] = points_partial_new_velocities[0][0]; -// expected_desired.velocities[1] = 0.0; -// expected_desired.velocities[2] = 0.0; -// expected_actual = expected_desired; -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "Joint 3 command should be current position"; + } -// /** -// * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); - -// // TODO(anyone): add expectations for velocities and accelerations -// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; -// std::vector> points_new{{{-1., -2., -3.}}}; - -// RCLCPP_INFO( -// traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time()); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0] trajectory -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); -// // New trajectory will end before current time -// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); -// expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; -// expected_desired = expected_actual; -// std::cout << "Sending old trajectory" << std::endl; -// publish(time_from_start, points_new, new_traj_start); -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } + if (traj_controller_->has_velocity_command_interface()) + { + // estimate the sign of the velocity + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); + EXPECT_NEAR(0.0, joint_vel_[2], threshold) + << "Joint 3 velocity should be 0.0 since it's not in the goal"; + } -// TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); - -// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; -// std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; - -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time()); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0]trajectory -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// RCLCPP_INFO( -// traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); -// // New trajectory first point is in the past, second is in the future -// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); -// expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; -// expected_desired = expected_actual; -// publish(time_from_start, points_new, new_traj_start); -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } + if (traj_controller_->has_effort_command_interface()) + { + // estimate the sign of the effort + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_NEAR(0.0, joint_eff_[2], threshold) + << "Joint 3 effort should be 0.0 since it's not in the goal"; + } + // TODO(anyone): add here checks for acceleration commands -// TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) -// { -// SetUpTrajectoryController(); -// auto traj_node = traj_controller_->get_node(); -// RCLCPP_WARN( -// traj_node->get_logger(), -// "Test disabled until current_trajectory is taken into account when adding a new trajectory."); -// // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ -// // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 -// return; - -// // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// subscribeToState(); -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// traj_node->set_parameter(partial_joints_parameters); -// traj_controller_->get_node()->configure(); -// traj_controller_->get_node()->activate(); - -// std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; -// std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; -// std::vector> partial_traj{ -// {{-1., -2.}, -// { -// -2., -// -4, -// }}}; -// std::vector> partial_traj_velocities{ -// {{-0.1, -0.2}, -// { -// -0.2, -// -0.4, -// }}}; -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; -// // Send full trajectory -// publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); -// // Sleep until first waypoint of full trajectory - -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0]trajectory and are starting points_old[1] -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// // Send partial trajectory starting after full trajecotry is complete -// RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); -// publish( -// points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); -// // Wait until the end start and end of partial traj - -// expected_actual.positions = { -// partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; -// expected_desired = expected_actual; - -// waitAndCompareState( -// expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); -// } + executor.cancel(); +} -// TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// // goal setup -// std::vector first_goal = {3.3, 4.4, 5.5}; -// std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; -// std::vector second_goal = {6.6, 8.8, 11.0}; -// std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; -// double state_from_command_offset = 0.3; - -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points{{first_goal}}; -// publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); -// traj_controller_->wait_for_trajectory(executor); -// updateController(rclcpp::Duration::from_seconds(1.1)); - -// if (traj_controller_->has_position_command_interface()) -// { -// // JTC is executing trajectory in open-loop therefore: -// // - internal state does not have to be updated (in this test-case it shouldn't) -// // - internal command is updated -// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - -// // Move joint further in the same direction as before (to the second goal) -// points = {{second_goal}}; -// publish(time_from_start, points, rclcpp::Time(1.0), {}, second_goal_velocities); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there should be a "jump" in opposite direction from command -// // (towards the state value) -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // Expect backward commands at first -// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); - -// // Finally the second goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - -// // Move joint back to the first goal -// points = {{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there should be a "jump" in the goal direction from command -// // (towards the state value) -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // Expect backward commands at first -// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); - -// // Finally the first goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// } - -// executor.cancel(); -// } +/** + * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled + * joints without allow_partial_joints_goal + */ +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); -// TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// // goal setup -// std::vector first_goal = {3.3, 4.4, 5.5}; -// std::vector second_goal = {6.6, 8.8, 11.0}; -// double state_from_command_offset = 0.3; - -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points{{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); -// updateController(rclcpp::Duration::from_seconds(1.1)); - -// if (traj_controller_->has_position_command_interface()) -// { -// // JTC is executing trajectory in open-loop therefore: -// // - internal state does not have to be updated (in this test-case it shouldn't) -// // - internal command is updated -// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - -// // Move joint further in the same direction as before (to the second goal) -// points = {{second_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there **should not** be a "jump" in opposite direction from -// // command (towards the state value) -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // There should not be backward commands -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); - -// // Finally the second goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - -// // Move joint back to the first goal -// points = {{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there **should not** be a "jump" in the goal direction from -// // command (towards the state value) -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // There should not be a jump toward commands -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); - -// // Finally the first goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// } - -// executor.cancel(); -// } + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); -// // Testing that values are read from state interfaces when hardware is started for the first -// // time and hardware state has offset --> this is indicated by NaN values in state interfaces -// TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; + const double initial_joint3_cmd = joint_pos_[2]; + trajectory_msgs::msg::JointTrajectory traj_msg; -// // set command values to NaN -// for (size_t i = 0; i < 3; ++i) -// { -// joint_pos_[i] = std::numeric_limits::quiet_NaN(); -// joint_vel_[i] = std::numeric_limits::quiet_NaN(); -// joint_acc_[i] = std::numeric_limits::quiet_NaN(); -// } -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + { + std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + traj_msg.joint_names = partial_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(2); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 1.0; + traj_msg.points[0].velocities.resize(2); + traj_msg.points[0].velocities[0] = 2.0; + traj_msg.points[0].velocities[1] = 1.0; + + trajectory_publisher_->publish(traj_msg); + } -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + traj_controller_->wait_for_trajectory(executor); + // update for 0.5 seconds + updateController(rclcpp::Duration::from_seconds(0.25)); -// for (size_t i = 0; i < 3; ++i) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - -// // check velocity -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); -// } - -// // check acceleration -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); -// } -// } - -// executor.cancel(); -// } + double threshold = 0.001; -// // Testing that values are read from state interfaces when hardware is started after some values -// // are set on the hardware commands -// TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "All joints command should be current position because goal was rejected"; + } -// // set command values to NaN -// for (size_t i = 0; i < 3; ++i) -// { -// joint_pos_[i] = 3.1 + i; -// joint_vel_[i] = 0.25 + i; -// joint_acc_[i] = 0.02 + i / 10.0; -// } -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + } -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + } -// for (size_t i = 0; i < 3; ++i) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - -// // check velocity -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); -// } - -// // check acceleration -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); -// } -// } - -// executor.cancel(); -// } + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], threshold) + << "All joints efforts should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], threshold) + << "All joints efforts should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], threshold) + << "All joints efforts should be 0.0 because goal was rejected"; + } + + executor.cancel(); +} + +/** + * @brief invalid_message Test mismatched joint and reference vector sizes + */ +TEST_P(TrajectoryControllerTestParameterized, invalid_message) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController( + executor, true, {partial_joints_parameters, allow_integration_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}; + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // Incompatible joint names + traj_msg = good_traj_msg; + traj_msg.joint_names = {"bad_name"}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few efforts + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].effort = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Non-strictly increasing waypoint times + traj_msg = good_traj_msg; + traj_msg.points.push_back(traj_msg.points.front()); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/// 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); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {allow_integration_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}; + good_traj_msg.points[0].accelerations.resize(1); + good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position and velocity data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // All empty + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + traj_msg.points[0].accelerations.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief test_trajectory_replace Test replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + + subscribeToState(); + + std::vector> points_old{{{2., 3., 4.}}}; + std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; + std::vector> points_partial_new{{1.5}}; + std::vector> points_partial_new_velocities{{0.15}}; + + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; + expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + // Check that we reached end of points_old trajectory + // Denis: delta was 0.1 with 0.2 works for me + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); + points_partial_new_velocities[0][0] = + copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); + + // Replaced trajectory is a mix of previous and current goal + expected_desired.positions[0] = points_partial_new[0][0]; + expected_desired.positions[1] = points_old[0][1]; + expected_desired.positions[2] = points_old[0][2]; + expected_desired.velocities[0] = points_partial_new_velocities[0][0]; + expected_desired.velocities[1] = 0.0; + expected_desired.velocities[2] = 0.0; + expected_actual = expected_desired; + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +} + +/** + * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}); + subscribeToState(); + + // TODO(anyone): add expectations for velocities and accelerations + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}}}; + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time()); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0] trajectory + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); + // New trajectory will end before current time + rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); + expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; + expected_desired = expected_actual; + std::cout << "Sending old trajectory" << std::endl; + publish(time_from_start, points_new, new_traj_start); + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +} + +TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}); + subscribeToState(); + + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; + + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time()); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO( + traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); + // New trajectory first point is in the past, second is in the future + rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); + expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; + expected_desired = expected_actual; + publish(time_from_start, points_new, new_traj_start); + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +} + +TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + subscribeToState(); + + RCLCPP_WARN( + traj_controller_->get_node()->get_logger(), + "Test disabled until current_trajectory is taken into account when adding a new trajectory."); + // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ + // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 + return; + + // *INDENT-OFF* + std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; + std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; + std::vector> partial_traj{{{-1., -2.}, {-2., -4}}}; + std::vector> partial_traj_velocities{{{-0.1, -0.2}, {-0.2, -0.4}}}; + // *INDENT-ON* + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; + // Send full trajectory + publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); + // Sleep until first waypoint of full trajectory + + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory and are starting points_old[1] + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + // Send partial trajectory starting after full trajecotry is complete + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + publish( + points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); + // Wait until the end start and end of partial traj + + expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; + expected_desired = expected_actual; + + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); +} + +// TODO(destogl) this test fails with errors +// second publish() gives an error, because end time is before current time +// as well as +// 2: The difference between joint_state_pos_[0] and joint_pos_[0] is 0.02999799000000003, +// which exceeds COMMON_THRESHOLD, where +// 2: joint_state_pos_[0] evaluates to 6.2999999999999998, +// 2: joint_pos_[0] evaluates to 6.2700020099999998, and +// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. +// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. +// test_jump_when_state_tracking_error_updated/0, where GetParam() = +// ({ "position" }, { "position" }) (3372 ms) + +#if 0 +TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set parameter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); + SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; + std::vector second_goal = {6.6, 8.8, 11.0}; + std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + std::vector> points{{first_goal}}; + publish(time_from_start, points, + rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(1.1)); + + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, + rclcpp::Time(1.0, 0.0, RCL_STEADY_TIME), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], state_from_command_offset + COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } + + executor.cancel(); +} +#endif + +// TODO(destogl) this test fails +// 2: The difference between second_goal[0] and joint_pos_[0] is 0.032986635000000319, +// which exceeds COMMON_THRESHOLD, where +// 2: second_goal[0] evaluates to 6.5999999999999996, +// 2: joint_pos_[0] evaluates to 6.5670133649999993, and +// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. +// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. +// test_no_jump_when_state_tracking_error_not_updated/1, where GetParam() = +// ({ "position" }, { "position", "velocity" }) (3374 ms) +#if 0 +TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector second_goal = {6.6, 8.8, 11.0}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + std::vector> points{{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(1.1)); + + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from + // command (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from + // command (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // There should not be a jump toward commands + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } + + executor.cancel(); +} +#endif + +// Testing that values are read from state interfaces when hardware is started for the first +// time and hardware state has offset --> this is indicated by NaN values in state interfaces +TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set parameter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to NaN + for (size_t i = 0; i < 3; ++i) + { + joint_pos_[i] = std::numeric_limits::quiet_NaN(); + joint_vel_[i] = std::numeric_limits::quiet_NaN(); + joint_acc_[i] = std::numeric_limits::quiet_NaN(); + } + SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + + for (size_t i = 0; i < 3; ++i) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + + // check velocity + if ( + std::find( + state_interface_types_.begin(), state_interface_types_.end(), + hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && + traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + } + + // check acceleration + if ( + std::find( + state_interface_types_.begin(), state_interface_types_.end(), + hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && + traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + } + } + + executor.cancel(); +} + +// Testing that values are read from state interfaces when hardware is started after some values +// are set on the hardware commands +TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set parameter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to NaN + for (size_t i = 0; i < 3; ++i) + { + joint_pos_[i] = 3.1 + i; + joint_vel_[i] = 0.25 + i; + joint_acc_[i] = 0.02 + i / 10.0; + } + SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + + for (size_t i = 0; i < 3; ++i) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + + // check velocity + if ( + std::find( + state_interface_types_.begin(), state_interface_types_.end(), + hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && + traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + } + + // check acceleration + if ( + std::find( + state_interface_types_.begin(), state_interface_types_.end(), + hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && + traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + } + } + + executor.cancel(); +} // position controllers INSTANTIATE_TEST_SUITE_P( @@ -1608,15 +1517,15 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"velocity"}), std::vector({"position", "velocity", "acceleration"})))); -// // only effort controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"effort"}), std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"effort"}), -// std::vector({"position", "velocity", "acceleration"})))); +// only effort controller +INSTANTIATE_TEST_SUITE_P( + OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); // TODO(destogl): this tests should be changed because we are using `generate_parameters_library` // TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 8f5f64d57f..9855341edf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -110,6 +110,8 @@ class TestableJointTrajectoryController bool has_velocity_command_interface() { return has_velocity_command_interface_; } + bool has_acceleration_command_interface() { return has_acceleration_command_interface_; } + bool has_effort_command_interface() { return has_effort_command_interface_; } bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } @@ -364,7 +366,13 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + // TODO(christophfroehlich): use the node's clock here for internal comparison + // if using RCL_STEADY_TIME -> + // C++ exception with description + // "can't compare times with different time sources" thrown in the test body. + // traj_controller_->update(clock.now(), clock.now() - start_time); + // maybe we can set the node clock to use RCL_STEADY_TIME too? + traj_controller_->update(node_->get_clock()->now(), clock.now() - start_time); } } From bc0f3fc2a3ebd282d2bf56202fc857b33d5bbda1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 25 Jun 2023 23:11:27 +0200 Subject: [PATCH 02/12] Increase action tests timeout (#680) --- joint_trajectory_controller/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 35223016b7..0552e0bc86 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -83,6 +83,7 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory_actions test/test_trajectory_actions.cpp ) + set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) From 5a05e17312f1f4afa64e5444aff59ede03647a27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 26 Jun 2023 19:36:57 +0200 Subject: [PATCH 03/12] [JTC] Fix time sources and wrong checks in tests (#686) * Fix time sources and wrong checks in tests * Use time from update-method instead of node clock * Readd test of last command in test_goal_tolerances_fail --------- Co-authored-by: Bence Magyar --- .../src/joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory_actions.cpp | 25 ++++++++++--------- .../test/test_trajectory_controller.cpp | 20 +++++++++------ .../test/test_trajectory_controller_utils.hpp | 8 +----- 4 files changed, 28 insertions(+), 27 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e3e0fd770..3b8f5f89bd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -231,7 +231,7 @@ controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - time_difference = get_node()->now().seconds() - traj_end.seconds(); + time_difference = time.seconds() - traj_end.seconds(); if (time_difference > default_tolerances_.goal_time_tolerance) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fc2dd42314..085c20cacd 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -66,11 +66,14 @@ class TestTrajectoryActions : public TrajectoryControllerTest goal_options_.feedback_callback = nullptr; } - void SetUpExecutor(const std::vector & parameters = {}) + void SetUpExecutor( + const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, true, parameters); + SetUpAndActivateTrajectoryController( + executor_, true, parameters, separate_cmd_and_state_values); SetUpActionClient(); @@ -472,15 +475,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) rclcpp::Parameter("constraints.joint3.goal", goal_tol), rclcpp::Parameter("constraints.goal_time", goal_time)}; - SetUpExecutor(params); + // separate command from states -> goal won't never be reached + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); - const double init_pos1 = joint_pos_[0]; - const double init_pos2 = joint_pos_[1]; - const double init_pos3 = joint_pos_[2]; - std::shared_future gh_future; - // send goal + // send goal; one point only -> command is directly set to reach this goal (no interpolation) { std::vector points; JointTrajectoryPoint point; @@ -502,14 +503,14 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding + // run an update, it should be holding the last received goal updateController(rclcpp::Duration::from_seconds(0.01)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(4.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(5.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(6.0, joint_pos_[2], COMMON_THRESHOLD); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 714d3c8d49..0c8b3ae939 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1092,7 +1092,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); + rclcpp::Time new_traj_start = + rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; std::cout << "Sending old trajectory" << std::endl; @@ -1108,23 +1109,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; - + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + + // send points_old and wait to reach first point publish(time_from_start, points_old, rclcpp::Time()); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + // send points_new before the old trajectory is finished RCLCPP_INFO( traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); - expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; - expected_desired = expected_actual; + rclcpp::Time new_traj_start = + rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); publish(time_from_start, points_new, new_traj_start); + // it should not have accepted the new goal but finish the old one + expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; + expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } @@ -1163,7 +1168,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); publish( - points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); + points_delay, partial_traj, rclcpp::Clock(RCL_STEADY_TIME).now() + delay * 2, {}, + partial_traj_velocities); // Wait until the end start and end of partial traj expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 9855341edf..97156f1a2e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -366,13 +366,7 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; while (clock.now() < end_time) { - // TODO(christophfroehlich): use the node's clock here for internal comparison - // if using RCL_STEADY_TIME -> - // C++ exception with description - // "can't compare times with different time sources" thrown in the test body. - // traj_controller_->update(clock.now(), clock.now() - start_time); - // maybe we can set the node clock to use RCL_STEADY_TIME too? - traj_controller_->update(node_->get_clock()->now(), clock.now() - start_time); + traj_controller_->update(clock.now(), clock.now() - start_time); } } From 48c6593707c904fce1858a631e19fac3dafded38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 28 Jun 2023 20:08:16 +0200 Subject: [PATCH 04/12] Match feature branches (#689) --- .github/workflows/rolling-binary-build-main.yml | 4 ++++ .github/workflows/rolling-binary-build-testing.yml | 4 ++++ .github/workflows/rolling-semi-binary-build-main.yml | 4 ++++ .github/workflows/rolling-semi-binary-build-testing.yml | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml index b51bbabe29..793db5d7e5 100644 --- a/.github/workflows/rolling-binary-build-main.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -6,9 +6,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml index a1db89b8d9..9b480d99c3 100644 --- a/.github/workflows/rolling-binary-build-testing.yml +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -6,9 +6,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build-main.yml index eca9483438..8b395e5163 100644 --- a/.github/workflows/rolling-semi-binary-build-main.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -5,9 +5,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml index 62214adae9..630881dc0a 100644 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -5,9 +5,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master From 0f8402423edc63410ee94723610568686f006be2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 29 Jun 2023 19:37:03 +0000 Subject: [PATCH 05/12] Revert "[JTC] Command final waypoint identically when traj_point_active_ptr_ is nullptr (#682)" This reverts commit 9ce288b9cd9caabb1d29df6fb4e1baefa52781cb. --- .../src/joint_trajectory_controller.cpp | 102 +++++++++--------- 1 file changed, 51 insertions(+), 51 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3b8f5f89bd..c870791daf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -241,6 +241,57 @@ controller_interface::return_type JointTrajectoryController::update( } } + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + if (use_closed_loop_pid_adapter_) + { + // Update PIDs + for (auto i = 0ul; i < dof_; ++i) + { + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_error_.positions[i], state_error_.velocities[i], + (uint64_t)period.nanoseconds()); + } + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + } + if (has_velocity_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + } + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); + } + } + + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; + } + const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { @@ -308,57 +359,6 @@ controller_interface::return_type JointTrajectoryController::update( set_hold_position(); RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); } - - // set values for next hardware write() if tolerance is met - if (!tolerance_violated_while_moving && within_goal_time) - { - if (use_closed_loop_pid_adapter_) - { - // Update PIDs - for (auto i = 0ul; i < dof_; ++i) - { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); - } - } - - // set values for next hardware write() - if (has_position_command_interface_) - { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); - } - if (has_velocity_command_interface_) - { - if (traj_point_active_ptr_ && use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[1], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); - } - } - if (has_acceleration_command_interface_) - { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); - } - if (has_effort_command_interface_) - { - if (traj_point_active_ptr_ && use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } - } - - // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; - } } } From ecb7d2a9fdcab987fccb9b47f2f95d9e15ede6a8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 29 Jun 2023 20:55:42 +0000 Subject: [PATCH 06/12] Add test to check PID status and velocity error --- .../test/test_trajectory_controller.cpp | 90 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 25 ++++-- 2 files changed, 107 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0c8b3ae939..bd547dc6be 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -673,6 +673,96 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) executor.cancel(); } +/** + * @brief check if use_closed_loop_pid is active + */ +TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + if ( + (traj_controller_->has_velocity_command_interface() && + !traj_controller_->has_position_command_interface() && + !traj_controller_->has_effort_command_interface() && + !traj_controller_->has_acceleration_command_interface() && + !traj_controller_->is_open_loop()) || + traj_controller_->has_effort_command_interface()) + { + EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); + } +} + +/** + * @brief check if velocity error is calculated correctly + */ +TEST_P(TrajectoryControllerTestParameterized, velocity_error) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points_positions{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.1, 0.1, 0.1}}, {{0.2, 0.2, 0.2}}, {{0.3, 0.3, 0.3}}}; + // *INDENT-ON* + publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(n_joints, state_msg->reference.velocities.size()); + EXPECT_EQ(n_joints, state_msg->feedback.velocities.size()); + EXPECT_EQ(n_joints, state_msg->error.velocities.size()); + } + if (traj_controller_->has_acceleration_state_interface()) + { + EXPECT_EQ(n_joints, state_msg->reference.accelerations.size()); + EXPECT_EQ(n_joints, state_msg->feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_msg->error.accelerations.size()); + } + + // no change in state interface should happen + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(state_msg->feedback.velocities, INITIAL_VEL_JOINTS); + } + // is the velocity error correct? + if ( + traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller + || (traj_controller_->has_velocity_state_interface() && + traj_controller_->has_velocity_command_interface())) + { + // don't check against a value, because spline intepolation might overshoot depending on + // interface combinations + EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_msg->error.velocities[2], points_velocities[0][2]); + } + + executor.cancel(); +} + /** * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from * internal controller order diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 97156f1a2e..4dd5f1a780 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -105,6 +105,11 @@ class TestableJointTrajectoryController { return last_commanded_state_; } + bool has_position_state_interface() { return has_position_state_interface_; } + + bool has_velocity_state_interface() { return has_velocity_state_interface_; } + + bool has_acceleration_state_interface() { return has_acceleration_state_interface_; } bool has_position_command_interface() { return has_position_command_interface_; } @@ -116,6 +121,8 @@ class TestableJointTrajectoryController bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + bool is_open_loop() { return params_.open_loop_control; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -294,13 +301,14 @@ class TrajectoryControllerTest : public ::testing::Test /// Publish trajectory msgs with multiple points /** * delay_btwn_points - delay between each points - * points - vector of trajectories. One point per controlled joint + * points_positions - vector of trajectory-positions. One point per controlled joint * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided * points + * points - vector of trajectory-velocities. One point per controlled joint */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, - const std::vector> & points, rclcpp::Time start_time, + const std::vector> & points_positions, rclcpp::Time start_time, const std::vector & joint_names = {}, const std::vector> & points_velocities = {}) { @@ -320,14 +328,15 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_msgs::msg::JointTrajectory traj_msg; if (joint_names.empty()) { - traj_msg.joint_names = {joint_names_.begin(), joint_names_.begin() + points[0].size()}; + traj_msg.joint_names = { + joint_names_.begin(), joint_names_.begin() + points_positions[0].size()}; } else { traj_msg.joint_names = joint_names; } traj_msg.header.stamp = start_time; - traj_msg.points.resize(points.size()); + traj_msg.points.resize(points_positions.size()); builtin_interfaces::msg::Duration duration_msg; duration_msg.sec = delay_btwn_points.sec; @@ -335,14 +344,14 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Duration duration(duration_msg); rclcpp::Duration duration_total(duration_msg); - for (size_t index = 0; index < points.size(); ++index) + for (size_t index = 0; index < points_positions.size(); ++index) { traj_msg.points[index].time_from_start = duration_total; - traj_msg.points[index].positions.resize(points[index].size()); - for (size_t j = 0; j < points[index].size(); ++j) + traj_msg.points[index].positions.resize(points_positions[index].size()); + for (size_t j = 0; j < points_positions[index].size(); ++j) { - traj_msg.points[index].positions[j] = points[index][j]; + traj_msg.points[index].positions[j] = points_positions[index][j]; } duration_total = duration_total + duration; } From 13c006f587723ac87ae82a7d8edb726a42ca7e11 Mon Sep 17 00:00:00 2001 From: Lars Tingelstad Date: Mon, 19 Jun 2023 21:31:22 +0200 Subject: [PATCH 07/12] Compute velocity errors when using an effort command interface (cherry picked from commit 2e0da5d2f0a8462f735e44c5b5ad0a58fe11aedb) --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c870791daf..a41fe62ac7 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -133,7 +133,7 @@ controller_interface::return_type JointTrajectoryController::update( { error.positions[index] = desired.positions[index] - current.positions[index]; } - if (has_velocity_state_interface_ && has_velocity_command_interface_) + if (has_velocity_state_interface_ && (has_velocity_command_interface_ || has_effort_command_interface_)) { error.velocities[index] = desired.velocities[index] - current.velocities[index]; } From 8e88c4dd9547504f1f0412f2d27eae7e1e190081 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 29 Jun 2023 21:08:43 +0000 Subject: [PATCH 08/12] Format code --- .../src/joint_trajectory_controller.cpp | 4 +++- joint_trajectory_controller/test/test_trajectory_actions.cpp | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a41fe62ac7..153515d089 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -133,7 +133,9 @@ controller_interface::return_type JointTrajectoryController::update( { error.positions[index] = desired.positions[index] - current.positions[index]; } - if (has_velocity_state_interface_ && (has_velocity_command_interface_ || has_effort_command_interface_)) + if ( + has_velocity_state_interface_ && + (has_velocity_command_interface_ || has_effort_command_interface_)) { error.velocities[index] = desired.velocities[index] - current.velocities[index]; } diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 085c20cacd..6482b8e921 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -304,7 +304,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal /** * Makes sense with position command interface only, * because no integration to position state interface is implemented -*/ + */ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { // set tolerance parameters @@ -349,7 +349,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) /** * Makes sense with position command interface only, * because no integration to position state interface is implemented -*/ + */ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { // set tolerance parameters From 811ae4dbcbf5be3f5a76dba51c9ec1bb6a5f4a1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 3 Jul 2023 21:53:16 +0200 Subject: [PATCH 09/12] [JTC] Reject trajectories with nonzero terminal velocity (#567) * Reject receiving trajectory of last velocity point is non-zero * Update docs * Add tests * Change to parameterized test * Rename parameter * not true -> false --------- Co-authored-by: Bence Magyar --- joint_trajectory_controller/doc/userdoc.rst | 5 + .../src/joint_trajectory_controller.cpp | 23 ++++ ...oint_trajectory_controller_parameters.yaml | 5 + .../test/test_trajectory_actions.cpp | 124 ++++++++++++++++++ .../test/test_trajectory_controller.cpp | 16 ++- 5 files changed, 170 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 7529a60918..b04f1109e3 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -175,6 +175,11 @@ open_loop_control (boolean) Default: false +allow_nonzero_velocity_at_trajectory_end (boolean) + If false, the last velocity point has to be zero or the goal will be rejected. + + Default: true + constraints (structure) Default values for tolerances if no explicit values are states in JointTrajectory message. diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 153515d089..dccb9fceff 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -64,6 +64,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() return CallbackReturn::ERROR; } + // TODO(christophfroehlich): remove deprecation warning + if (params_.allow_nonzero_velocity_at_trajectory_end) + { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to " + "true. The default behavior will change to false."); + } + return CallbackReturn::SUCCESS; } @@ -1321,6 +1330,20 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + if (!params_.allow_nonzero_velocity_at_trajectory_end) + { + for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) + { + if (trajectory.points.back().velocities.at(i) != 0.) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f", + trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i)); + return false; + } + } + } + rclcpp::Duration previous_traj_time(0ms); for (size_t i = 0; i < trajectory.points.size(); ++i) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 90ca12a268..77eaf3537f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -66,6 +66,11 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } + allow_nonzero_velocity_at_trajectory_end: { + type: bool, + default_value: true, + description: "If false, the last velocity point has to be zero or the goal will be rejected", + } gains: __map_joints: p: { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 6482b8e921..fdea551c30 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -616,6 +616,130 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) } } +TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) +{ + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal with nonzero last velocities + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 4.0; + point2.velocities[1] = 5.0; + point2.velocities[2] = 6.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + // will be accepted despite nonzero last point + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) +{ + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)}; + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal with nonzero last velocities + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 4.0; + point2.velocities[1] = 5.0; + point2.velocities[2] = 6.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_FALSE(gh_future.get()); + + // send goal with last velocity being zero + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 0.0; + point2.velocities[1] = 0.0; + point2.velocities[2] = 0.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + + EXPECT_TRUE(gh_future.get()); +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index bd547dc6be..d22498a795 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -65,6 +65,8 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // const auto future_handle_ = std::async(std::launch::async, spin, &executor); @@ -202,7 +204,9 @@ TEST_P(TrajectoryControllerTestParameterized, activate) TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpAndActivateTrajectoryController(executor, true, params); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -259,6 +263,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param { rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor, false); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch SetParameters(); @@ -450,7 +456,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -557,7 +565,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true); subscribeToState(); size_t n_joints = joint_names_.size(); From de7083e5d9178e24e24efae411cb2f228fdd09c3 Mon Sep 17 00:00:00 2001 From: gwalck Date: Fri, 7 Jul 2023 13:00:08 +0200 Subject: [PATCH 10/12] Fixed update period computation in test (#693) --- .../test/test_trajectory_controller_utils.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 4dd5f1a780..5ec0f4f29e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -373,9 +373,13 @@ class TrajectoryControllerTest : public ::testing::Test auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; + auto previous_time = start_time; + while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + auto now = clock.now(); + traj_controller_->update(now, now - previous_time); + previous_time = now; } } From 68ca86039bd4661a08f33a753652a543391958dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 16 Jul 2023 08:12:33 +0200 Subject: [PATCH 11/12] Fix namespace for parameter traits(#703) --- .../joint_trajectory_controller/validate_jtc_parameters.hpp | 5 ++--- .../src/joint_trajectory_controller_parameters.yaml | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index fc6319fabf..5a656e7754 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -18,12 +18,11 @@ #include #include -#include "parameter_traits/parameter_traits.hpp" #include "rclcpp/parameter.hpp" #include "rsl/algorithm.hpp" #include "tl_expected/expected.hpp" -namespace parameter_traits +namespace joint_trajectory_controller { tl::expected command_interface_type_combinations( rclcpp::Parameter const & parameter) @@ -95,6 +94,6 @@ tl::expected state_interface_type_combinations( return {}; } -} // namespace parameter_traits +} // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 77eaf3537f..5627f1d8f5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -22,7 +22,7 @@ joint_trajectory_controller: validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], - command_interface_type_combinations: null, + "joint_trajectory_controller::command_interface_type_combinations": null, } } state_interfaces: { @@ -32,7 +32,7 @@ joint_trajectory_controller: validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], - state_interface_type_combinations: null, + "joint_trajectory_controller::state_interface_type_combinations": null, } } allow_partial_joints_goal: { From 5850be53d1624eaa4db9e2e002d5604309154f3b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 17 Jul 2023 19:34:08 +0200 Subject: [PATCH 12/12] [JTC] Reject messages with effort fields (#699) --- .../src/joint_trajectory_controller.cpp | 24 +++++++++---------- .../test/test_trajectory_controller.cpp | 21 ++-------------- 2 files changed, 14 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index dccb9fceff..4b3a73c3a9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -289,14 +289,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_effort_command_interface_) { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } + assign_interface_from_point(joint_command_interface_[3], tmp_command_); } // store the previous command. Used in open-loop control mode @@ -1267,8 +1260,9 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", - joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + get_node()->get_logger(), + "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", joint_names_size, + string_for_vector_field.c_str(), vector_field.size(), i); return false; } return true; @@ -1384,11 +1378,17 @@ bool JointTrajectoryController::validate_trajectory_msg( !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, true) || - !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) + joint_count, points[i].accelerations, "accelerations", i, true)) { return false; } + // reject effort entries + if (!points[i].effort.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Trajectories with effort fields are currently not supported."); + return false; + } } return true; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index d22498a795..b4da9424bc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -802,15 +802,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.points[0].velocities[1] = -0.1; traj_msg.points[0].velocities[2] = -0.1; } - - if (traj_controller_->has_effort_command_interface()) - { - traj_msg.points[0].effort.resize(3); - traj_msg.points[0].effort[0] = -0.1; - traj_msg.points[0].effort[1] = -0.1; - traj_msg.points[0].effort[2] = -0.1; - } - trajectory_publisher_->publish(traj_msg); } @@ -833,13 +824,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) EXPECT_GT(0.0, joint_vel_[1]); EXPECT_GT(0.0, joint_vel_[2]); } - - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_GT(0.0, joint_eff_[0]); - EXPECT_GT(0.0, joint_eff_[1]); - EXPECT_GT(0.0, joint_eff_[2]); - } // TODO(anyone): add here checks for acceleration commands } @@ -1050,10 +1034,9 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg.points[0].accelerations = {1.0, 2.0}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - // Incompatible data sizes, too few efforts + // Effort is not supported in trajectory message traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].effort = {1.0, 2.0}; + traj_msg.points[0].effort = {1.0, 2.0, 3.0}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); // Non-strictly increasing waypoint times