JTC does not stop velocity command after goal succeed

[tonynajjar]

Describe the bug

I'm using the JTC with a velocity command interface. After the trajectory goal succeeds, a small velocity command is still continuously sent to the hardware interface.

To Reproduce
setup JTC with velocity command and send a goal

Expected behavior

I would expect the velocity command to be reset to 0.

Environment (please complete the following information):

• OS: Ubuntu Jammy
• Version: master

Additional context

[tonynajjar]

I noticed that this only happens when use_closed_loop_pid_adapter_ = true, so when there is only a velocity command, which is my case

[christophfroehlich]

Even worse (?): I realized now that JTC with use_closed_loop_pid_adapter_ does not stop the movement after a goal gets cancelled, but the last velocity is maintained.

[christophfroehlich]

We have now two PRs addressing the hold-position at startup or after canceling a goal. I think now, that the issue of this PR has a different cause because set_hold_position() is not called after reaching the goal:

ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp

Lines 323 to 333 in b70a8d0

	if (!outside_goal_tolerance)
	{
	auto res = std::make_shared<FollowJTrajAction::Result>();
	res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
	active_goal->setSucceeded(res);
	// TODO(matthew-reynolds): Need a lock-free write here
	// See https://github.com/ros-controls/ros2_controllers/issues/168
	rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
	RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");
	}

I tried different action goals, with velocity command interface (use_closed_loop_pid_adapter_ is true). I could not reproduce the behavior you described if the action goal has only position entries.
But if I add velocities to the points, where the last point is nonzero, the controller does not stop after reaching the goal. (behavior is depending on the combination of ff_velocity_scale and feedback gains)
https://github.com/christophfroehlich/gazebo_ros2_control/blob/66eabd30851847da2143714a586ec9524d79a73a/gazebo_ros2_control_demos/examples/example_position_with_velocity.cpp#L132-L137

Might that be your case? I'm not sure what the controller should do if the last received commanded velocity is different from zero. Should we call set_hold_position() then?

[tonynajjar]

I moved on from this and don't use JTC anymore unfortunately, I won't be able to help

[christophfroehlich]

Ok. If the problem was a nonzero velocity of the trajectory point at the end, it's maybe not a bug but some missing (optional) feature to automatically set it zero.

[christophfroehlich]

Fixed with #558