accounting for scene updates and obstacles

edit

Hi,

I have a node that computes simple motions using Move Group interface. I can see these motions within rviz. Following some of the MoveIt tutorials, I can create motions towards a pose goal, or a joint-space goal, as well as, add collision objects to the scene. Concerning collision objects (obstacles) I understand that the following call

planning_scene_interface.applyCollisionObjects

lets planning scene to account for the added obstacles, and hence the planner (OMPL-based) will compute motion plan taking into account the obstacles. The collision objects are static. This means that, if we add a collision object after we have invoked the motion plan computation / execution, this collision object will not be accounted for. We could see this in rviz and the robotic arm moving through it.

I have created another node, that creates obstacle objects (cylinder, box etc) and publishes on the topic

"collision_object"

Running this node in a separate terminal, I was able to see the obstacles being added.

My first question,

rviz does not automatically show the new collision object that were published. Instead we have to press Add->moveit_ros_visualization->planning_scene on rviz window. Is this the usual procedure to show published obstacles in the simulator?

At this moment, I want to have the functionality such that, once the motion has begun, I could know when a new obstacle has appeared, and if the original trajectory would be invalid on account of this new obstacle, then, I could issue move_group.stop. Following different threads, I have created another node where I have an instance of PlanningSceneMonitor. Here is a code snippet.

    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ptr;
      planning_scene_monitor_ptr = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
      planning_scene_monitor_ptr->requestPlanningSceneState("/get_planning_scene");
      planning_scene_monitor_ptr->startSceneMonitor("/move_group/monitored_planning_scene");
      planning_scene_monitor::LockedPlanningSceneRO ps(planning_scene_monitor_ptr);

ros::Rate loop_rate(1);

  while (ros::ok()) {

    bool valid = ps->isPathValid(my_path_msg.init_state, my_path_msg.traj, "manipulator");

    if (!valid) {
      ROS_INFO("Invalid path");
    } else {
      ROS_INFO("valid********** path");

    }
   loop_rate.sleep();
  }

where, I have a custom path message that store information about the trajectory that was computed following a successful call to move_group.plan(my_plan). My custom path message

#MsgPath.msg
moveit_msgs/RobotState init_state
moveit_msgs/RobotTrajectory traj

move_group will publish this message on a specific topic and the above node subscribes to it so that it can check if the computed trajectory remain valid as the scene updates occur. However, this node always reports the trajectory to be valid, as well as, it only reports one collision object and no more even when I add other collision objects.

Any guidance on how should I be checking the scene updates.

thanks,

zahid