This package is an extension of the [elevation_mapping package] (https://github.com/ANYbotics/elevation_mapping) of Anybotics. It maps the environment to gridmap containing values for the height. In the addition to the already existing height layer there are two extra layers added to the grid map containing the roll and the pitch of the environment. This is published as the elevation map.
To add roll and pitch to the grid map the class PlaneDetection extracts the roll and pitch of the given pointcloud. This is done in three steps:
To speed up the process the biggest plane will be determined using pcl::SACSegmentation. Paramaters for the algorithm can be tweaked in talos_robot.yaml. This algorithm gives the plane equation which easily converts to the planes normal vector.
The normal vectors of the rest of the cloud are calculated with pcl::NormalEstimation. Paramaters for the algorithm can be tweaked in talos_robot.yaml.
With the previous algorithms the normal vector of each point in the pointcloud is calculated. With pcl::RegionGrowing different planes are extracted. Paramaters for the algorithm can be tweaked in talos_robot.yaml. The topic /Output/planeCloud shows the different planes in different colors. Points of the pointcloud that are not part of any plane according to the algorithm are displayed in red and get a pitch and roll value of pi/2.
The values for height, roll and pitch are added to the elevation map. Note that only the topic /elevation_mapping/elevation_map_raw contains the values for pitch and roll. The map fusion still needs to be fixed such that the topic /elevation_mapping/elevation_map_fused also contains the orientation.
