Skip to content

Commit ff3096c

Browse files
christophfroehlichchristophfroehlich
committed
Hold the position after the goal was reached successfully
Optionally reject receiving trajectory of last velocity point is non-zero
1 parent 1508d45 commit ff3096c

4 files changed

Lines changed: 186 additions & 16 deletions

File tree

‎joint_trajectory_controller/src/joint_trajectory_controller.cpp‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
6464
return CallbackReturn::ERROR;
6565
}
6666

67+
// TODO(christophfroehlich): remove deprecation warning
68+
if (params_.allow_nonzero_velocity_stop)
69+
{
70+
RCLCPP_WARN(
71+
get_node()->get_logger(),
72+
"[Deprecated]: \"allow_nonzero_velocity_stop\" is set to "
73+
"true. The default behavior will change to false.");
74+
}
75+
6776
return CallbackReturn::SUCCESS;
6877
}
6978

@@ -322,6 +331,7 @@ controller_interface::return_type JointTrajectoryController::update(
322331
{
323332
if (!outside_goal_tolerance)
324333
{
334+
set_hold_position();
325335
auto res = std::make_shared<FollowJTrajAction::Result>();
326336
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
327337
active_goal->setSucceeded(res);
@@ -1216,6 +1226,20 @@ bool JointTrajectoryController::validate_trajectory_msg(
12161226
}
12171227
}
12181228

1229+
if (!params_.allow_nonzero_velocity_stop)
1230+
{
1231+
for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
1232+
{
1233+
if (trajectory.points.back().velocities.at(i) != 0.)
1234+
{
1235+
RCLCPP_ERROR(
1236+
get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f",
1237+
trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
1238+
return false;
1239+
}
1240+
}
1241+
}
1242+
12191243
rclcpp::Duration previous_traj_time(0ms);
12201244
for (size_t i = 0; i < trajectory.points.size(); ++i)
12211245
{

‎joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,11 @@ joint_trajectory_controller:
6666
one_of<>: [["splines", "none"]],
6767
}
6868
}
69+
allow_nonzero_velocity_stop: {
70+
type: bool,
71+
default_value: true,
72+
description: "If not set true, the last velocity point has to be zero or the goal will be rejected",
73+
}
6974
gains:
7075
__map_joints:
7176
p: {

‎joint_trajectory_controller/test/test_trajectory_actions.cpp‎

Lines changed: 147 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -70,17 +70,13 @@ class TestTrajectoryActions : public TrajectoryControllerTest
7070
{
7171
setup_executor_ = true;
7272

73-
executor_ = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
74-
75-
SetUpAndActivateTrajectoryController(true, parameters);
76-
77-
executor_->add_node(traj_controller_->get_node()->get_node_base_interface());
73+
SetUpAndActivateTrajectoryController(executor_, true, parameters);
7874

7975
SetUpActionClient();
8076

81-
executor_->add_node(node_->get_node_base_interface());
77+
executor_.add_node(node_->get_node_base_interface());
8278

83-
executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_->spin(); });
79+
executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_.spin(); });
8480
}
8581

8682
void SetUpControllerHardware()
@@ -132,7 +128,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest
132128
if (setup_executor_)
133129
{
134130
setup_executor_ = false;
135-
executor_->cancel();
131+
executor_.cancel();
136132
executor_future_handle_.wait();
137133
}
138134
}
@@ -169,7 +165,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest
169165
int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
170166

171167
bool setup_executor_ = false;
172-
rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_;
168+
rclcpp::executors::MultiThreadedExecutor executor_;
173169
std::future<void> executor_future_handle_;
174170

175171
bool setup_controller_hw_ = false;
@@ -417,7 +413,7 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail)
417413
common_action_result_code_);
418414

419415
// run an update, it should be holding
420-
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
416+
updateController(rclcpp::Duration::from_seconds(0.01));
421417

422418
EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD);
423419
EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD);
@@ -467,7 +463,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail)
467463
common_action_result_code_);
468464

469465
// run an update, it should be holding
470-
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
466+
updateController(rclcpp::Duration::from_seconds(0.01));
471467

