Skip to content

Publish planning scene while planning#3689

Merged
rhaschke merged 17 commits intomoveit:masterfrom
riv-mjohnson:publish_planning_scene_while_planning
Feb 20, 2025
Merged

Publish planning scene while planning#3689
rhaschke merged 17 commits intomoveit:masterfrom
riv-mjohnson:publish_planning_scene_while_planning

Conversation

@riv-mjohnson
Copy link
Copy Markdown
Contributor

@riv-mjohnson riv-mjohnson commented Jan 23, 2025

Description

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

@riv-mjohnson riv-mjohnson marked this pull request as draft January 23, 2025 14:15
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This points into the right direction. I'm sure there are many more PS locks.
Question: Do we still need ExecutableMotionPlan::planning_scene_monitor_ after all?

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

riv-mjohnson commented Jan 23, 2025

Question: Do we still need ExecutableMotionPlan::planning_scene_monitor_ after all?

I was wondering this. It looks like the only place it is used (other than to lock it) is when ApproachAndTranslateStage::executeAttachObject calls motion_plan.planning_scene_monitor_->triggerSceneUpdateEvent.

If I understand ExecutableMotionPlan correctly, this is not really how it is supposed to be used?

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

riv-mjohnson commented Jan 23, 2025

From poking around the code, I think I probably have misunderstood ExecutableMotionPlan.

I assumed the planning_scene_ member was supposed to be frozen when the class was made, but it looks like it's only supposed to be frozen when the planning scene is locked?

So I wonder if I should keep the planning_scene_monitor_ variable, but remove the planning_scene_ variable (in favour of calling copyPlanningScene or LockedPlanningScene as required)?

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

(This PR now solves the original problem - now I need to stop it breaking everything else)

@rhaschke
Copy link
Copy Markdown
Contributor

I assumed the planning_scene_ member was supposed to be frozen when the class was made, but it looks like it's only supposed to be frozen when the planning scene is locked?

I don't know the used of it by heart and would need to dive into the code. Unfortunately, I don't have time to do that before the weekend.

@codecov-commenter
Copy link
Copy Markdown

codecov-commenter commented Jan 23, 2025

⚠️ Please install the 'codecov app svg image' to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 92.00000% with 2 lines in your changes missing coverage. Please review.

Project coverage is 47.89%. Comparing base (7bc8da0) to head (4c8a736).
Report is 1 commits behind head on master.

Files with missing lines Patch % Lines
...ce_capability/src/pick_place_action_capability.cpp 50.00% 2 Missing ⚠️

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

Additional details and impacted files
@@            Coverage Diff             @@
##           master    #3689      +/-   ##
==========================================
- Coverage   48.01%   47.89%   -0.11%     
==========================================
  Files         605      605              
  Lines       61228    61350     +122     
  Branches     7054     7028      -26     
==========================================
- Hits        29390    29375      -15     
- Misses      31417    31554     +137     
  Partials      421      421              

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

@riv-mjohnson riv-mjohnson marked this pull request as ready for review January 27, 2025 16:20
@riv-mjohnson riv-mjohnson requested a review from v4hn as a code owner January 27, 2025 16:20
@riv-mjohnson riv-mjohnson changed the title WIP Publish planning scene while planning Publish planning scene while planning Jan 27, 2025
@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

I think my attempted changes to plan_execution.cpp are unworkable: the workflow seems to require that plan.planning_scene_ be modified with diff(scene_diff) but still updated by the planning scene monitor.

I think I'm going to remove those changes, and limit the scope of this PR to the other places where ExecutableMotionPlan is used.

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

This approach doesn't seem to be working. I'm testing it by planning a series of motions, with subsequent motions planning while earlier motions are executing.

The first motion plans and executes fine, but later motions start printing "Returning dirty link transforms", and occasionally spurious collisions are reported. Then the move group node crashes with backtrace

