[moveit_ros] fix race condition when stopping trajectory execution (backport #3198)

[mergify]

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

---

This is an automatic backport of pull request #3198 done by [Mergify](https://mergify.com).

[mergify]

Cherry-pick of 6595dee has failed:

On branch mergify/bp/humble/pr-3198
Your branch is up to date with 'origin/humble'.

You are currently cherry-picking commit 6595deea4.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

no changes added to commit (use "git add" and/or "git commit -a")

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

[codecov-commenter]

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

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 51.40%. Comparing base (7a425d9) to head (196d2fc).
Report is 3 commits behind head on humble.

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

Additional details and impacted files

@@            Coverage Diff             @@
##           humble    #3239      +/-   ##
==========================================
- Coverage   51.41%   51.40%   -0.01%     
==========================================
  Files         382      382              
  Lines       31893    31893              
==========================================
- Hits        16396    16392       -4     
- Misses      15497    15501       +4     

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