accounting for scene updates and obstacles

asked 2021-01-24 11:17:27 -0500

zahid990170 gravatar image


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


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


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::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");


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

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.



edit retag flag offensive close merge delete


There is an experimental feature for replanning when an obstacle moves into the robot's path that you could try and/or improve. I am surprised that Rviz does not show the obstacles in the planning scene until you add a planning scene display though. What does the "Scene Objects" tab in the Motion Planning plugin say?

fvd gravatar image fvd  ( 2021-01-27 18:52:36 -0500 )edit

thanks @fvd, this brings me to the related question. Motion Planning plugin "Scene Objects" tab lists the name of the collision object. But, I have to manually Add this plugin (Motion Planning plugin) like I did for the planning scene. However, once, I add this, it loads the scene objects (obstacles, collision objects) as well, and the mentioned tab lists their name. How can, I ensure that when I issue my launch command, rviz already loads these plugins.

zahid990170 gravatar image zahid990170  ( 2021-01-28 05:16:19 -0500 )edit

Hi @zahid990170, if I understood correctly, you need to 'automatically' add the Motion Planning plugin in the rviz. If that is the case, you can edit the moveit.rviz file under your {PACKAGE}_moveit_config/launch directory. Find this setting there MotionPlanning: collapsed: false Then, every time you launch your node (of course the moveit.rviz needs to be called from the related launch file), it will by default load the plugin.

eco gravatar image eco  ( 2021-04-29 09:09:46 -0500 )edit