Fix seg fault with attached objects during motion execution (backport #3466)

[mergify]

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.

---

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

[codecov]

Codecov Report

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

Project coverage is 45.80%. Comparing base (89506ab) to head (32dcd9d).
Report is 1 commits behind head on jazzy.

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             @@
##            jazzy    #3470      +/-   ##
==========================================
- Coverage   46.18%   45.80%   -0.38%     
==========================================
  Files         718      718              
  Lines       62661    62647      -14     
  Branches     7582     7586       +4     
==========================================
- Hits        28932    28687     -245     
- Misses      33565    33795     +230     
- Partials      164      165       +1     

☔ 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.