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
• ROS2 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. Specifically, I am using the launch file from here, i.e., ros2 launch moveit2_tutorials move_group.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 this message I get when launching the robot with move_group.launch.py:

[move_group.moveit.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates

• Through this similar question I found this launch file (which includes octomap configurations when starting up move_group_node). I tried to implement something similar in my move_group.launch.py file but without success. Here is the code of move_group.launch.py (unchanged from MoveIt's example, the comments are what I have unsuccessfully added trying to add the point cloud to OccupancyMapMonitor):

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(file_path="config/panda.urdf.xacro")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .to_moveit_configs()
    )
    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            moveit_config.to_dict(),
            # {
            #     'octomap_resolution': 0.05,
            #     'monitor_octomap': True,
            # },
        ],
    )

    # RViz
    rviz_config_file = (
        get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz"
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description],
    )

    # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory("moveit_resources_panda_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_path],
        remappings=[
            ("/controller_manager/robot_description", "/robot_description"),
        ],
        output="both",
    )

    # Load controllers
    load_controllers = []
    for controller in [
        "panda_arm_controller",
        "panda_hand_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]
        
    # # Octomap Monitor node
    # octomap_monitor_node = Node(
    #     package="moveit_ros_perception",
    #     executable="occupancy_map_server",
    #     output="screen",
    #     parameters=[
    #         {
    #             'sensor_plugin': 'occupancy_map_monitor/PointCloudOctomapUpdater',
    #             'max_range': 5.0,  
    #             'point_cloud_topic': '/pcl',  
    #             'frame_id': 'world',
    #         }
    #     ],
    # )

    return LaunchDescription(
        [
            rviz_node,
            static_tf,
            robot_state_publisher,
            run_move_group_node,
            ros2_control_node,
            # octomap_monitor_node,
        ]
        + load_controllers
    )

When trying to launch the robot with move_group.launch.py with the now commented-out parts, I got this error, which I feel opens up a whole new set of issues for me to resolve:

Caught exception in launch (see debug for traceback): package 'moveit_ros_perception' found at '/home/user/ws_moveit/install/moveit_ros_perception', but libexec directory '/home/user/ws_moveit/install/moveit_ros_perception/lib/moveit_ros_perception' does not exist

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