Removal of pushAndExecute in TrajectoryExecutionManager removes functionality

[Martin-Oehler]

Description

Hello everyone,
the function TrajectoryExecutionManager::pushAndExecute has been removed in this commit 24c18f6, apparently as part of an ongoing rework here #3156 . As far as I am aware, the pushAndExecute function has been the only way to send a new trajectory without aborting the currently running one first. This can be used for continuous planning where we want to update the currently executed trajectory. With this being removed, I do not see an alternative way to achieve this with moveit.

Why has this function been removed on the main branch when the rework is not finished yet? Is there an alternative way to achieve this behavior somehow? Can the previous behavior be restored until the rework is actually progressed to a point where this capability is available again?

Thank you for your support.

Your environment

• ROS Distro: Noetic
• OS Version: Ubuntu 20.04
• Source: noetic-devel

Edit: Updated commit hash to point to commit on this repo.

[Martin-Oehler]

The function was even part of the public cpp interface here.

[simonschmeisser]

Sorry for any inconvenience, do you have some sample code that uses pushAndExecute in the way you described so we can see what's needed to revive that functionality?

[Martin-Oehler]

I am using pushAndExecute as previously implemented in the non-blocking execute function of the MoveItCpp Interface here: https://github.com/ros-planning/moveit/blob/0d46974ac7c3fe7f3aa2128f907c3bd2d78a25c6/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp#L277

Another example implementation of continuous replanning using this interface can be found here: Code, Discussion on Google Groups, YouTube Video.

[simonschmeisser]

I had a quick glance at your code and would like to confirm that I'm understanding correctly what you're doing. You adjust and partly replace the trajectory that is currently executed and call pushAndExecute again. ros_control will then splice or update the trajectory it is already executing?

[Martin-Oehler]

Thank you for looking into this. Yes, this is pretty much what I am doing. Trajectory replacement is done by the JointTrajectoryController. It only works if the current goal is aborted by a new goal (and not cancelled first). The only way known to me to achieve this with MoveIt has been the pushAndExecute function.

[cambel]

@Martin-Oehler, I added a replace() method, to this PR #3243, which you could use to achieve the same behavior as before. It would be great if you could check it out and test it.

[Martin-Oehler]

Thank you, I will try it out.

[Martin-Oehler]

Thank you for taking the time to look into my issue. I tested the replace() method today but could not get it to work. It crashes with a segmentation fault during execution of executeTrajectory() with the following stack trace (seemingly from another thread):

#0  0x00007ffff7be1674 in ros::Timer::stop() () from /opt/ros/noetic/lib/libroscpp.so
#1  0x00007ffff74c7cb4 in trajectory_execution_manager::TrajectoryExecutionManager::runEventManager (this=<optimized out>)
    at /usr/include/c++/9/bits/shared_ptr_base.h:1020
#2  0x00007ffff79bade4 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff7da3609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#4  0x00007ffff77f4133 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

There is no prior warning printed on the console. Also, the replace() method always returns false, which seems like a bug to me.

I also had the following problems when using this function compared to the previous pushAndExecute:

• When using the replace method with the default execution behavior (not simultaneous), it seemingly is stuck in an endless loop. It seems like, this is not supported, because trajectory IDs are only used for the simultaneous execution. Yet, the function does not check this.
• If you just want your trajectory to be executed, you now have to keep track if a trajectory is already running or not to either start a new one or replace an old one
• There is no easy way to figure out which trajectories are running in which states with which IDs, forcing you to keep track of them externally (as done in moveit_cpp)
• The trajectory validation always fails because in a continuous planning scheme, the robot already moves during planning. So the first state of the new trajectory corresponds to a state in the past, not the current state

[cambel]

@Martin-Oehler
Thank you for testing the feature. Could you provide a simple demo of your use case for me to test and fix the issue?

[Martin-Oehler]

Could you point me to a demo or test that I could modify to reflect my use-case?

[cambel]

Sure, I have been using this environment for testing

[dcconner]

Any update on this?
I'm trying to catch up on converting our flexible_manipulation package that works with FlexBE to Noetic. We were using pushAndExecute there. It is unclear from discussion at #3156 what the correct usage of pushAndExecute is given comments about replace() above.

[simonschmeisser]

@dcconner please provide a bit more detail about how and where (links to code) FlexBE is using pushAndExecute.

[dcconner]

We defined an extended_capability that interfaced with trajectory_manager using this pushAndExecute API.
https://github.com/CNURobotics/flexible_manipulation/blob/devel/flexible_manipulation_moveit_capabilities/src/helper/continuous_plan_execution.cpp

As linked above by @Martin-Oehler , this was based on code from https://github.com/team-vigir/vigir_manipulation_planning/blob/master/vigir_plan_execution/src/continuous_plan_execution.cpp

This was done back in 2018, and I haven't closely followed all of the recent MoveIt developments. I'm trying to do some cleanup and bring the system forward to Noetic in anticipation of ROS 2 conversion this summer.

If the consensus of the active MoveIt community was to remove this pushAndExecute functionality, then I can drop that particular capability. But given lack of response to @Martin-Oehler original issue I wanted to follow up before making that decision.

Is there a method to send updated trajectories during prior trajectory execution?
Is there a way to "stitch" trajectories in the new design? Or, is this now verboten.

My goal is to have FlexBE enabled state interfaces for all of MoveIt capabilities in Noetic and eventually ROS 2.

[simonschmeisser]

The original feature was kind of implicit and well hidden so the idea was and still is to come up with a better and more explicit API for trajectory replacement/stitching/splicing. You and @Martin-Oehler are both very welcome to participate in defining, testing and implementing this new API. A simple but very explicit example of how to use either the old api, the proposal implemented by @cambel or your dream of an API would be a very useful starting point.