The point cloud data and the octomap coordinates do not coincide, and the octomap coordinate system is incorrect

[JiangShangJiu]

I passed the data from the depth camera in the ignition gazebo into the moviet, but the octomap coordinates are not normal, but the point cloud and depth map are normal. May I ask where I wrote it wrong?

sensors_3d.yaml
`sensors:

• camera_1_pointcloud
• camera_2_depth_image
  camera_1_pointcloud:
  sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: /rgbd_camera/points
  max_range: 5.0
  point_subsample: 1
  padding_offset: 0.1
  padding_scale: 1.0
  max_update_rate: 30.0
  filtered_cloud_topic: /camera_1/filtered_points

camera_2_depth_image:
sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
image_topic: /rgbd_camera/depth_image
queue_size: 5
near_clipping_plane_distance: 0.3
far_clipping_plane_distance: 5.0
shadow_threshold: 0.2
padding_scale: 1.0
max_update_rate: 30.0
filtered_cloud_topic: /camera_2/filtered_points
**moveit_rviz.launch.py**

def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)

try:
    with open(absolute_file_path, 'r') as file:
        return yaml.safe_load(file)
except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
    return None

def generate_launch_description():

# Command-line arguments

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

# planning_context

franka_semantic_xacro_file = os.path.join(
    get_package_share_directory('franka_fr3_moveit_config'),
    'srdf',
    'fr3_arm.srdf.xacro'
)

robot_description_semantic_config = Command(
    [FindExecutable(name='xacro'), ' ',
     franka_semantic_xacro_file, ' hand:=true']
)

robot_description_semantic = {'robot_description_semantic': ParameterValue(
    robot_description_semantic_config, value_type=str)}

kinematics_yaml = load_yaml(
    'franka_fr3_moveit_config', 'config/kinematics.yaml'
)
sensors_yaml = load_yaml(
    'franka_fr3_moveit_config', 'config/sensors_3d.yaml'
)

# Planning Functionality
ompl_planning_pipeline_config = {
    'move_group': {
        'planning_plugin': 'ompl_interface/OMPLPlanner',
        'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization '
                            'default_planner_request_adapters/ResolveConstraintFrames '
                            'default_planner_request_adapters/FixWorkspaceBounds '
                            'default_planner_request_adapters/FixStartStateBounds '
                            'default_planner_request_adapters/FixStartStateCollision '
                            'default_planner_request_adapters/FixStartStatePathConstraints',
        'start_state_max_bounds_error': 0.1,
    }
}
ompl_planning_yaml = load_yaml(
    'franka_fr3_moveit_config', 'config/ompl_planning.yaml'
)
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)

# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
    'franka_fr3_moveit_config', 'config/fr3_controllers.yaml'
)
moveit_controllers = {
    'moveit_simple_controller_manager': moveit_simple_controllers_yaml,
    'moveit_controller_manager': 'moveit_simple_controller_manager'
                                 '/MoveItSimpleControllerManager',
}

trajectory_execution = {
    'moveit_manage_controllers': True,
    'trajectory_execution.allowed_execution_duration_scaling': 10.2,
    'trajectory_execution.allowed_goal_duration_margin': 0.5,
    'trajectory_execution.allowed_start_tolerance': 0.01,
}

planning_scene_monitor_parameters = {
    'publish_planning_scene': True,
    'publish_geometry_updates': True,
    'publish_state_updates': True,
    'publish_transforms_updates': True,
}

# Start the actual move_group node/action server  启动moveit group节点
run_move_group_node = Node(
    package='moveit_ros_move_group',
    executable='move_group',
    output='screen',
    parameters=[
        robot_description_semantic,
        kinematics_yaml,
        ompl_planning_pipeline_config,
        trajectory_execution,
        moveit_controllers,
        planning_scene_monitor_parameters,
        {'use_sim_time': True},
        sensors_yaml,
    ],
)

# RViz
rviz_base = os.path.join(get_package_share_directory(
    'panda_gazebo'), 'rviz')
rviz_full_config = os.path.join(rviz_base, 'moveit2.rviz')

rviz_node = Node(
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='log',
    arguments=['-d', rviz_full_config],
    parameters=[
        robot_description_semantic,
        ompl_planning_pipeline_config,
        kinematics_yaml,
        sensors_yaml,
    ],
)


return LaunchDescription(
    [
     db_arg,
     rviz_node,
     run_move_group_node,
     ]
)

`

[mikeferguson]

Please provide the information that is requested in the bug report ticket - most importantly - what version of ROS / MoveIt - debs or source? If Source, what branch.

(I'm going to note that I have been using the main branch built against both Iron/Jazzy and not had this issue with the octomap)

[JiangShangJiu]

Thank you for replying so quickly
ROS2: Humble
MoveIt also is the Humble version
My guess is that I loaded the depth map and point cloud data of an RGBD sensor at the same time and passed it to moveit to generate an octomap. The octomap shown in my image is generated from the data of the depth map. I can't generate an octomap when I'm using point cloud data alone. I'm not sure if the octomap generated from the depth map data is correct.

In addition, I am using the simulation environment Ignition Gazebo.


and the sensor_name="d435"

[Danilrivero]

Seems like an error related to the sensor used from Ignition, this PR may contain helpful information for you:

gazebosim/gz-sensors#458

The depth and pcd are in a frame_id that you do not expect and moreover generating an octomap in the wrong frame. Not sure if this what's going in Ignition though.

May be helpful to setup the octomap frame_id as well, for example:

sensors:
  - default_sensor
default_sensor:
  filtered_cloud_topic: filtered_cloud
  max_range: 3.0
  max_update_rate: 1.0
  padding_offset: 0.01
  padding_scale: 0.01
  point_cloud_topic: /camera/depth/color/points
  point_subsample: 1
  sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
octomap_frame: {frame_here}

[JiangShangJiu]

Thank you very much for your answer, but I looked at the link you gave and tried the method you gave, but still can't solve it. I found that the point cloud data generated from the depth image in RVIZ2 was not the same as the point cloud data read directly from the Ignition gazabo, and I think there must have been some inconsistencies.

Have you solved this problem yet

[JorgePH]

It is related to this link too:
gazebosim/gz-sensors#29

You can try to choose between the depth image or the pointcloud and stick to one of them (setting the frame so the data is consistent). The sensors3d config supports pointclouds and depth images too.