[ROS][feat] Extract slice and estimate area

Goal

Add a functionality to the aggregation node to extract a slice and to compute the inner area.

Changes

• Add a callback in aggregation node to get the SLAM pose
  • When a pose is received, the closest points are extracted using the voxel grid. The outer voxels are used.
  • Those points are projected on a plane built using the local trajectory part.
  • A circular histogram is built from the trajectory position to store the points
  • A boundary pointcloud is built from this histogram
  • The pointcloud is used to estimate the area of the polygon
• Add 2 publishers : the slice points and the area as a float64 message

Commands to reproduce the results

Data

fast-filtered.bag

Build

catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release

Run

• Enable the slice computation in aggregation parameters file :

slice:
  enable: true         # Enable or disable the slice extraction and the area computation
  traj_length: 1.      # Length of trajectory to consider to define the slice plane
  width: 0.2           # Width of the pointcloud slice to take to create the slice projection
  max_dist: 4.         # Maximal distance of the points from the trajectory position onto the slice plane
  angle_resolution: 3. # Resolution of the slice in degrees from the trajectory position

roslaunch lidar_slam slam_ouster.launch outdoor:=false aggregate:=true

rosbag play --clock -d 2 path/to/fast-filtered.bag.bag /wire_converted:=wheel_odom

Check results

RVIZ +

rostopic echo /slice_area

Results

What remains to solve

Problem when doing back and forth and points were acquired on the robot. In a future MR the aggregation node should be updated so the close points are removed.