From 822aac7db6bed36fdaa016e7038b29d7083769a6 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 3 Feb 2025 16:05:37 +0100 Subject: [PATCH 1/3] Add test for controller_deactivation cancels action --- .../test/test_trajectory_actions.cpp | 110 ++++++++++++++++++ 1 file changed, 110 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index d4979deef8..e78b6a61dc 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -969,6 +969,116 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr EXPECT_TRUE(gh_future.get()); } +TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_action) +{ + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); + + // We use our own hardware thread here, as we want to make sure the controller is deactivated + auto controller_thread = std::thread( + [&]() + { + // controller hardware cycle update loop + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + auto now_time = clock.now(); + auto last_time = now_time; + rclcpp::Duration wait = rclcpp::Duration::from_seconds(0.5); + auto end_time = last_time + wait; + while (clock.now() < end_time) + { + now_time = clock.now(); + traj_controller_->update(now_time, now_time - last_time); + last_time = now_time; + } + RCLCPP_INFO(node_->get_logger(), "Controller hardware thread finished"); + traj_controller_->get_node()->deactivate(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + }); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(2.5); + point.positions = point_positions; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + + controller_thread.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::CANCELED, common_resultcode_); + auto state = traj_controller_->get_state_reference(); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint + // method. + if (traj_controller_->use_closed_loop_pid_adapter() == false) + { + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(state.positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(state.positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(state.positions.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(state.velocities.at(0), joint_vel_[0]); + EXPECT_EQ(state.velocities.at(1), joint_vel_[1]); + EXPECT_EQ(state.velocities.at(2), joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + else // traj_controller_->use_closed_loop_pid_adapter() == true + { + // velocity or effort PID? + // --> set kp > 0.0 in test + if (traj_controller_->has_velocity_command_interface()) + { + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + state.positions.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + << "test position point " << state.positions.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + } + } + if (traj_controller_->has_effort_command_interface()) + { + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + state.positions.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + << "test position point " << state.positions.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + } + } + } +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, From d22db79cf6f568ac6c3249f409e95053e6b46559 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 3 Feb 2025 16:17:39 +0100 Subject: [PATCH 2/3] Abort trajectory upon controller deactivation --- .../src/joint_trajectory_controller.cpp | 2 +- joint_trajectory_controller/test/test_trajectory_actions.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a4ad5210be..bbe9afde75 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1000,7 +1000,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled during deactivate transition."); - active_goal->setCanceled(action_res); + active_goal->setAborted(action_res); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index e78b6a61dc..f1e7090dd0 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -1014,7 +1014,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_acti controller_thread.join(); EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::CANCELED, common_resultcode_); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + auto state = traj_controller_->get_state_reference(); // run an update From 201386ad99282c819545aab945a05465fcf27fe0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 3 Feb 2025 21:09:14 +0100 Subject: [PATCH 3/3] Fix test Checking for the command_interface's values should be sufficient --- .../test/test_trajectory_actions.cpp | 78 +++++++------------ 1 file changed, 26 insertions(+), 52 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index f1e7090dd0..6f846703f9 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -986,11 +986,12 @@ TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_acti auto last_time = now_time; rclcpp::Duration wait = rclcpp::Duration::from_seconds(0.5); auto end_time = last_time + wait; - while (clock.now() < end_time) + while (now_time < end_time) { - now_time = clock.now(); + now_time = now_time + rclcpp::Duration::from_seconds(0.01); traj_controller_->update(now_time, now_time - last_time); last_time = now_time; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } RCLCPP_INFO(node_->get_logger(), "Controller hardware thread finished"); traj_controller_->get_node()->deactivate(); @@ -1016,67 +1017,40 @@ TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_acti EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); - auto state = traj_controller_->get_state_reference(); + auto state_ref = traj_controller_->get_state_reference(); + auto state = traj_controller_->get_state_feedback(); // run an update updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint // method. - if (traj_controller_->use_closed_loop_pid_adapter() == false) + if (traj_controller_->has_position_command_interface()) { - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(state.positions.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(state.positions.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(state.positions.at(2), joint_pos_[2], COMMON_THRESHOLD); - } - - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_EQ(state.velocities.at(0), joint_vel_[0]); - EXPECT_EQ(state.velocities.at(1), joint_vel_[1]); - EXPECT_EQ(state.velocities.at(2), joint_vel_[2]); - } + EXPECT_NEAR(state_ref.positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(state_ref.positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(state_ref.positions.at(2), joint_pos_[2], COMMON_THRESHOLD); + } - if (traj_controller_->has_acceleration_command_interface()) - { - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_EQ(0.0, joint_eff_[0]); - EXPECT_EQ(0.0, joint_eff_[1]); - EXPECT_EQ(0.0, joint_eff_[2]); - } + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); } - else // traj_controller_->use_closed_loop_pid_adapter() == true + + if (traj_controller_->has_effort_command_interface()) { - // velocity or effort PID? - // --> set kp > 0.0 in test - if (traj_controller_->has_velocity_command_interface()) - { - for (size_t i = 0; i < 3; i++) - { - EXPECT_TRUE(is_same_sign_or_zero( - state.positions.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) - << "test position point " << state.positions.at(i) << ", position state is " - << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; - } - } - if (traj_controller_->has_effort_command_interface()) - { - for (size_t i = 0; i < 3; i++) - { - EXPECT_TRUE(is_same_sign_or_zero( - state.positions.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) - << "test position point " << state.positions.at(i) << ", position state is " - << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; - } - } + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); } }