Planners ignore the obstacles wrongly -- How to publish obstacles into the planning scene

[Pittmann-XIE]

Description

I added obstacles by publishing obstacle A to the "monitored_planning_scene." I can see them in RViz2. However, when I use the MoveIt panel in RViz for motion planning, the planner can still successfully find a trajectory, even though the target pose and the trajectory collide with the obstacles.

I then discovered that I need to manually publish obstacle A in the "Motion Planning" panel in RViz; only then does the planner consider the obstacles.

Unfortunately, MoveIt for Humble does not support the Python API. If I'm mistaken, please let me know.

Your Environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04
• Build Type: Source build
• Source Repository: git clone https://github.com/moveit/moveit2.git -b $ROS_DISTRO

Steps to Reproduce

1. Download the tutorials from MoveIt Tutorials.
2. Create another ROS2 package called "add_obstacles":

   import rclpy
   from rclpy.node import Node
   from moveit_msgs.msg import CollisionObject, PlanningScene
   from shape_msgs.msg import SolidPrimitive
   from geometry_msgs.msg import Pose
   from icecream import ic
   
   class AddObstacles(Node):
       def __init__(self):
           super().__init__("add_obstacles")
           self.publisher = self.create_publisher(PlanningScene, "/monitored_planning_scene", 10)
           self.timer = self.create_timer(1.0, self.add_objects)
   
       def add_objects(self):
           # Create first collision object (A)
           object_A = CollisionObject()
           object_A.id = "A"
           object_A.header.frame_id = "world"
   
           A_primitive = SolidPrimitive()
           A_primitive.type = SolidPrimitive.BOX
           A_primitive.dimensions = [0.6, 0.03, 0.6]
   
           A_pose = Pose()
           A_pose.position.x = 0.0
           A_pose.position.y = 0.20
           A_pose.position.z = 0.0
           A_pose.orientation.w = 1.0
   
           object_A.primitives.append(A_primitive)
           object_A.primitive_poses.append(A_pose)
           object_A.operation = CollisionObject.ADD
   
           # Create PlanningScene and add the collision object
           planning_scene = PlanningScene()
           planning_scene.world.collision_objects.append(object_A)
           planning_scene.is_diff = True
   
           self.publisher.publish(planning_scene)
           ic("Object A has been added.")
   
   def main(args=None):
       rclpy.init(args=args)
       node = AddObstacles()
       rclpy.spin(node)
       node.destroy_node()
       rclpy.shutdown()
   
   if __name__ == "__main__":
       main()
   

3. Build and source the setup.bash.
4. Run ros2 launch moveit2_tutorials demo.launch.py.
5. Run ros2 run add_obstacles add_obstacles.
6. Perform motion planning in RViz2: we can see the newly added obstacles, but the planner doesn't take them into consideration. This is the problem.
7. Go to the "Scene Objects" sub-panel in the Motion Planning panel of RViz, check obstacle A, link it to "link0," and publish.
8. Repeat step 6; now the planner works properly and considers the obstacles.

Expected Behavior

The MoveIt motion planning panel should return "Failed" after planning when I publish obstacles into the planning scene without manually adding them in RViz.

[rhaschke]

You are publishing your obstacles to the wrong topic: There are two planning scene topics, planning_scene and monitored_planning_scene. The first one, planning_scene, is the one the move_group node's PlanningSceneMonitor actually listens to. The latter one is used to publish the monitored scene, e.g. to rviz (which runs its own PSM).
For this reason, you can see the collision objects in rviz, although they are not considered by the move_group node.

As this causes confusion for many users, even experts (moveit/moveit2#1042), the corresponding doc should be improved: PR #983

[Pittmann-XIE]

Thank you very much.