ROS2/MoveIt/RViz: PLY file added to collision environment as point cloud via OctoMap, but motion planning does not fully work

Question

I am trying to read a PLY file and add it to the collision environment so that when motion planning, it is seen as a collision object.

My setup:

• Ubuntu 22.04
• ROS 2 Humble

My code/workspace: https://github.com/wallelikesjinglebells/ws_pcl

Here's what I'm doing:

1. My starting point is a PLY file, and I am successfully reading and publishing the file as a PointCloud2 message (package: pcl_publisher). Used command: ros2 run pcl_publisher pcl_publisher_node ~/ws_pcl/src/pcl_publisher/resource/TrialWall_PC.ply
2. I am taking the PointCloud2 message and remapping octomap_server_node's subscription to /pcl, my point cloud topic. Used command: ros2 run octomap_server octomap_server_node --ros-args -p frame_id:=world -r /cloud_in:=/pcl
3. I am taking the data from octomap_server_node and publishing it to the planning scene (package: octomap_publisher). Used command: ros2 run octomap_publisher octomap_publisher_node
4. I am trying out my code on the demo panda robot from the MoveIt examples. For this purpose, I copied the corresponding folders into my workspace. Used command: ros2 launch moveit_resources_panda_moveit_config demo.launch.py.

Up until this point, everything is working the way I want it to: The PLY file is correctly displayed in RViz as a collision object in my planning environment, and when I manually move the robot, the links that are colliding with the wall are marked red:

However, when I set a start (green) and goal (orange) state and hit 'Plan' in RViz, the robot moves through the collision object (the transparent robot body):

My trains of thought following this unwanted result:

• I first thought that the OctoMap was not updated frequently enough, but commands such as ros2 topic echo /monitored_planning_scene showed me that the topic does frequently receive the published data from octomap_publisher.
• I suspect my problem might have something to do with these messages I get when launching the robot with demo.launch.py:

[move_group-4] [WARN] [1726576043.279205138] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-4] [ERROR] [1726576043.279247810] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates

• When I run ros2 service call /get_planning_scene moveit_msgs/srv/GetPlanningScene "{}", I get the following result. Towards the end, I found collision_objects=[] and frame_id=''), binary=False, id='', resolution=0.0, data=[]. I believe these should not be empty; the first is related to the whole PlanningScene and the second to Octomap.

requester: making request: moveit_msgs.srv.GetPlanningScene_Request(components=moveit_msgs.msg.PlanningSceneComponents(components=0))

response:
moveit_msgs.srv.GetPlanningScene_Response(scene=moveit_msgs.msg.PlanningScene(name='(noname)+', robot_state=moveit_msgs.msg.RobotState(joint_state=sensor_msgs.msg.JointState(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='world'), name=['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1', 'panda_finger_joint2'], position=[0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785, 0.0, 0.0], velocity=[], effort=[]), multi_dof_joint_state=sensor_msgs.msg.MultiDOFJointState(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='world'), joint_names=['virtual_joint'], transforms=[geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))], twist=[], wrench=[]), attached_collision_objects=[], is_diff=False), robot_model_name='panda', fixed_frame_transforms=[geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='world'), child_frame_id='world', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)))], allowed_collision_matrix=moveit_msgs.msg.AllowedCollisionMatrix(entry_names=['panda_hand', 'panda_leftfinger', 'panda_link0', 'panda_link1', 'panda_link2', 'panda_link3', 'panda_link4', 'panda_link5', 'panda_link6', 'panda_link7', 'panda_rightfinger'], entry_values=[moveit_msgs.msg.AllowedCollisionEntry(enabled=[False, True, False, False, False, True, True, False, True, True, True]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[True, False, False, False, False, True, True, False, True, True, True]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[False, False, False, True, True, True, True, False, False, False, False]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[False, False, True, False, True, True, True, False, False, False, False]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[False, False, True, True, False, True, True, False, True, False, False]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[True, True, True, True, True, False, True, True, True, True, True]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[True, True, True, True, True, True, False, True, True, True, True]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[False, False, False, False, False, True, True, False, True, False, False]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[True, True, False, False, True, True, True, True, False, True, True]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[True, True, False, False, False, True, True, False, True, False, True]), moveit_msgs.msg.AllowedCollisionEntry(enabled=[True, True, False, False, False, True, True, False, True, True, False])], default_entry_names=[], default_entry_values=[]), link_padding=[moveit_msgs.msg.LinkPadding(link_name='panda_hand', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_leftfinger', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link0', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link1', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link2', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link3', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link4', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link5', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link6', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_link7', padding=0.0), moveit_msgs.msg.LinkPadding(link_name='panda_rightfinger', padding=0.0)], link_scale=[moveit_msgs.msg.LinkScale(link_name='panda_hand', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_leftfinger', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link0', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link1', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link2', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link3', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link4', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link5', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link6', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_link7', scale=1.0), moveit_msgs.msg.LinkScale(link_name='panda_rightfinger', scale=1.0)], object_colors=[], world=moveit_msgs.msg.PlanningSceneWorld(collision_objects=[], octomap=octomap_msgs.msg.OctomapWithPose(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='world'), origin=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), octomap=octomap_msgs.msg.Octomap(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), binary=False, id='', resolution=0.0, data=[]))), is_diff=False))

I believe my overall problem might be caused by an issue that only loads the OctoMap into RViz, i.e., it is visualized and RViz can help with detecting collisions between the robot and the obstacle when manually moving. However, the OctoMap is not loaded into the planning scene in a way that the robot takes notice of it, which makes motion planning with the OctoMap obstacle impossible.

If you have not figured it out by now, I am quite a newbie to everything ROS 2-related, and I would be eternally grateful if someone could help me fix my problem.

Answer 1

I found a dirty fix: Add (and remove) a random scene object (under the "Scene Objects" tab in the MotionPlanning window). The robot avoids the obstacle. Found here.