How to debug message filter problems? Filter dropping message for unknown reason

asked 2023-07-13 09:16:14 -0500

NotARobot gravatar image

I have a simulated camera in Gazebo and want to use the point cloud of the camera for MoveIt2s Octomap. The points are getting published on the topic Canon_Powershot_G7_MKIII/points with the reference frame being Canon_Powershot_G7_MKIII_virtual_frame_link.

Example message:

header:
  stamp:
    sec: 632
    nanosec: 175000000
  frame_id: Canon_Powershot_G7_MKIII_virtual_frame_link
height: 1
width: 307200
fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
- name: rgb
  offset: 16
  datatype: 7
  count: 1
is_bigendian: false
point_step: 32
row_step: 9830400
data:
- 82
- '...'
is_dense: true

The TF tree is also set properly. TF tree

The octomap_updater_config of the move_group_node is:

# Camera setup
    camera_configuration = load_yaml(
        "modproft_camera_description", "config/canon_powershot_g7_mk3.yaml")

    if prefix:
        # camera_name = ''.join([prefix, str(camera_configuration["camera"]["sensor"]["name"])])
        camera_name = str(camera_configuration["camera"]["sensor"]["name"])
    else:
        camera_name = str(camera_configuration["camera"]["sensor"]["name"])

    octomap_updater_config = {
        "sensors": [camera_name],
        camera_name: {
            "sensor_plugin": "occupancy_map_monitor/PointCloudOctomapUpdater",
            "point_cloud_topic": "/{}".format('/'.join([camera_name, "points"])),
            "max_range": 5.0,
            "point_subsample": 1,
            "padding_offset": 0.1,
            "padding_scale": 1.0,
            "max_update_rate": 60.0,
            "filtered_cloud_topic": "/{}".format('/'.join([camera_name, "filtered_cloud"]))
        }
    }
camera_link_virtual = "{}{}".format(camera_name, '_virtual_frame_link')

    octomap_config = {"octomap_frame": camera_link_virtual,
                      "octomap_resolution": 0.05,
                      "max_range": 5.0}

    # Start the actual move_group node/action server
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            ompl_planning_pipeline_config,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
            octomap_config,
            octomap_updater_config,
            {"use_sim_time": use_sim_time,
             "verbose": False}
        ],
        arguments=['--ros-args', '--log-level', 'debug']
    )

tf_monitor also yields a result:

RESULTS: for world to Canon_Powershot_G7_MKIII_virtual_frame_link
Chain is: world -> robot_table_link -> robot_table_tabletop_link -> base_link -> base_link_inertia -> shoulder_link -> upper_arm_link -> forearm_link -> wrist_1_link -> wrist_2_link -> wrist_3_link -> flange -> tool0 -> Canon_Powershot_G7_MKIII_link -> Canon_Powershot_G7_MKIII_virtual_frame_link
Net delay     avg = 0.00105632: max = 0.00300813

Frames:
Frame: Canon_Powershot_G7_MKIII_link, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: Canon_Powershot_G7_MKIII_virtual_frame_link, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: base_link, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: base_link_inertia, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: flange, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: forearm_link, published by <no authority available>, Average Delay: 0.000539706, Max Delay: 0.0022819
Frame: robot_table_link, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: robot_table_tabletop_link, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: shoulder_link, published by <no authority available>, Average Delay: 0.000544725, Max Delay: 0.00228477
Frame: tool0, published by <no authority available>, Average Delay: 1.68924e+09, Max Delay: 1.68924e+09
Frame: upper_arm_link, published by <no authority available>, Average Delay: 0.000542384, Max Delay: 0.00228357
Frame: wrist_1_link, published by <no authority available>, Average Delay: 0.000546804, Max Delay: 0.00228572
Frame: wrist_2_link, published by <no authority available>, Average Delay: 0.000548919, Max Delay: 0.00228667
Frame: wrist_3_link, published by <no authority available>, Average Delay: 0.000550851, Max Delay: 0.00228739

All Broadcasters:
Node: <no authority available> 581.328 Hz, Average ...
(more)
edit retag flag offensive close merge delete

Comments

1

Off-topic perhaps, but:

The TF tree is also set properly.

while it's all connected (that's good), it appears you've mounted your camera and "adapter manipulator" links to tool0. As the robot is probably one of the UR models, it comes with a flange link. flange would be where you'd connect models of your EEF, not tool0. See also Coordinate Frames for Serial Industrial Manipulators: flange (not an official REP, but it does codify current best-practices).

gvdhoorn gravatar image gvdhoorn  ( 2023-07-15 10:02:02 -0500 )edit

Thank you for that caveat! I'll have a look on the best-practices. ATM the robots URDF is not in the final configuration, since some of the mounts for the devices on the arm have to be constructed and printed, so I've done just the connection to tool0. I'm developing in parallel on a real robot, while testing path planning etc. in Gazebo.

NotARobot gravatar image NotARobot  ( 2023-07-17 01:24:13 -0500 )edit