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/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index d056e0406c..7aa099db5a 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -67,10 +67,18 @@ INSTANTIATE_TEST_SUITE_P( // wrong length selected axes std::make_tuple( std::string("admittance.selected_axes"), - rclcpp::ParameterValue(std::vector() = {1, 2, 3})), - // invalid robot description - std::make_tuple( - std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))); + rclcpp::ParameterValue(std::vector() = {1, 2, 3})) + // invalid robot description. + // TODO(anyone): deactivated, because SetUpController returns SUCCESS here? + // ,std::make_tuple( + // std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot"))) + )); + +// Test on_init returns ERROR when a parameter is invalid +TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, invalid_parameters) +{ + ASSERT_EQ(SetUpController(), controller_interface::return_type::ERROR); +} TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index be78f05bb9..04c629a21b 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -27,7 +27,6 @@ #include "gmock/gmock.h" -#include "6d_robot_description.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,6 +37,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" +#include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" @@ -454,9 +454,15 @@ class AdmittanceControllerTestParameterizedInvalidParameters static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } protected: - void SetUpController() + controller_interface::return_type SetUpController() { - AdmittanceControllerTest::SetUpController("test_admittance_controller"); + auto param_name = std::get<0>(GetParam()); + auto param_value = std::get<1>(GetParam()); + std::vector parameter_overrides; + rclcpp::Parameter param(param_name, param_value); + parameter_overrides.push_back(param); + return AdmittanceControllerTest::SetUpController( + "test_admittance_controller", parameter_overrides); } }; diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp similarity index 98% rename from admittance_controller/test/6d_robot_description.hpp rename to admittance_controller/test/test_asset_6d_robot_description.hpp index f67b5bd054..4d38df7c30 100644 --- a/admittance_controller/test/6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ -#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ +#define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #include @@ -310,4 +310,4 @@ const auto valid_6d_robot_srdf = } // namespace ros2_control_test_assets -#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#endif // TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ 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/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 40aa8e6ba1..5b8bab8edf 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -22,6 +22,12 @@ state_interfaces (list(string)) Values: position (mandatory) [velocity, [acceleration]]. Acceleration interface can only be used in combination with position and velocity. +state_publish_rate (double) + + Publish-rate of the controller's “state” topic. + + Default: 50.0 + action_monitor_rate (double) Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). 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 602b5528ff..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) { @@ -1312,8 +1323,12 @@ void JointTrajectoryController::sort_to_local_joint_order( get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } - std::vector output; - output.resize(mapping.size(), 0.0); + static std::vector output(dof_, 0.0); + // Only resize if necessary since it's an expensive operation + if (output.size() != mapping.size()) + { + output.resize(mapping.size(), 0.0); + } for (size_t index = 0; index < mapping.size(); ++index) { auto map_index = mapping[index]; @@ -1412,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); } } }