Not possible to visualize traversability costmap

[maverickpeter]

Not possible to visualize traversability costmap

[MaverickPeter]

@paucarrecardona, Thanks for your following. I've updated the docker image and README.

[paucarrecardona]

Thanks @MaverickPeter !
I'll test ASAP

[paucarrecardona]

@MaverickPeter
First of all, thank you so much for quickly tackling the problem and updating the repo.

I pulled the latest version and I tried the following:
1- "Quick Demo" from the Readme.md did not show any data in RViz, just an empty grid
2- "Full Usage" playing bag loop-22.bag, and only using robot_2 scripts.
This also did not show anything beyond an empty grid in RVIz:

I cannot see any TF being published either.

After playing the two bags, and running rostopic list I get the following topics:

/robot_1/imu
/robot_1/pointcloud
/robot_1/submap
/robot_2/Laser_map
/robot_2/Odometry
/robot_2/aft_mapped_path
/robot_2/aft_mapped_to_init
/robot_2/aft_mapped_to_init_high_frec
/robot_2/cloud_effected
/robot_2/cloud_registered
/robot_2/cloud_registered_body
/robot_2/dense_mapping
/robot_2/disco
/robot_2/elevation_map_raw
/robot_2/global_octomap
/robot_2/global_point
/robot_2/history_point
/robot_2/image_rect
/robot_2/imu
/robot_2/keyframe_pc
/robot_2/laser_cloud_corner_last
/robot_2/laser_cloud_flat
/robot_2/laser_cloud_less_flat
/robot_2/laser_cloud_less_sharp
/robot_2/laser_cloud_map
/robot_2/laser_cloud_sharp
/robot_2/laser_cloud_surf_last
/robot_2/laser_cloud_surround
/robot_2/laser_odom_path
/robot_2/laser_odom_to_init
/robot_2/laser_remove_points
/robot_2/local_octomap
/robot_2/map_saving
/robot_2/merged_cloud_registered
/robot_2/merged_keyframe
/robot_2/new_keyframe
/robot_2/opt_keyframes
/robot_2/opt_map
/robot_2/orthomosaic
/robot_2/path
/robot_2/pointcloud
/robot_2/submap
/robot_2/velodyne_cloud_2
/robot_2/velodyne_cloud_3
/robot_2/velodyne_cloud_registered
/robot_2/visual_map
/robot_2/visualpoints
/robot_2/voxel_grid/output
/robot_3/submap
/rosout
/rosout_agg
/tf
/tf_static
/tmp2_2g
/tmp_2g
/voxel_grid_2g_x/parameter_descriptions
/voxel_grid_2g_x/parameter_updates
/voxel_grid_2g_y/parameter_descriptions
/voxel_grid_2g_y/parameter_updates
/voxel_grid_2g_z/parameter_descriptions
/voxel_grid_2g_z/parameter_updates

Can you help me to visualize the traversabilty costmap?

[MaverickPeter]

@paucarrecardona
Based on the topic list you provided, I guess the global manager and costmap converter are not working. You may need to check these modules. Also, loop-22.bag is corresponding to the robot_1 scripts.
As for costmap visualization, you need to add a map topic and choose the global_costmap topic.

[paucarrecardona]

@MaverickPeter
Thanks for your help.
I found this method that created the subscriptions: https://github.com/MaverickPeter/MR_SLAM/blob/main/Mapping/src/elevation_mapping_periodical/elevation_mapping/src/ElevationMapping.cpp#L100

It seems it is never called and thus the subscriptions are not created.
Shall this method and all their callbacks be removed from the code or is the method supposed to be called?

[MaverickPeter]

@paucarrecardona
In the quick demo, the elevation mapping is not called because the elevation map is preprocessed in the provided bags. You can choose /global_manager/merged_map to visualize the global elevation map.
As for full usage, the elevation mapping should be called to process the incoming lidar points.

[paucarrecardona]

@MaverickPeter
Thanks
I'm trying to figure out why I can't generate elevation maps.
I stumbled upon something I don't understand and I was wondering if you'd be able to help me to understand the code.
As far as I understand, void ElevationMapping::processpoints(pcl::PointCloud::ConstPtr pointCloud) is the main method to compute the elevation map.
As far as I understand, the height shall be stored in float point_height[point_num];

But after the method is finished, I can't see how the height/elevation is used.
I assume the elevation is stored in the GPU, specifically in device float *map_elevation;

And to use the elevation, one needs to use void Map_feature(int length, float *elevation, float *var, int *colorR, int *colorG, int *colorB, float *rough, float *slope, float *traver, float *intensity) which pulls the elevation from the GPU and copies in the CPU. This method is called in Map_feature(length_, elevation, var, point_colorR, point_colorG, point_colorB, rough, slope, traver, intensity); .
The elevation is copied into float elevation[length_ * length_];
which is only used in orthoImage = map_.show(timeStamp, robot_name, trackPointTransformed_x, trackPointTransformed_y, length_, elevation, var, point_colorR, point_colorG, point_colorB, rough, slope, traver, intensity);

Is then orthoImage = map_.show(timeStamp, robot_name, trackPointTransformed_x, trackPointTransformed_y, length_, elevation, var, point_colorR, point_colorG, point_colorB, rough, slope, traver, intensity); the generation of the elevation + traversability map?

[MaverickPeter]

@paucarrecardona
You're right about the generation process. After map_.show(), the elevation and traversability are transferred into map_ defined in ElevationMap.cpp which is further visualized using visualPointMap() function. This elevation_mapping is modified from GEM
In my experience, this might be some configuration problem rather than a code issue,