Send ESDF/TSDF information from voxl-mapper to ROS2

Jeffrey Mao

Hi Modalai,

Currently, I have the voxl mapper running, but when I run voxl_mpa_to_ros2. I see that only voxl_mapper_aligned_ptcloud is brought over to ROS2 because the interface for voxl_mpa_to_ros2 only has a pointcloud interface. What sort of interface would be needed to be made for the voxl_mapper_costmap to ROS2 or is there a certain guide. Also would it be possible to expose either the TSDF/ESDF from voxl-mapper to the MPA and then bring it to ROS2?

voxl_mapper_aligned_ptcloud
voxl_mapper_costmap
voxl_mesh
voxl_mesh_endpoint

Thanks,

Jeff

Moderator

@Jeffrey-Mao voxl-mapper leverages voxlblox. You could look at their ROS implementation and try to port it over https://github.com/ethz-asl/voxblox/tree/master/voxblox_ros/src

Jeffrey Mao

@Moderator Right I think that is a bit too difficult.

I just edited the voxl-mapper library to send the pointcloud and it works now to get the map in ROS2 with the mpa_ros2 interface. It is a bit weird that you guys have this esdf publish pipe as a pointcloud but it doesn't seem exposed naturally

https://gitlab.com/voxl-public/voxl-sdk/services/voxl-mapper/-/blob/master/server/voxl-mapper/voxl_mapper.cc?ref_type=heads

void TsdfServer::publishEsdfPointcloud(){
    std::vector<point_xyz_i> esdf_ptcloud;

    voxblox::Layer<voxblox::EsdfVoxel> *layer_ptr = esdf_map_->getEsdfLayerPtr();
    createDistancePointcloudFromEsdfLayer(*layer_ptr, esdf_ptcloud);

    // intensity ptcloud for distance
    point_cloud_metadata_t esdf_meta;
    esdf_meta.magic_number = POINT_CLOUD_MAGIC_NUMBER;
    esdf_meta.timestamp_ns = rc_nanos_monotonic_time();
    esdf_meta.n_points = esdf_ptcloud.size();
    esdf_meta.format = POINT_CLOUD_FORMAT_FLOAT_XYZC;

    pipe_server_write_point_cloud(COSTMAP_CH, esdf_meta, esdf_ptcloud.data());
}