How to change sensor frame of PointCloudOctomapUpdater?

asked 2019-08-26 05:15:21 -0500

Atle gravatar image

System: Ubuntu 16.04 / ROS Kinetic

Hi. I have a large setup consisting of multiple industrial robots in a lab mapped by several ROS sensor nodes. The sensor nodes filter, crop, transpose, and compress pointclouds. These compressed streams are decompressed to pcl2 on a master ROS-node. All decompressed pointclouds are showing correctly in RViz and are aligned in a common world-frame.

This leads to my question. To use all point clouds with MoveIt, I had to transpose them back to original sensor frame so that raytracing is able to map free space correctly.

Example from sensors_pointclouds.yaml (loaded by sensor_manager.launch.xml):

sensors:
  -
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /master/jetson1/points_camframe    #pcl2 in sensor frame
    filtered_cloud_topic: filtered_cloud1
    max_range: 9.0    #range from sensor
    max_update_rate: 20.0
    padding_offset: 0.1
    padding_scale: 1.0
    point_subsample: 1

I'm trying to avoid these unnecessary translations and found that sensor_pose can be given as input. However, using a pointcloud in the world reference frame and giving sensor_pose causes the pointcloud to be offset with the sensor position (orientation is kept). That is, a world point W(10,10,0) in a pcl2 is move by the given sensor_pose. So if sensor_pose is S(15,15,2), W will be moved to P(25,25,2). Secondly, it the example below it will be removed as max_range is calculated from W and not S.

sensors:
  -
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /master/jetson1/kinect_decomp    #pcl2 in world frame
    sensor_pose: jetson1_ir_optical_frame    #tf of the sensor
    filtered_cloud_topic: filtered_cloud1
    max_range: 9.0    #range from "world"
    max_update_rate: 20.0
    padding_offset: 0.1
    padding_scale: 1.0
    point_subsample: 1

Is there a way to perform raytracing from sensor_pose even if the point_cloud_topic is in the world frame or do I have to offset the pointcloud so sensor is in origin?

Thanks in advance!

edit retag flag offensive close merge delete