[patrickKXMD]

Description

It is not thread-safe calling stopExecution or publishing a "stop" to trajectory_execution_event while trajectory executing. Sometimes it crashes, here's a sample

Program terminated with signal SIGSEGV, Segmentation fault.

#0 0x0000ffffa367ac48 in _gnu_cxx::normal_iterator<moveit_msgs::msg::RobotTrajectory<std::allocator > const*, std::vector<moveit_msgs::msg::RobotTrajectory<std::allocator >, std::allocator<moveit_msgs::msg::RobotTrajectory<std::allocator > > > >::__normal_iterator (__i=, this=)
at /usr/include/c++/11/bits/stl_iterator.h:1028
b1028 : _M_current(__i) { }
[Current thread is 1 (Thread 0xffff1e7cf120 (LWP 4402))]
(gdb)
(gdb)
(gdb) bt
#0 0x0000ffffa367ac48 in _gnu_cxx::normal_iterator<moveit_msgs::msg::RobotTrajectory<std::allocator > const*, std::vector<moveit_msgs::msg::RobotTrajectory<std::allocator >, std::allocator<moveit_msgs::msg::RobotTrajectory<std::allocator > > > >::__normal_iterator (i=, this=)
at /usr/include/c++/11/bits/stl_iterator.h:1028
#1 std::vector<moveit_msgs::msg::RobotTrajectory<std::allocator >, std::allocator<moveit_msgs::msg::RobotTrajectory<std::allocator > > >::end (
this=) at /usr/include/c++/11/bits/stl_vector.h:839
#2 trajectory_execution_manager::TrajectoryExecutionManager::waitForRobotToStop (this=this@entry=0xaaaaeed29f20, context=..., wait_time=wait_time@entry=1)
at /workspace/source/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp:1592
#3 0x0000ffffa36829d4 in trajectory_execution_manager::TrajectoryExecutionManager::executeThread(std::function<void (moveit_controller_manager::ExecutionStatus const&)> const&, std::function<void (unsigned long)> const&, bool) (this=0xaaaaeed29f20, callback=..., part_callback=..., auto_clear=) at /usr/include/c++/11/bits/stl_vector.h:1043
#4 0x0000ffffba7431fc in ?? () from /lib/aarch64-linux-gnu/libstdc++.so.6
#5 0x0000ffffbb92d5c8 in start_thread (arg=0x0) at ./nptl/pthread_create.c:442
#6 0x0000ffffbb995edc in thread_start () at ../sysdeps/unix/sysv/linux/aarch64/clone.S:79

This CL introduces a lock for interactions with trajectories_ and can fix this issue.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 0% with 6 lines in your changes missing coverage. Please review.

Project coverage is 46.01%. Comparing base (9f1a53d) to head (d32373b).
Report is 2 commits behind head on main.

Files with missing lines	Patch %	Lines
...ution_manager/src/trajectory_execution_manager.cpp	0.00%	6 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3198      +/-   ##
==========================================
+ Coverage   45.66%   46.01%   +0.35%     
==========================================
  Files         714      714              
  Lines       62296    62320      +24     
  Branches     7530     7532       +2     
==========================================
+ Hits        28444    28673     +229     
+ Misses      33686    33481     -205     
  Partials      166      166              

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[sea-bass]

Thanks!

In addition to looking at the CI failures since something isn't building, left a few suggestions.

Rolling is failing now until #3197, but the rest should pass.

[sjahr]

Thanks, once the remaining comment is addressed and the PR that updates the deprecated headers is merged I think this is good to go

[rhaschke]

We might avoid introducing a new mutex and reuse execution_state_mutex_.
In all locations calling clear(), the flag execution_complete_ is True:

• TrajectoryExecutionManager::stopExecution
• TrajectoryExecutionManager::execute: execution_complete_ is still true when calling clear()
• TrajectoryExecutionManager::executeThread

However, currently clear() is a public method. Thus, calling it directly will face the race condition.
I suggest changing its visibility to protected or even private.

[patrickKXMD]

Hi @sea-bass @rhaschke , in the latest commit I got waitForRobotToStop covered by scoped lock, so there's no need to extract joint names.

[sea-bass]

This looks really clean now -- as long as this still fixes your tests, looks great to me. Thanks!

[rhaschke]

If you follow my suggestion #3198 (review), you should also change visibility of clear() to protected or private!

Also, as a safe-guard for future changes, you should add an assertion to clear(), e.g. performing the following change to the line:

moveit2/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

Line 1279 in 80c7f61

RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed");

-    RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed");
+    RCLCPP_FATAL(logger_, "Expecting execution_complete_ to be true!");

[patrickKXMD]

If you follow my suggestion #3198 (review), you should also change visibility of clear() to protected or private!

Also, as a safe-guard for future changes, you should add an assertion to clear(), e.g. performing the following change to the line:

moveit2/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

Line 1279 in 80c7f61

RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed");

-    RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed");
+    RCLCPP_FATAL(logger_, "Expecting execution_complete_ to be true!");

Hi @rhaschke,
Thank you for your suggestion, I added an assertion in clear.

But I have concern for changing visibility of clear now. It has been public for a long time, changing it might break someone's code.
In fact moveit2 uses it's public visibility too.
e.g.

moveit2/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp

Line 109 in 75286c7

context_->trajectory_execution_manager_->clear();

moveit2/moveit_ros/planning/plan_execution/src/plan_execution.cpp

Line 405 in 75286c7

trajectory_execution_manager_->clear();

moveit2/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp

Lines 203 to 206 in 75286c7

	.def("clear", &trajectory_execution_manager::TrajectoryExecutionManager::clear,
	R"(
	Clear the trajectories to execute.
	)")

[rhaschke]

All these calls to clear() should be replaced by stopExecution(true).

[patrickKXMD]

All these calls to clear() should be replaced by stopExecution(true).

Hi I opened a new pr #3226 for this

[rhaschke]

I suggest to merge #3226 as part of this PR.