0x00007de5ab7ea576 in Eigen::internal::transform_transform_product_impl<Eigen::Transform<double, 3, 1, 0>, Eigen::Transform<double, 3, 1, 0>, false>::run(Eigen::Transform<double, 3, 1, 0> const&, Eigen::Transform<double, 3, 1, 0> const&) () from /opt/ros/noetic/lib/libmoveit_planning_scene_monitor.so.1.1.16
(gdb) bt
#0  0x00007de5ab7ea576 in Eigen::internal::transform_transform_product_impl<Eigen::Transform<double, 3, 1, 0>, Eigen::Transform<double, 3, 1, 0>, false>::run(Eigen::Transform<double, 3, 1, 0> const&, Eigen::Transform<double, 3, 1, 0> const&) () from /opt/ros/noetic/lib/libmoveit_planning_scene_monitor.so.1.1.16
#1  0x00007de5aadc765f in moveit::core::AttachedBody::computeTransform(Eigen::Transform<double, 3, 1, 0> const&) () from /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.16
#2  0x00007de5aadea75c in moveit::core::RobotState::attachBody(std::unique_ptr<moveit::core::AttachedBody, std::default_delete<moveit::core::AttachedBody> >) ()
   from /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.16
#3  0x00007de5aadeabab in moveit::core::RobotState::copyFrom(moveit::core::RobotState const&) () from /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.16
#4  0x00007de5aadead68 in moveit::core::RobotState::RobotState(moveit::core::RobotState const&) () from /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.16
#5  0x00007de5aaa082c8 in planning_scene::PlanningScene::getCurrentStateNonConst() () from /opt/ros/noetic/lib/libmoveit_planning_scene.so.1.1.16
#6  0x00007de5a12ca235 in pilz_industrial_motion_planner::TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&) () from /opt/ros/noetic/lib/libplanning_context_loader_circ.so
#7  0x00007de5a12ca8f6 in pilz_industrial_motion_planner::TrajectoryGenerator::generate(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, double) () from /opt/ros/noetic/lib/libplanning_context_loader_circ.so
#8  0x00007de5a127015f in pilz_industrial_motion_planner::PlanningContextBase<pilz_industrial_motion_planner::TrajectoryGeneratorLIN>::solve(planning_interface::MotionPlanResponse&)
    () from /opt/ros/noetic/lib/libplanning_context_loader_lin.so
#9  0x00007de5aa28575f in boost::detail::function::function_obj_invoker3<planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(std::shared_ptr<planning_interface::PlannerManager> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const::{lambda(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)#1}, bool, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&>::invoke(boost::detail::function::function_buffer&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanReq--Type <RET> for more, q to quit, c to continue without paging--
uest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) () from /opt/ros/noetic/lib/libmoveit_planning_request_adapter.so.1.1.16
#10 0x00007de5a1caae6c in default_planner_request_adapters::FixWorkspaceBounds::adaptAndPlan(boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const ()
   from /opt/ros/noetic/lib/libmoveit_default_planning_request_adapter_plugins.so
#11 0x00007de5aa285983 in planning_request_adapter::(anonymous namespace)::callAdapter(planning_request_adapter::PlanningRequestAdapter const&, boost::function<bool (std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) () from /opt/ros/noetic/lib/libmoveit_planning_request_adapter.so.1.1.16
#12 0x00007de5aa286b17 in planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(std::shared_ptr<planning_interface::PlannerManager> const&, std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /opt/ros/noetic/lib/libmoveit_planning_request_adapter.so.1.1.16
#13 0x00007de5aaee459a in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const ()
   from /opt/ros/noetic/lib/libmoveit_planning_pipeline.so.1.1.16
