Fix JTC from immediately returning success

[MarqRazz]

When using the JTC action server we have found a race condition where it can return SUCCESS as soon as the trajectory goal is accepted. I have confirmed that after the first goal is given to the controller the update() method maintains access to the last traj_point_active_ptr_ causing it continuously command the hardware to the end of the last goal's trajectory while also setting:

auto res = std::make_shared<FollowJTrajAction::Result>();
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
active_goal->setSucceeded(res);

If a new goal is received while processing update() there is a chance for it to immediately return SUCCESS but continue to execute the new goal.

This PR removes the ability for the update() method to command the arm when it does not have an active goal by setting traj_point_active_ptr_ to a nullptr after it has completed/aborted. When a new goal is received it is copied over to the traj_point_active_ptr_ allowing it to execute again.

Background on our problem:

This causes problems with one of our pipeline where additional motion planning is staged up after each trajectory is completed. When the goal returns and the arm is still in motion the next plan grabs the start_state from the robot while it's moving and then generates and sends the new plan for execution. The controller happly accepts the new goal which will cause the arm to quickly jump back to where it grabbed the start_state and then execute the new trajectory. This jump in the joint commands frequently causes our hardware to throw a fault. Below is a timeline of the happening on our hardware along with a graph of the joint_commands.

[mechwiz]

I think I noticed a similar if not the same issue a while back and made a PR that handles the issue here - #410. However your implementation may be better. Will review

[MarqRazz]

I think I noticed a similar if not the same issue a while back and made a PR that handles the issue here - #410. However your implementation may be better. Will review

I'm not sure if your PR will fix the whole issue. I think it might keep the second trajectory from being executed but will still immediately return SUCCESS for the action result if this section of code is being executed when a new goal is received.

[mechwiz]

I would agree with you if the active-goal is queried within that section of code. However, my PR specifically removes that query (auto active_goal = *rt_active_goal_.readFromRT();) from occurring in that section of code for the reason you are describing and instead does it earlier when checking when a new external message has been received which, in my opinion, is where that query should exist.

[MarqRazz]

I'm open to either fix. If you have changes you would like made here or we can continue to work on your PR just let me know.

I do think that this section of code should not be executed unless we have an active goal though which means it should be checked in your if statement...

if (active_goal && traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg() && !mismatch )

[mechwiz]

I worked through the logic between the 2 PRs and I think I prefer your implementation since yours will definitely ensure that that section of code won't execute after the previous goal finished until a new goal has successfully been obtained externally. (I think mine solves the issue in a less elegant way). I would +1 but I don't have any approval rights

[MarqRazz]

Any feedback from the ros2_control maintainer team?

[bmagyar]

Thanks for the insightful fix! The double accounting we do with the pointer Vs the buffer is the root of the issue which we may be able to refactor so we can get rid of this... Or maybe not! For now I'm happy with the change

[moriarty]

I believe this PR is failing on Rolling because some of the github ci jobs do pull in current dependencies to the upstream_workspace and a change to control_msgs has renamed some msg fields ros-controls/control_msgs#86

[bmagyar]

Aye I'm aware of those, that's why we have all these variants for the CI ;) eventually pending PRs on the releases will be merged and synced

[MarqRazz]

Do we need to do anything special to get this back ported to humble?

[bmagyar]

Nothing hopefully, but to remind me which you've already done!

[bmagyar]

@Mergifyio backport humble

[mergify]

backport humble

✅ Backports have been created

• #592 Fix JTC from immediately returning success (backport #565) has been created for branch humble

[egordon]

Unfortunately, this has introduced a bug on our end (realized with 2.20.0-1jammy.20230522.072811, but just noticed this week) with the use of a velocity hardware interface.

If we are within the goal tolerance (but not yet commanding identically 0 velocity because we aren't exactly at the goal), the controller stops commanding the hardware, and this means that the hardware interface is left with a non-0 velocity at the end of the trajectory, causing the robot to drift from the goal position over time.

I've confirmed that the bug is fixed by just commenting out L336 in the PR (L339 in the current humble build). I'll make a bug report.