Fix seg fault with attached objects during motion execution

[MarqRazz]

Description

#3327 introduced a bug where motion execution can segfault by attempting to indexing into arrays of zero length.

[move_group-5] [INFO moveit_task_constructor_visualization.execute_task_solution 1746211985.025712029]: Executing TaskSolution
[move_group-5] [INFO move_group.moveit.moveit.plugins.simple_controller_manager 1746211985.025804455]: Returned 1 controllers in list
[move_group-5] [INFO move_group.moveit.moveit.ros.trajectory_execution_manager 1746211985.025904844]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-5] [INFO move_group.moveit.moveit.ros.trajectory_execution_manager 1746211985.026685553]: Starting trajectory execution ...
[move_group-5] Stack trace (most recent call last) in thread 3045:
[move_group-5] #10   Object "", at 0xffffffffffffffff, in 
[move_group-5] #9    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x78936d34fc3b, in 
[move_group-5] terminate called after throwing an instance of 'std::length_error'
[move_group-5]   what():  basic_string::_M_create
[move_group-5] #8    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x78936d2c2aa3, in 
[move_group-5] Stack trace (most recent call last) in thread 3046:
[move_group-5] #7    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x78936d552db3, in 
[move_group-5] #6    Object "/root/underlay_ws/install/moveit_task_constructor_capabilities/lib/libmoveit_task_constructor_capabilities.so", at 0x78934c6ff157, in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::*)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability*, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void>::_M_run()
[move_group-5] #5    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x78936d2c7ed2, in 
[move_group-5] #4    Object "/opt/ros/jazzy/lib/libmoveit_planning_scene_monitor.so.2.12.3", at 0x78936dac97f1, in std::__future_base::_State_baseV2::_M_do_set(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*)
[move_group-5] #3    Object "/root/underlay_ws/install/moveit_task_constructor_capabilities/lib/libmoveit_task_constructor_capabilities.so", at 0x78934c6fdd1f, in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::*)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability*, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void> >::_M_invoke(std::_Any_data const&)
[move_group-5] #2    Object "/root/underlay_ws/install/moveit_task_constructor_capabilities/lib/libmoveit_task_constructor_capabilities.so", at 0x78934c6fd29e, in move_group::ExecuteTaskSolutionCapability::execCallback(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&)
[move_group-5] #1    Object "/opt/ros/jazzy/lib/libmoveit_plan_execution.so.2.12.3", at 0x78936d16703c, in plan_execution::PlanExecution::executeAndMonitor(plan_execution::ExecutableMotionPlan&, bool)
[move_group-5] #0    Object "/opt/ros/jazzy/lib/libmoveit_robot_state.so.2.12.3", at 0x78936d10674b, in moveit::core::RobotState::getAttachedBodies(std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, moveit::core::AttachedBody const*, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, moveit::core::AttachedBody const*> > >&) const
[move_group-5] Segmentation fault (Address not mapped to object [0xe8])
[move_group-5] #14   Object "", at 0xffffffffffffffff, in 

I assume this is because MTC sends execution requests where only the planing scene in modified, so trajectory lengths are zero. I also added some guards in robot_state to make sure we are not adding null objects to the attached_bodies list/map.

[forrest-rm]

Thanks for fixing the segfault

[codecov]

Codecov Report

Attention: Patch coverage is 8.69565% with 21 lines in your changes missing coverage. Please review.

Project coverage is 45.84%. Comparing base (3758a0f) to head (598dd30).
Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...ros/planning/plan_execution/src/plan_execution.cpp	0.00%	19 Missing ⚠️
moveit_core/robot_state/src/robot_state.cpp	50.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3466      +/-   ##
==========================================
- Coverage   46.23%   45.84%   -0.39%     
==========================================
  Files         718      718              
  Lines       62677    62662      -15     
  Branches     7583     7587       +4     
==========================================
- Hits        28975    28720     -255     
- Misses      33535    33775     +240     
  Partials      167      167              

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

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[MarqRazz]

Unfortunately this was still seg faulting here. Initializing current_attached_objects (which has raw pointers to the AttachedBody) with the initial planning_scene->getCurrentState and then updating it every loop was causing them to go out of scope and crashing the program.

[move_group-4] [move_group.moveit.moveit.ros.plan_execution 1746550138.595949405]: Attached object 'pick_object' is not in the robot state. Adding it.
[move_group-4] Stack trace (most recent call last) in thread 1049059:
[move_group-4] #8    Object "", at 0xffffffffffffffff, in 
[move_group-4] #7    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7a2fe21a2c3b, in 
[move_group-4] #6    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7a2fe2115aa3, in 
[move_group-4] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x7a2fe23a5db3, in 
[move_group-4] #4    Object "/root/colcon_ws/install/moveit_ros_planning/lib/libmoveit_trajectory_execution_manager.so.2.13.2", at 0x7a2fe1d5dec4, in trajectory_execution_manager::TrajectoryExecutionManager::executeThread(std::function<void (moveit_controller_manager::ExecutionStatus const&)> const&, std::function<void (unsigned long)> const&, bool)
[move_group-4] #3    Object "/root/colcon_ws/install/moveit_ros_planning/lib/libmoveit_plan_execution.so.2.13.2", at 0x7a2fe1fb4739, in plan_execution::PlanExecution::successfulTrajectorySegmentExecution(plan_execution::ExecutableMotionPlan const&, unsigned long)
[move_group-4] #2    Object "/root/colcon_ws/install/moveit_ros_planning/lib/libmoveit_plan_execution.so.2.13.2", at 0x7a2fe1fb389c, in plan_execution::PlanExecution::isRemainingPathValid(plan_execution::ExecutableMotionPlan const&, std::pair<int, int> const&)
[move_group-4] #1    Object "/root/colcon_ws/install/moveit_ros_planning/lib/libmoveit_plan_execution.so.2.13.2", at 0x7a2fe1fbddf9, in moveit::core::AttachedBody::AttachedBody(moveit::core::AttachedBody const&)
[move_group-4] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7a2fe2201a80, in 
[move_group-4] Segmentation fault (Signal sent by the kernel [(nil)])