0
$\begingroup$

Rosanswers logo

What is the correct way to create a MoveIt PlanningSceneMonitor in C++ for the default planning scene managed by the move group node? (With the default move group node, I mean the one launched by the demo.launch file of a MoveIt configuration package for a robot.)

In the planning_scene tutorial is written:

This is, however, not the recommended way to instantiate a PlanningScene. The PlanningSceneMonitor is the recommended method to create and maintain the current planning scene

However, the planning_scene_monitor tutorial does not contain a concrete example of how to use the PlanningSceneMonitor class. (This is in the latest branch of the tutorials, the tutorial does not exist yet in melodic.)

I found an example in moveit_visual_tools, but there is a lot going on there that I don't understand. As far as I understand, it creates a separate one, not connected to the move group node.

  • What monitors should I start to enable collision checking between the robot and the scene?
  • When should I call the update method?

As done in these lines of code:

planning_scene_monitor::LockedPlanningSceneRW scene(getPlanningSceneMonitor());
scene->getCurrentStateNonConst().update();  // TODO: remove hack to prevent bad transforms

More context for the questions

For the development of a new planner, I'm using the planner through the plugin interface. I have an instance of planning_interface::PlannerManager, called planner_instance which can then be used for motion planning in the following way:

planning_interface::MotionPlanRequest req; // fill in this request ...
planning_interface::MotionPlanResponse res;

// somehow get access to the planning scene
planning_scene = ...

auto context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
bool success = context->solve(res);

I have difficulty understanding how to use the planning scene from the move_group node, or creating one myself. Currently, I can add collision objects using moveit_visual_tools.

moveit_visual_tools::MoveItVisualTools mvt("/base_link", "/visualization_marker_array");
mvt.publishCollisionCuboid(box_pose, 0.3, 0.1, 1.0, "box_1");
mvt.trigger();

auto planning_scene_monitor = mvt.getPlanningSceneMonitor();
planning_scene_monitor::LockedPlanningSceneRW planning_scene(planning_scene_monitor);
// (clean alternative for planning_scene_monitor.getPlanningScene())

Originally posted by jeroendm on ROS Answers with karma: 168 on 2020-07-01

Post score: 2

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

visual tools is only for visualization purpose I believe. To add or remove collision objects you need to use PlanningSceneInterface, methods like addCollisionObjects and removeCollisionObjects

For collision checking you can use:

planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor)->checkCollision(collision_request, collsion_result)

In general, I would say:

  • Use PlanningSceneInterface to add/remove objects to the scene.
  • Use PlanningSceneMonitor to keep track of the updated planning scene. LockedPlanningSceneRO gives access to the up-to-date planning scene instance. From there you can do things like collision checking or other methods from planning scene.
  • Use MoveGroupInterface to set targets, planners and do the actual motion planning

About the PlanningSceneMonitor tutorial, I agree it needs a comprehensive example so referring from PlanningScene tutorial makes sense.


Originally posted by Omid with karma: 21 on 2020-07-05

This answer was NOT ACCEPTED on the original site

Post score: 2

$\endgroup$
0
$\begingroup$

Rosanswers logo

I understand the recommendations in Omid's answer, but it does not contain instructions to create the PlanningSceneMonitor yet, so that's why added this answer.

To create a planning scene monitor that is up to date with the planning scene maintained by the move_group node, you can do the following:

auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
psm->startSceneMonitor("/move_group/monitored_planning_scene");

The planning scene monitor will subscribe to the ROS topic /move_group/monitored_planning_scene published by the move_group node.

However, this does not take into account collision objects that were published before the node was launched. Getting the planning scene monitor psm up to date with the existing scene is achieved through a ROS service request to a service /get_planning_scene published by the move_group node.

bool success = psm->requestPlanningSceneState("/get_planning_scene");

This leaves us with a planning scene monitor that allows us access to an up to date PlanningScenePtr. I understand that this pointer should not be used in "normal" MoveIt use.

(There are many more ways to interact with the planning scene monitor, but this is the one I wanted to understand.)

Edit: I created a minimal working example here.


Originally posted by jeroendm with karma: 168 on 2020-07-06

This answer was NOT ACCEPTED on the original site

Post score: 8

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.