New tem2 #2

cambel · 2022-08-12T06:05:34Z

Continuous trajectory execution feature proposal

Each new request push()-ed to the trajectory execution manager would spawn a new thread for its execution.
The thread would validate and if valid execute the trajectory:

Validate trajectory
1. Ensure that controllers are active
2. Check that required controllers are not being used already
3. Check that the start state of the trajectory matches the current state of the robot. It already considers the case where no such restriction is required.
4. Collision checking
  1. Check collision against currently active trajectories
  2. Check collision against the current state of the planning scene
Execute trajectories
Monitor execution duration

~~I wanted to overload the executeThread method but I get an error when attempting that.~~

If a vector of trajectories is pushed in continuous mode, the trajectories will be executed in sequential mode only, not simultaneously. Additionally, if one trajectory fails, subsequent trajectories will be aborted.

simonschmeisser

I added some first comments

simonschmeisser · 2022-08-17T20:07:41Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -989,6 +1046,12 @@ void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callba
 void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback,
                                         const PathSegmentCompleteCallback& part_callback, bool auto_clear)
 {
+  if (allow_continuous_execution_)
+  {
+    ROS_WARN_NAMED(LOGNAME, "Calls to execute() are not allowed when using continuous execution.");


Maybe reformulate a bit more positive. "In continous execution mode push()ed trajectories are started automatically. Ignoring call to execute()."

simonschmeisser · 2022-08-17T20:10:01Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

-  execution_thread_ = std::make_unique<boost::thread>(&TrajectoryExecutionManager::executeThread, this, callback,
-                                                      part_callback, auto_clear);
+  execution_thread_ =
+      std::make_unique<boost::thread>([this](const ExecutionCompleteCallback& c, const PathSegmentCompleteCallback& pc,


Suggested change

std::make_unique<boost::thread>([this](const ExecutionCompleteCallback& c, const PathSegmentCompleteCallback& pc,

std::make_unique<std::thread>([this](const ExecutionCompleteCallback& c, const PathSegmentCompleteCallback& pc,

this way it fits into one line in github diff view 😉

I will replace boost with standard libraries on all occurrences

simonschmeisser · 2022-08-17T20:16:58Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

    trajectories_.clear();
  }
  else
    ROS_ERROR_NAMED(LOGNAME, "Cannot push a new trajectory while another is being executed");
 }

+void TrajectoryExecutionManager::executeThread(


Please add a comment explaining which overload is which

simonschmeisser · 2022-08-17T20:26:17Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+    if (jointTrajPointToRobotState(active_trajectory.joint_trajectory, i, start_state))
+    {
+      robotStateToRobotStateMsg(start_state, start_state_msg);
+      if (!ps->isPathValid(start_state_msg, new_trajectory, new_trajectory.group_name, true))


Do we want to add a todo to add interpolation here?

I think we do want to improve that later so I will add it

simonschmeisser · 2022-08-17T20:28:14Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+                                                                   const moveit_msgs::RobotTrajectory& active_trajectory)
+{
+  // Before we start checking collisions, ensure that we have the latest robot state received...
+  planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());


if new_trajectory and active_trajectory cover all joints we could skip waiting for the updated robot state

Do they cover all joints?

No not generally, but in case they do this could be optimized ... but that's not a high priority

simonschmeisser · 2022-08-17T20:29:46Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+    {
+      ROS_DEBUG_STREAM_NAMED(
+          LOGNAME,
+          "Collision found between trajectory with (group_name, duration): "


Suggested change

"Collision found between trajectory with (group_name, duration): "

"Collision found between incoming trajectory with (group_name, duration): "

or similar

simonschmeisser · 2022-08-17T20:31:23Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+  return true;
+}
+
+bool TrajectoryExecutionManager::checkCollisionsWithCurrentState(const moveit_msgs::RobotTrajectory& trajectory)


isn't this redundant with the other check?

Maybe, but if there are no active trajectories then the other check wouldn't happen. But this could be done more efficiently

simonschmeisser · 2022-08-17T20:39:17Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+    const TrajectoryExecutionContext& context, std::set<moveit_controller_manager::MoveItControllerHandlePtr>& handles)
+{
+  // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
+  ros::Time current_time = ros::Time::now();


Suggested change

ros::Time current_time = ros::Time::now();

ros::Time start_time = ros::Time::now();

simonschmeisser · 2022-08-17T20:45:59Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+  {
+    if (execution_duration_monitoring_)
+    {
+      if (!handle->waitForExecution(expected_trajectory_duration))


doesn't this potentially wait handles.size()*(expected_trajectory_duration - epsilon) if the first handle finishes just in time, the second takes 2 * duration and so on. I know it's contrived but ...

Suggested change

if (!handle->waitForExecution(expected_trajectory_duration))

if (!handle->waitForExecution(expected_trajectory_duration - (ros::Time::now() - current_time))

Yes, this actually does not makes much sense to me either. It technically waits for the expected trajectory duration time for each handle

simonschmeisser · 2022-08-17T20:59:01Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+      std::make_unique<boost::thread>([this](const ExecutionCompleteCallback& c, const PathSegmentCompleteCallback& pc,
+                                             bool ac) { executeThread(c, pc, ac); },
+                                      callback, part_callback, auto_clear);


Suggested change

std::make_unique<boost::thread>([this](const ExecutionCompleteCallback& c, const PathSegmentCompleteCallback& pc,

bool ac) { executeThread(c, pc, ac); },

callback, part_callback, auto_clear);

std::make_unique<std::thread>([&]() { executeThread(callback, part_callback, auto_clear);});

or similar

This was necessary to explicitly declare which overloaded method to use. But I think that won't be necessary anymore

simonschmeisser · 2022-08-17T21:11:05Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+    }
+    else
+    {
+      ROS_DEBUG_NAMED(LOGNAME, "Execution stopped before starting");


We can end up here with eg the second trajectory of a bundle of 3. Then this message would be confusing.

Okay, I will change it

simonschmeisser · 2022-08-17T21:18:21Z

This does not yet include the "backlog" right? Is that by design or do you intend to add it in this PR as well? I'm starting to understand @v4hn rationale of maybe implementing the "backlog" in higher levels and outside the TEM so it might make sense to finish this PR first? Rejecting trajectories that are temporarily in collision could allow users to change plans and modify trajectories or reorder tasks/goals

simonschmeisser · 2022-08-17T21:28:32Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+
+  if (allow_continuous_execution_)
+  {
+    ROS_INFO_NAMED(LOGNAME, "Creating thread");


I wonder if the validation should happen here already and basically the push being rejected (return false) if the trajectory cannot currently be executed?

cambel added 4 commits August 5, 2022 12:52

Add dynamic reconfigure flag for continuous execution

59112c6

Change raw pointers to smart pointers

796b5bf

Foward planning scene to TEM

3fbcd85

Add group_name to robot trajectory

776674a

cambel force-pushed the master branch from 6964263 to a63580e Compare August 12, 2022 06:06

cambel force-pushed the new-tem2 branch 3 times, most recently from 5c5233a to 426d575 Compare August 12, 2022 06:24

Adding simultaneous execution feature to TEM

d86fe2d

cambel force-pushed the new-tem2 branch from 426d575 to d86fe2d Compare August 12, 2022 07:49

cambel added 6 commits August 15, 2022 10:55

Fix overloading issue

e9fc818

Fix initialization order

bf6b216

Add stopping criteria

fa6e3b6

Fix collision check with current state

7729fdf

Add whole trajectory to TrajectoryExecutionContext

fe3e89f

Clean up

b077756

cambel force-pushed the new-tem2 branch from a601eda to b077756 Compare August 15, 2022 04:19

Add support for execution of interdependent trajectories

fc88969

If a vector of trajectories is pushed in continuous mode, the trajectories will be executed in sequential mode only, not simultaneously. Additionally, if one trajectory fails, subsequent trajectories will be aborted.

cambel force-pushed the new-tem2 branch from bcb9fbf to fc88969 Compare August 16, 2022 02:49

simonschmeisser reviewed Aug 17, 2022

View reviewed changes

cambel mentioned this pull request Aug 18, 2022

GSoC 2022: Simultaneous Trajectory Execution moveit/moveit#3156

Open

cambel mentioned this pull request Aug 25, 2022

Allow to stream "stopless" trajectories executions moveit/moveit#3189

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

New tem2 #2

New tem2 #2

cambel commented Aug 12, 2022 •

edited

simonschmeisser left a comment

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 23, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser Aug 17, 2022

cambel Aug 18, 2022

simonschmeisser commented Aug 17, 2022

simonschmeisser Aug 17, 2022

	std::make_unique<boost::thread>([this](const ExecutionCompleteCallback& c, const PathSegmentCompleteCallback& pc,
	std::make_unique<std::thread>([this](const ExecutionCompleteCallback& c, const PathSegmentCompleteCallback& pc,

	"Collision found between trajectory with (group_name, duration): "
	"Collision found between incoming trajectory with (group_name, duration): "

	ros::Time current_time = ros::Time::now();
	ros::Time start_time = ros::Time::now();

	if (!handle->waitForExecution(expected_trajectory_duration))
	if (!handle->waitForExecution(expected_trajectory_duration - (ros::Time::now() - current_time))

New tem2 #2

Are you sure you want to change the base?

New tem2 #2

Conversation

cambel commented Aug 12, 2022 • edited

Continuous trajectory execution feature proposal

simonschmeisser left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

simonschmeisser commented Aug 17, 2022

Choose a reason for hiding this comment

cambel commented Aug 12, 2022 •

edited