Crash in MeshFilter/gl_renderer while runtime

Description

Hello,

I have found a bug in https://github.com/moveit/moveit2/blob/6454a073d27c4eeb23d4f748364b22f5b072cf92/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp#L390 , which the at method of std::map is misused.

The following line is used in moveit2

s_context.at(thread_id) = std::pair<unsigned, GLuint>(1, 0);

But it gives rise to crash the some part of moveit2 while running demo. It is indicated that in this page, at method throws out of range error for this situations:

[std::out_of_range](https://en.cppreference.com/w/cpp/error/out_of_range) if the container does not have:
1,2) an element with the specified key.
3,4) the specified element, that is, if find(x) == end() is true.

But it is missed in this lines.

https://github.com/moveit/moveit2/blob/6454a073d27c4eeb23d4f748364b22f5b072cf92/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp#L385-L390

I have a PR in order to fix this bug.

Your environment

ROS Distro: Iron
OS Version: Ubuntu 22.04
Source from main branch
RMW : Fast DDS

Steps to reproduce

Follow the get started documentation and then launch this file with this configuration after creating a new package named hello_moveit. Don't forget to build the relevant package again with this command.

colcon build --mixin debug --packages-select moveit_ros_perception

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():

    # Command-line arguments
    rviz_config_arg = DeclareLaunchArgument(
        "rviz_config",
        default_value="moveit.rviz",
        description="RViz configuration file",
    )

    db_arg = DeclareLaunchArgument(
        "db", default_value="False", description="Database flag"
    )

    ros2_control_hardware_type = DeclareLaunchArgument(
        "ros2_control_hardware_type",
        default_value="mock_components",
        description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]",
    )

    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(
            file_path="config/panda.urdf.xacro",
            mappings={
                "ros2_control_hardware_type": LaunchConfiguration(
                    "ros2_control_hardware_type"
                )
            },
        )
        .robot_description_semantic(file_path="config/panda.srdf")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .planning_pipelines(
            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
        )
        .sensors_3d(file_path = "/home/cihat/ws_moveit/src/hello_moveit/config/sensors_3d.yaml")
        # .sensors_3d(file_path = "config/sensors_kinect_pointcloud.yaml")
        .to_moveit_configs()
    )

    # Start the actual move_group node/action server
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config.to_dict()],
        arguments=["--ros-args", "--log-level", "info"],
        prefix=["xterm -e gdb -ex run --args"]
    )

    # RViz
    rviz_base = LaunchConfiguration("rviz_config")
    rviz_config = PathJoinSubstitution(
        [FindPackageShare("moveit_resources_panda_moveit_config"), "launch", rviz_base]
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.planning_pipelines,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
        ],
    )

    # Static TF
    static_tf_node = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "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="screen",
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    panda_arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["panda_arm_controller", "-c", "/controller_manager"],
    )

    panda_hand_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["panda_hand_controller", "-c", "/controller_manager"],
    )

    # Warehouse mongodb server
    db_config = LaunchConfiguration("db")
    mongodb_server_node = Node(
        package="warehouse_ros_mongo",
        executable="mongo_wrapper_ros.py",
        parameters=[
            {"warehouse_port": 33829},
            {"warehouse_host": "localhost"},
            {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
        ],
        output="screen",
        condition=IfCondition(db_config),
    )

    return LaunchDescription(
        [
            rviz_config_arg,
            db_arg,
            ros2_control_hardware_type,
            rviz_node,
            static_tf_node,
            robot_state_publisher,
            move_group_node,
            ros2_control_node,
            joint_state_broadcaster_spawner,
            panda_arm_controller_spawner,
            panda_hand_controller_spawner,
            mongodb_server_node,
        ]
    )

sensor_3d.yaml

sensors:
  - kinect_pointcloud
  - kinect_depthimage
kinect_pointcloud:
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /head_mount_kinect/depth_registered/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud
kinect_depthimage:
    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
    image_topic: /head_mount_kinect/depth_registered/image_raw
    queue_size: 5
    near_clipping_plane_distance: 0.3
    far_clipping_plane_distance: 5.0
    shadow_threshold: 0.2
    padding_scale: 4.0
    padding_offset: 0.03
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

Change the path in .sensors_3d(file_path = "/home/cihat/ws_moveit/src/hello_moveit/config/sensors_3d.yaml") by you.

And finally run this command

source /opt/ros/iron/setup.bash
source install/setup.bash
ros2 launch ros2 launch hello_moveit demo.launch.py --debug

Expected behaviour

It should not be crashed while running demo with sensor_3d option.

Actual behaviour

It crashed while running demo with sensor_3d option.

Backtrace or Console output

Backtrace Output : https://gist.github.com/CihatAltiparmak/c4e82c7bfb0650e33716edadaac6a93d#file-backtrace_outputs-txt

Console Output: https://gist.github.com/CihatAltiparmak/c4e82c7bfb0650e33716edadaac6a93d#file-error_when_moved_group_node_is_ran

moveit / moveit2