How to use the planning scene monitor in C++

edit

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.

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.