472468
EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD);
473469
EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD);
@@ -514,7 +510,7 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail)
514510
common_action_result_code_);
515511

516512
// run an update, it should be holding
517-
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
513+
updateController(rclcpp::Duration::from_seconds(0.01));
518514

519515
EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD);
520516
EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD);
@@ -563,9 +559,147 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position)
563559
const double prev_pos3 = joint_pos_[2];
564560

565561
// run an update, it should be holding
566-
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
562+
updateController(rclcpp::Duration::from_seconds(0.01));
567563

568564
EXPECT_EQ(prev_pos1, joint_pos_[0]);
569565
EXPECT_EQ(prev_pos2, joint_pos_[1]);
570566
EXPECT_EQ(prev_pos3, joint_pos_[2]);
571567
}
568+
569+
TEST_F(TestTrajectoryActions, test_allow_nonzero_velocity_stop_true)
570+
{
571+
// set parameter
572+
command_interface_types_ = {"velocity"};
573+
std::vector<rclcpp::Parameter> params = {
574+
rclcpp::Parameter("command_interfaces", "velocity"),
575+
rclcpp::Parameter("allow_nonzero_velocity_stop", true)};
576+
577+
SetUpExecutor(params);
578+
SetUpControllerHardware();
579+
580+
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
581+
// send goal
582+
{
583+
std::vector<JointTrajectoryPoint> points;
584+
JointTrajectoryPoint point1;
585+
point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
586+
point1.positions.resize(joint_names_.size());
587+
point1.velocities.resize(joint_names_.size());
588+
589+
point1.positions[0] = 4.0;
590+
point1.positions[1] = 5.0;
591+
point1.positions[2] = 6.0;
592+
point1.velocities[0] = 4.0;
593+
point1.velocities[1] = 5.0;
594+
point1.velocities[2] = 6.0;
595+
points.push_back(point1);
596+
597+
JointTrajectoryPoint point2;
598+
point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
599+
point2.positions.resize(joint_names_.size());
600+
point2.velocities.resize(joint_names_.size());
601+
602+
point2.positions[0] = 7.0;
603+
point2.positions[1] = 8.0;
604+
point2.positions[2] = 9.0;
605+
point2.velocities[0] = 4.0;
606+
point2.velocities[1] = 5.0;
607+
point2.velocities[2] = 6.0;
608+
points.push_back(point2);
609+
610+
gh_future = sendActionGoal(points, 1.0, goal_options_);
611+
}
612+
controller_hw_thread_.join();
613+
614+
// will be accepted despite nonzero last point
615+
EXPECT_TRUE(gh_future.get());
616+
617+
EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
618+
619+
// run an update, it should be holding the end position
620+
updateController(rclcpp::Duration::from_seconds(0.1));
621+
EXPECT_EQ(0.0, joint_vel_[0]);
622+
EXPECT_EQ(0.0, joint_vel_[1]);
623+
EXPECT_EQ(0.0, joint_vel_[2]);
624+
}
625+
626+
TEST_F(TestTrajectoryActions, test_allow_nonzero_velocity_stop_false)
627+
{
628+
// set parameter
629+
std::vector<rclcpp::Parameter> params = {
630+
rclcpp::Parameter("command_interfaces", "velocity"),
631+
rclcpp::Parameter("allow_nonzero_velocity_stop", false)};
632+
633+
SetUpExecutor(params);
634+
SetUpControllerHardware();
635+
636+
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
637+
// send goal with nonzero last velocities
638+
{
639+
std::vector<JointTrajectoryPoint> points;
640+
JointTrajectoryPoint point1;
641+
point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
642+
point1.positions.resize(joint_names_.size());
643+
point1.velocities.resize(joint_names_.size());
644+
645+
point1.positions[0] = 4.0;
646+
point1.positions[1] = 5.0;
647+
point1.positions[2] = 6.0;
648+
point1.velocities[0] = 4.0;
649+
point1.velocities[1] = 5.0;
650+
point1.velocities[2] = 6.0;
651+
points.push_back(point1);
652+
653+
JointTrajectoryPoint point2;
654+
point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
655+
point2.positions.resize(joint_names_.size());
656+
point2.velocities.resize(joint_names_.size());
657+
658+
point2.positions[0] = 7.0;
659+
point2.positions[1] = 8.0;
660+
point2.positions[2] = 9.0;
661+
point2.velocities[0] = 4.0;
662+
point2.velocities[1] = 5.0;
663+
point2.velocities[2] = 6.0;
664+
points.push_back(point2);
665+
666+
gh_future = sendActionGoal(points, 1.0, goal_options_);
667+
}
668+
controller_hw_thread_.join();
669+
670+
EXPECT_FALSE(gh_future.get());
671+
672+
// send goal with last velocity being zero
673+
{
674+
std::vector<JointTrajectoryPoint> points;
675+
JointTrajectoryPoint point1;
676+
point1.time_from_start = rclcpp::Duration::from_seconds(0.0);
677+
point1.positions.resize(joint_names_.size());
678+
point1.velocities.resize(joint_names_.size());
679+
680+
point1.positions[0] = 4.0;
681+
point1.positions[1] = 5.0;
682+
point1.positions[2] = 6.0;
683+
point1.velocities[0] = 4.0;
684+
point1.velocities[1] = 5.0;
685+
point1.velocities[2] = 6.0;
686+
points.push_back(point1);
687+
688+
JointTrajectoryPoint point2;
689+
point2.time_from_start = rclcpp::Duration::from_seconds(0.1);
690+
point2.positions.resize(joint_names_.size());
691+
point2.velocities.resize(joint_names_.size());
692+
693+
point2.positions[0] = 7.0;
694+
point2.positions[1] = 8.0;
695+
point2.positions[2] = 9.0;
696+
point2.velocities[0] = 0.0;
697+
point2.velocities[1] = 0.0;
698+
point2.velocities[2] = 0.0;
699+
points.push_back(point2);
700+
701+
gh_future = sendActionGoal(points, 1.0, goal_options_);
702+
}
703+
704+
EXPECT_TRUE(gh_future.get());
705+
}

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,8 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
6565
{
6666
rclcpp::executors::MultiThreadedExecutor executor;
6767
SetUpTrajectoryController(executor);
68+
traj_controller_->get_node()->set_parameter(
69+
rclcpp::Parameter("allow_nonzero_velocity_stop", true));
6870

6971
// const auto future_handle_ = std::async(std::launch::async, spin, &executor);
7072

@@ -315,7 +317,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
315317
TEST_P(TrajectoryControllerTestParameterized, cleanup)
316318
{
317319
rclcpp::executors::MultiThreadedExecutor executor;
318-
SetUpAndActivateTrajectoryController(executor);
320+
std::vector<rclcpp::Parameter> params = {rclcpp::Parameter("allow_nonzero_velocity_stop", true)};
321+
SetUpAndActivateTrajectoryController(executor, true, params);
319322

320323
// send msg
321324
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
@@ -369,6 +372,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
369372
{
370373
rclcpp::executors::MultiThreadedExecutor executor;
371374
SetUpTrajectoryController(executor, false);
375+
traj_controller_->get_node()->set_parameter(
376+
rclcpp::Parameter("allow_nonzero_velocity_stop", true));
372377

373378
// This call is replacing the way parameters are set via launch
374379
SetParameters();
@@ -512,7 +517,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
512517
{
513518
rclcpp::executors::MultiThreadedExecutor executor;
514519
constexpr double k_p = 10.0;
515-
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false);
520+
std::vector<rclcpp::Parameter> params = {rclcpp::Parameter("allow_nonzero_velocity_stop", true)};
521+
SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false);
516522
subscribeToState();
517523

518524
size_t n_joints = joint_names_.size();
@@ -610,7 +616,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
610616
{
611617
rclcpp::executors::MultiThreadedExecutor executor;
612618
constexpr double k_p = 10.0;
613-
SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true);
619+
std::vector<rclcpp::Parameter> params = {rclcpp::Parameter("allow_nonzero_velocity_stop", true)};
620+
SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true);
614621
subscribeToState();
615622

616623
size_t n_joints = joint_names_.size();

0 commit comments

Comments
 (0)