#14 0x00007de5aaee5d83 in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) const () from /opt/ros/noetic/lib/libmoveit_planning_pipeline.so.1.1.16
#15 0x00007de59bf22fdf in pilz_industrial_motion_planner::CommandListManager::solveSequenceItems(std::shared_ptr<planning_scene::PlanningScene const> const&, std::shared_ptr<planning_pipeline::PlanningPipeline> const&, moveit_msgs::MotionSequenceRequest_<std::allocator<void> > const&) const () from /opt/ros/noetic/lib/libsequence_capability.so
--Type <RET> for more, q to quit, c to continue without paging--
#16 0x00007de59bf2377d in pilz_industrial_motion_planner::CommandListManager::solve(std::shared_ptr<planning_scene::PlanningScene const> const&, std::shared_ptr<planning_pipeline::PlanningPipeline> const&, moveit_msgs::MotionSequenceRequest_<std::allocator<void> > const&) () from /opt/ros/noetic/lib/libsequence_capability.so
#17 0x00007de59bf18a76 in pilz_industrial_motion_planner::MoveGroupSequenceService::plan(moveit_msgs::GetMotionSequenceRequest_<std::allocator<void> >&, moveit_msgs::GetMotionSequenceResponse_<std::allocator<void> >&) () from /opt/ros/noetic/lib/libsequence_capability.so
#18 0x00007de59bf1c917 in ros::ServiceCallbackHelperT<ros::ServiceSpec<moveit_msgs::GetMotionSequenceRequest_<std::allocator<void> >, moveit_msgs::GetMotionSequenceResponse_<std::allocator<void> > > >::call(ros::ServiceCallbackHelperCallParams&) () from /opt/ros/noetic/lib/libsequence_capability.so
#19 0x00007de5ab4d7429 in ros::ServiceCallback::call() () from /opt/ros/noetic/lib/libroscpp.so
#20 0x00007de5ab528162 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/noetic/lib/libroscpp.so
#21 0x00007de5ab529873 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/noetic/lib/libroscpp.so
#22 0x00007de5ab57c37e in ros::AsyncSpinnerImpl::threadFunc() () from /opt/ros/noetic/lib/libroscpp.so
#23 0x00007de5aa8e743b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#24 0x00007de5ab407609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#25 0x00007de5ab055353 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
(gdb) 

@rhaschke
Copy link
Copy Markdown
Contributor

The workflow seems to require that plan.planning_scene_ be modified with diff(scene_diff) but still updated by the planning scene monitor.

Do you know where/how these PSM updates are used?

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

riv-mjohnson commented Jan 28, 2025

Do you know where/how these PSM updates are used?

If I'm honest, I don't completely follow what plan_execution.cpp is doing. But passing a copy of the planning scene around rather than using plan.planning_scene_ broke a lot of tests.

My best guess is that planAndExecute is relying on plan.planning_scene_ updating, to track execution progress, so freezing it via copyPlanningScene breaks things. But this is only a guess.

@rhaschke
Copy link
Copy Markdown
Contributor

Could you pick an exemplary test and report its error output here?

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

riv-mjohnson commented Jan 28, 2025

Could you pick an exemplary test and report its error output here?

