How to debug message filter problems? Filter dropping message for unknown reason
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.
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 ...
Off-topic perhaps, but:
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 aflange
link.flange
would be where you'd connect models of your EEF, nottool0
. See also Coordinate Frames for Serial Industrial Manipulators: flange (not an official REP, but it does codify current best-practices).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.