Description
Generate an occupancy grid map using the Nav2 module and slam_toolbox package. Test it with rosbags and simulation to evaluate effectiveness.
Tutorial from the Nav2 wiki is here.
For the odom frame (that should be an estimate of the pose based on the IMU data), we can use the localization package we are using to fuse IMU and GPS for the map->base_link frames right now (so the odom/filtered message), but maybe using other parameters that ignore the GPS later on if it creates issues in the odom estimate and consequently in SLAM.
Something else that is needed before integrating with Nav2 is creating a node (or using the one that currently filters the LiDAR data) to remove elements that we don't want from the point cloud, especially trees or other objects that are above a certain z threshold, and make sure the water or any leaves on it that cause reflections are filtered out to not be considered as obstacles. That node should also project the points onto a 2D plane (slice or project?) parallel to the water, so that then Nav2 works with 2D laser scans. First try using this node to convert the PointCloud2 message to a LaserScan that is needed for the SLAM package.
Potentially helpful ideas for the point cloud filtering/projection node:
- Just a simple z-value threshold should get rid of most of objects far above the water
- After removing most of the z-value range, the RANSAC that filters out the water (if that's a big issue, and it's not just the leaves causing reflections, rosbag testing needed to determine that), should have a really low error threshold and a very high inlier number that should be reached before filtering out the inliers of the detected plane, as otherwise the buoys, or some random small plane-like surface that's not the water, might be filtered out instead, respectively.