Check rclcpp::ok before sleeping for clean shutdown

[MarqRazz]

Description

In MTC PR#684 I found move_group fails to shut down cleanly when preempting motion. This is because we are asking the node to sleep when the stack has been requested to shut down. To fix this I added a simple guard to make sure rclcpp::ok() before sleeping.

Here is a snippet of the stack trace when shutting down.

[move_group-5] #10   Source "/root/my_ws/src/moveit2/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp", line 549, in scenePublishingThread [0x7d8db9c1afaf]
[move_group-5]         546:       planning_scene_publisher_->publish(msg);
[move_group-5]         547:       if (is_full)
[move_group-5]         548:         RCLCPP_DEBUG(logger_, "Published full planning scene: '%s'", msg.name.c_str());
[move_group-5]       > 549:       rate.sleep();
[move_group-5]         550:     }
[move_group-5] #9    Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x7d8db914c480, in rclcpp::Rate::sleep()
[move_group-5] #8    Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x7d8db90a0238, in rclcpp::Clock::sleep_for(rclcpp::Duration, std::shared_ptr<rclcpp::Context>)

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]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 46.24%. Comparing base (7907501) to head (d178050).
Report is 1 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3484      +/-   ##
==========================================
+ Coverage   45.84%   46.24%   +0.40%     
==========================================
  Files         718      718              
  Lines       62668    62690      +22     
  Branches     7587     7589       +2     
==========================================
+ Hits        28727    28985     +258     
+ Misses      33774    33538     -236     
  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.

[rhaschke]

Is this a new issue, i.e. a regression in rclcpp? If so, that should be reported upstream.