https://github.com/moveit/moveit/actions/runs/12993254139/job/36235033434 fails with

  You can start planning now!
  
  
  [ INFO] [1737995255.254085707]: Planning attempt 1 of at most 1
  [ INFO] [1737995255.259109660]: Added plan for pipeline 'pick'. Queue is now of size 1
  [ INFO] [1737995255.268943934]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
  [ INFO] [1737995255.269436627]: panda_arm/panda_arm: Starting planning with 1 states already in datastructure
  [ INFO] [1737995255.282968187]: panda_arm/panda_arm: Created 7 states (3 start + 4 goal)
  [ INFO] [1737995255.283013542]: Solution found in 0.013681 seconds
  [ INFO] [1737995255.300124651]: SimpleSetup: Path simplification took 0.017033 seconds and changed from 5 to 2 states
  [ INFO] [1737995255.302003582]: Found successful manipulation plan!
  [ INFO] [1737995255.302289333]: Pickup planning completed after 0.047977 seconds
  [ INFO] [1737995255.354201555]: Fake execution of trajectory
  [ INFO] [1737995255.854523005]: Fake execution of trajectory
  [ INFO] [1737995256.354979800]: Fake execution of trajectory
  [ INFO] [1737995256.855233508]: Fake execution of trajectory
  [ INFO] [1737995257.355626854]: Found a contact between 'object' (type 'Object') and 'object' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.
  [ INFO] [1737995257.355675725]: Collision checking is considered complete (collision was found and 0 contacts are stored)
  [ INFO] [1737995257.355713376]: Upcoming trajectory component 'retreat' is invalid
  [ INFO] [1737995257.355751577]: Fake execution of trajectory
  [ INFO] [1737995257.364245115]: Found a contact between 'object' (type 'Object') and 'object' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.
  [ INFO] [1737995257.364280051]: Collision checking is considered complete (collision was found and 0 contacts are stored)
  [ INFO] [1737995257.364401899]: Stopping execution because the path to execute became invalid(probably the environment changed)
  [ WARN] [1737995257.364321979]: Trajectory component 'retreat' is invalid after scene update
  [ INFO] [1737995257.364455029]: Stopped trajectory execution.
  [ INFO] [1737995257.855914550]: Completed trajectory execution with status PREEMPTED ...
  [ INFO] [1737995257.856071771]: Waiting for a 2.000000 seconds before attempting a new plan ...
  [ INFO] [1737995259.856218312]: Done waiting
  [ WARN] [1737995259.856841203]: Fail: ABORTED: Solution found but the environment changed during execution and the path was aborted
  [Testcase: testmove_group_pick_place_test] ... ok
  
  [ROSTEST]-----------------------------------------------------------------------

 [moveit_ros_planning_interface.rosunit-move_group_pick_place_test/PickPlaceTest][FAILURE]
  /home/runner/work/moveit/moveit/.work/target_ws/src/moveit/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp:222
  Expected equality of these values:
    move_group_->pick("object", grasps)
      Which is: MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE
    moveit::core::MoveItErrorCode::SUCCESS
      Which is: 1
  --------------------------------------------------------------------------------
  
  
  SUMMARY
   * RESULT: FAIL
   * TESTS: 1
   * ERRORS: 0
   * FAILURES: 1

I tried to track back where this was coming from, but my grasp of the codebase is not good enough to follow it through.

@rhaschke
Copy link
Copy Markdown
Contributor

This looks like PS updates are processed where they shouldn't (and not the other way around as you assumed).

@v4hn
Copy link
Copy Markdown
Contributor

v4hn commented Jan 28, 2025

[ INFO] [1737995257.355626854]: Found a contact between 'object' (type 'Object') and 'object' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.

This is a classic error when the scene used for checking is not updated during attaching an object.

My best guess is that planAndExecute is relying on plan.planning_scene_ updating, to track execution progress, so freezing it via copyPlanningScene breaks things. But this is only a guess.

Exactly. c174715

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

Replacing the diff calls with clone fixes all the problems I've been seeing in my tests.

From my end, this now works as intended.

I'd rather not dive back into plan_execution.cpp: I don't think I understand the stack well enough to make changes without breaking things. I also get the impression it only locks the planning scene monitor briefly anyway.

@riv-mjohnson riv-mjohnson requested review from rhaschke and v4hn February 6, 2025 10:18
@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

@rhaschke @v4hn could I ask you to re-review this, so I can port it to Moveit2?

@rhaschke are you happy with my changes to SingleUnlock, or could you suggest how you want me to change it?

Copy link
Copy Markdown
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM. I reverted the change of visibility for SingleUnlock.

@riv-mjohnson
Copy link
Copy Markdown
Contributor Author

@v4hn could you re-review this when you get a moment?

Copy link
Copy Markdown
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

thanks for the polite ping. 🥇

This patch looks good and adds a use-case that was never considered before.
I commented with some nits, mainly about reducing future confusion.
Could you please rebase the branch, and optionally address the comments?
Assuming CI still passes then, I approve.

@riv-mjohnson riv-mjohnson force-pushed the publish_planning_scene_while_planning branch from 384e308 to c5f040e Compare February 20, 2025 13:38
@rhaschke rhaschke merged commit e461d34 into moveit:master Feb 20, 2025
6 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Updating rviz planning scene when monitored planning scene is locked

4 participants