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 diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index cf64269e65..73d01ef132 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -78,13 +78,13 @@ if(BUILD_TESTING) ros2_control_test_assets ) - # TODO(andyz): Disabled due to flakiness - # ament_add_gmock(test_trajectory_actions - # test/test_trajectory_actions.cpp - # ) - # target_link_libraries(test_trajectory_actions - # joint_trajectory_controller - # ) + 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 + ) endif() diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 798cc161e5..54341721d5 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -159,6 +159,11 @@ open_loop_control (boolean) If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. +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/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index 91c652013e..6c909e3278 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 @@ -19,10 +19,11 @@ #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) @@ -94,6 +95,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.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ba299dd736..cc033b76cf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -67,6 +67,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; } @@ -136,7 +145,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_) + if ( + has_velocity_state_interface_ && + (has_velocity_command_interface_ || has_effort_command_interface_)) { error.velocities[index] = desired.velocities[index] - current.velocities[index]; } @@ -234,7 +245,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) { @@ -1416,6 +1427,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 e710b33d07..a754929ef7 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: { @@ -74,6 +74,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 10dd3a6b90..fdea551c30 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -66,21 +66,20 @@ 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; - executor_ = std::make_unique(); - - SetUpAndActivateTrajectoryController(true, parameters); - - executor_->add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpAndActivateTrajectoryController( + executor_, true, parameters, separate_cmd_and_state_values); SetUpActionClient(); - executor_->add_node(node_->get_node_base_interface()); + executor_.add_node(node_->get_node_base_interface()); - executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_->spin(); }); + executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_.spin(); }); } void SetUpControllerHardware() @@ -132,7 +131,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest if (setup_executor_) { setup_executor_ = false; - executor_->cancel(); + executor_.cancel(); executor_future_handle_.wait(); } } @@ -169,7 +168,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; bool setup_executor_ = false; - rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_; + rclcpp::executors::MultiThreadedExecutor executor_; std::future executor_future_handle_; bool setup_controller_hw_ = false; @@ -201,7 +200,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(); @@ -227,12 +243,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(); @@ -274,11 +293,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 @@ -312,11 +338,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 @@ -367,12 +400,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; @@ -417,14 +453,17 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) common_action_result_code_); // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + 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; @@ -436,15 +475,13 @@ TEST_F(TestTrajectoryActions, 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; @@ -466,15 +503,18 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // run an update, it should be holding the last received goal + 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(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); + } } -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; @@ -514,14 +554,17 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) common_action_result_code_); // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + 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(); @@ -563,9 +606,180 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) const double prev_pos3 = joint_pos_[2]; // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + updateController(rclcpp::Duration::from_seconds(0.01)); + + 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]); + } +} + +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()); - EXPECT_EQ(prev_pos1, joint_pos_[0]); - EXPECT_EQ(prev_pos2, joint_pos_[1]); - EXPECT_EQ(prev_pos3, joint_pos_[2]); + 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, + ::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 e3512d4d94..55d0ee0dfe 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -29,7 +29,6 @@ #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" -#include "control_msgs/msg/detail/joint_trajectory_controller_state__struct.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -49,10 +48,11 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/state.hpp" #include "std_msgs/msg/header.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_trajectory_controller_utils.hpp" + using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; @@ -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); @@ -199,123 +201,12 @@ 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; - 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); @@ -338,13 +229,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - // update for 0.25 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); - - // should be home pose again - 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); executor.cancel(); } @@ -369,6 +253,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(); @@ -428,21 +314,15 @@ 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()); - - // 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)); - - 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); + // reactivated + // wait so controller process the third point when reactivated + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + ActivateTrajectoryController(); + state = traj_controller_->get_state(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - // state = traj_controller_->get_node()->configure(); - // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + // TODO(christophfroehlich) add test if there is no active trajectory after + // reactivation once #558 or #609 got merged (needs methods for TestableJointTrajectoryController) executor.cancel(); } @@ -605,7 +485,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(); @@ -659,14 +541,20 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->output.positions[2], 3 * allowed_delta); } if (traj_controller_->has_velocity_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + EXPECT_LT(0.0, state_msg->output.velocities[0]); + EXPECT_LT(0.0, state_msg->output.velocities[1]); + EXPECT_LT(0.0, state_msg->output.velocities[2]); // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) @@ -688,9 +576,12 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) if (traj_controller_->has_effort_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + EXPECT_LT(0.0, state_msg->output.effort[0]); + EXPECT_LT(0.0, state_msg->output.effort[1]); + EXPECT_LT(0.0, state_msg->output.effort[2]); } executor.cancel(); @@ -703,7 +594,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(); @@ -762,14 +655,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) if (traj_controller_->has_velocity_command_interface()) { - // check command interface - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - EXPECT_GE(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], @@ -778,6 +666,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); @@ -785,16 +674,39 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) else { // interpolated points_velocities only - EXPECT_LE(0.0, joint_vel_[2]); + // 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_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(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(); @@ -847,804 +759,883 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) // test_state_publish_rate_target(0); // } -// /** -// * @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 check if use_closed_loop_pid is active + */ +TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +{ + rclcpp::executors::MultiThreadedExecutor executor; -// 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]); -// } + SetUpAndActivateTrajectoryController(executor); -// 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]); -// } -// } + 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 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); +/** + * @brief check if velocity error is calculated correctly + */ +TEST_P(TrajectoryControllerTestParameterized, velocity_error) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}, true); + subscribeToState(); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + size_t n_joints = joint_names_.size(); -// 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; + // 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); -// { -// 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(); -// } + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); -// /** -// * @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); + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + // 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()); + } -// 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; + // 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]); + } -// { -// 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(); -// } + 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( -// 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)); -// } +/** + * @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; + } + trajectory_publisher_->publish(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(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)); -// } + 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)); -// /** -// * @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(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_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()) + { + EXPECT_GT(0.0, joint_vel_[0]); + EXPECT_GT(0.0, joint_vel_[1]); + EXPECT_GT(0.0, joint_vel_[2]); + } + // TODO(anyone): add here checks for acceleration commands +} -// 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); -// } +/** + * @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); -// 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); -// } + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); -// 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(); -// } + 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; -// 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(); -// } + { + 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); + } -// // 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); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(0.25)); -// // 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); + double threshold = 0.001; -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + 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"; + } -// 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(); -// } + 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"; + } -// // 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_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 -// // 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); + executor.cancel(); +} -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); +/** + * @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); -// 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(); -// } + 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]; + 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; + + 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"; + } + + 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"; + } + + 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"; + } + + 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)); + +// Effort is not supported in trajectory message +#if 0 + // TODO(christophfroehlich) activate with #730 + traj_msg = good_traj_msg; + traj_msg.points[0].effort = {1.0, 2.0, 3.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +#endif + + // 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(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; + 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.}}}; + 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()); + 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(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); +} + +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(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]}; + 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( @@ -1684,26 +1675,25 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); -// // only velocity controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::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, 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 velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::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, 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 0c01eb1357..874a814a5d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -105,15 +105,24 @@ 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_; } 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_; } + bool is_open_loop() { return params_.open_loop_control; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -316,13 +325,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 = {}) { @@ -342,14 +352,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; @@ -357,14 +368,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; } @@ -386,9 +397,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; } } @@ -413,7 +428,7 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); } } @@ -423,7 +438,8 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + EXPECT_NEAR( + expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); } } }