Surface reconstruction and Slam server

[fmrico]

Hi,

I have decided to move the code from the currently developing easynav_navmap_stack to this package to have the function from_pointcloud2 with the asociated type BuildParams:

struct BuildParams
{
  Eigen::Vector3f seed = {0.0, 0.0, 0.0};
  float resolution = 1.0;
  float max_edge_len = 2.0;
  float max_slope_deg = 30.0f;   // maximum slope w.r.t. vertical
  float neighbor_radius = 2.0f;  // search radius
  int   k_neighbors = 20;        // k-NN alternative to radius
  float min_area = 1e-6f;        // minimum triangle area to avoid degenerates
  bool  use_radius = true;
  float min_angle_deg = 20.0f;   // minimum interior angle (deg) to avoid sliver triangles
};

navmap::NavMap from_pointcloud2(
  const sensor_msgs::msg::PointCloud2 & pc2,
  navmap_ros_interfaces::msg::NavMap & out_msg,
  BuildParams params);

I have also added an executable slam_server_app which is able to receive nav_msgs::msg::OccupancyGrid and sensor_msgs::msg::PointCloud2 and publish the corresponding navmap_ros_interfaces::msg::NavMap. Usage, for example, as follows:

ros2 run navmap_ros slam_server_app --ros-args -r /incoming_pc2_map:=/map 

I hope it helps