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 d4979deef8..6f846703f9 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -969,6 +969,91 @@ 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 (now_time < end_time) + { + 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(); + 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::ABORTED, common_resultcode_); + + 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_->has_position_command_interface()) + { + 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_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_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]); + } +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized,