Skip to content

Commit

Permalink
[JTC]: Abort goal on deactivate (#1517)
Browse files Browse the repository at this point in the history
  • Loading branch information
urfeex authored Feb 4, 2025
1 parent 694d6f1 commit 28845e6
Show file tree
Hide file tree
Showing 2 changed files with 86 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -1000,7 +1000,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
auto action_res = std::make_shared<FollowJTrajAction::Result>();
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());
}

Expand Down
85 changes: 85 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_actions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<rclcpp::Parameter> 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<typename GoalHandle::SharedPtr> gh_future;
// send goal
std::vector<double> point_positions{1.0, 2.0, 3.0};
{
std::vector<JointTrajectoryPoint> 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,
Expand Down

0 comments on commit 28845e6

Please sign in to comment.