How to use the planning scene monitor in C++
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())