Ask Your Question
0

How to use the planning scene monitor in C++

asked 2020-07-01 04:57:21 -0500

jeroendm gravatar image

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())
edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
1

answered 2020-07-06 03:29:05 -0500

jeroendm gravatar image

updated 2020-07-06 03:56:36 -0500

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.

edit flag offensive delete link more
0

answered 2020-07-09 05:30:03 -0500

munseng gravatar image

Hello jeroendm,

I did not use PlanningSceneMonitor but I use CurrentStateMonitor which subscribes to the joint_states topic to monitor the current state of the robot, because I want to do only self-collision checking of the robot when there is an update of the joint state in the planning scene. But I think this is similar using the PlanningSceneMonitor.

Here is how I use the CurrentStateMonitor and the addUpdateCallback() function (I refer to one example code online but I couldn't find it in google now):

void collision_checking(planning_scene_monitor::CurrentStateMonitor* csm, const sensor_msgs::JointStateConstPtr &)
{
  // called when there are joint state updates
  ROS_INFO("Received an update to the current joint state of the robot.");

  const robot_model::RobotModelConstPtr robot_model = csm->getRobotModel();
  robot_state::RobotStatePtr current_state = csm->getCurrentState();

  // collision checking function on the current state here ...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "robot_state_monitor");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>();
  std::shared_ptr<tf2_ros::TransformListener> tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer, nh);

  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  const robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

  // set up current state monitor
  planning_scene_monitor::CurrentStateMonitor csm(robot_model, tf_buffer);
  csm.startStateMonitor(const std::string & joint_states_topic = "joint_states");
  if (!csm.isActive())
  {
    ROS_ERROR("The state monitor is not started");
    return 1;
  }
  // Robot State Monitor -> Trigger Collision Checking
  csm.addUpdateCallback(boost::bind(&collision_checking, &csm, _1));

  ros::waitForShutdown();
}

I think the addUpdateCallback() is somewhat a subscriber callback function (correct me if I am wrong). But I am now not sure how to publish to a topic inside the addUpdateCallback() function as the standard approach (defining class) not working here.

edit flag offensive delete link more

Comments

Good idea to check whether the connection was successful with csm.isActive()! The PlanningSceneMonitor does not have this, but I can use getMonitoredTopics() to do the same.

jeroendm gravatar image jeroendm  ( 2020-07-10 10:02:03 -0500 )edit
0

answered 2020-07-05 10:45:26 -0500

Omid gravatar image

updated 2020-07-05 10:46:28 -0500

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-07-01 04:57:21 -0500

Seen: 54 times

Last updated: Jul 06