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

[mergify]

Cherry-pick of c81ac91 has failed:

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

You are currently cherry-picking commit c81ac917e.
  (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_core/robot_state/src/robot_state.cpp
	both modified:   moveit_ros/planning/plan_execution/src/plan_execution.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

[MarqRazz]

The back port of this feature was never merged into Humble (#3413). I will hold off on this because till we decide if it is going to be included there.