Improve surface creation

navmap_ros/CMakeLists.txt

@@ -79,6 +79,7 @@ ament_export_dependencies(
   navmap_ros_interfaces
   geometry_msgs
   sensor_msgs
+  std_srvs
   PCL
   pcl_conversions
 )

navmap_ros/include/navmap_ros/conversions.hpp

@@ -84,29 +84,44 @@ navmap_ros_interfaces::msg::NavMap to_msg(
  * @details
  *  - Intended to be the inverse of ::navmap_ros::to_msg for a round-trip without loss.
  *  - Assumes that the message is internally consistent (sizes and indices match).
+ *
+ * @throw std::runtime_error If the message describes inconsistent geometry or layer sizes.
  */
 navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg);
 
 /**
- * \brief Convert a single layer from a NavMap into a ROS message.
+ * @brief Convert a single layer from a NavMap into a ROS message.
+ *
+ * @param[in] nm     Input NavMap.
+ * @param[in] layer  Name of the layer to export.
+ * @return A NavMapLayer message containing the layer values and metadata.
  *
- * \param nm     Input NavMap.
- * \param layer  Name of the layer to export.
- * \return A NavMapLayer message containing the layer values and metadata.
- * \throw std::runtime_error if the layer does not exist.
+ * @details
+ *  - The returned message contains the layer name, type tag, and exactly one populated data
+ *    array whose length equals the number of NavCels in @p nm.
+ *  - The function performs a type-safe extraction (U8/F32/F64).
+ *
+ * @throw std::runtime_error If the layer does not exist or has an unsupported type.
  */
 navmap_ros_interfaces::msg::NavMapLayer to_msg(
   const navmap::NavMap & nm,
   const std::string & layer);
 
 /**
- * \brief Import a single NavMapLayer message into a NavMap.
+ * @brief Import a single NavMapLayer message into a NavMap.
  *
- * If the layer already exists in \p nm, it is overwritten. Otherwise, it is created.
+ * If the layer already exists in @p nm, it is overwritten. Otherwise, it is created.
  * Performs type dispatch based on the message field `type`.
  *
- * \param msg Input NavMapLayer message.
- * \param nm  Destination NavMap (must already have navcels sized correctly).
+ * @param[in] msg Input NavMapLayer message.
+ * @param[in,out] nm  Destination NavMap (must already have navcels sized correctly).
+ *
+ * @details
+ *  - The function verifies that the length of the populated data array matches
+ *    the number of triangles (NavCels) in @p nm.
+ *  - Exactly one of the arrays `data_u8`, `data_f32`, or `data_f64` must be set.
+ *
+ * @throw std::runtime_error If sizes are inconsistent or the message is ill-formed.
  */
 void from_msg(
   const navmap_ros_interfaces::msg::NavMapLayer & msg,
@@ -155,22 +170,91 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid);
  * @note The fallback path assumes the presence of an `"occupancy"` layer. The precise sampling
  *       strategy (bounds, resolution, and handling of cells without a containing navcel) is
  *       implementation-defined.
+ *
+ * @warning If the map does not carry grid metadata or the `"occupancy"` layer is missing,
+ *          the result may be incomplete or implementation-defined.
  */
 nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm);
 
+/**
+ * @brief Parameters controlling NavMap construction from unorganized points.
+ *
+ * These parameters guide neighborhood search, local meshing, and basic geometric filtering
+ * used by the point-cloud based builders.
+ */
 struct BuildParams
 {
+  /** @brief Seed position (world frame) used by region growing or initial search heuristics. */
   Eigen::Vector3f seed = {0.0, 0.0, 0.0};
+
+  /** @brief Target in-plane sampling resolution (meters) used by voxelization or gridding. */
   float resolution = 1.0;
+
+  /** @brief Maximum allowed edge length (meters) when forming triangles. */
   float max_edge_len = 2.0;
+
+  /** @brief Maximum slope with respect to the vertical axis (degrees). */
   float max_slope_deg = 30.0f;   // maximum slope w.r.t. vertical
+
+  /** @brief Neighborhood radius (meters) for candidate connectivity. */
   float neighbor_radius = 2.0f;  // search radius
+
+  /** @brief Alternative to radius: number of nearest neighbors (k-NN). */
   int   k_neighbors = 20;        // k-NN alternative to radius
+
+  /** @brief Minimum triangle area (square meters) to reject degenerate faces. */
   float min_area = 1e-6f;        // minimum triangle area to avoid degenerates
+
+  /** @brief If true, use radius-based neighborhoods; otherwise use k-NN. */
   bool  use_radius = true;
+
+  /** @brief Minimum interior angle (degrees) to avoid sliver triangles. */
   float min_angle_deg = 20.0f;   // minimum interior angle (deg) to avoid sliver triangles
 };
 
+/**
+ * @brief Build a NavMap surface from a PCL point cloud.
+ *
+ * @param[in] input_points Point set in world coordinates (`pcl::PointXYZ`).
+ * @param[out] out_msg     Output transport message mirroring the created NavMap.
+ * @param[in] params       Meshing and filtering parameters (see ::BuildParams).
+ * @return The constructed `navmap::NavMap`.
+ *
+ * @details
+ *  Typical steps implemented by this builder include:
+ *  - Optional downsampling according to @p params.resolution.
+ *  - Local neighborhood discovery using either radius (@p params.use_radius) or k-NN.
+ *  - Edge and face filtering based on @p params.max_edge_len, @p params.min_area and
+ *    @p params.min_angle_deg.
+ *  - Optional slope gating using @p params.max_slope_deg with respect to the vertical axis.
+ *  - Creation of a single surface with shared vertices and triangle indices.
+ *  The function also fills @p out_msg with the compact ROS representation of the resulting map.
+ *
+ * @note Input is treated as an unorganized cloud. If normals or intensities are present,
+ *       they are ignored by this overload.
+ * @throw std::runtime_error If meshing fails due to inconsistent parameters or empty input.
+ */
+navmap::NavMap from_points(
+  const  pcl::PointCloud<pcl::PointXYZ> & input_points,
+  navmap_ros_interfaces::msg::NavMap & out_msg,
+  BuildParams params);
+
+/**
+ * @brief Build a NavMap surface from a ROS `sensor_msgs::msg::PointCloud2`.
+ *
+ * @param[in] pc2      Input PointCloud2 message (expects fields `x`, `y`, `z`).
+ * @param[out] out_msg Output transport message mirroring the created NavMap.
+ * @param[in] params   Meshing and filtering parameters (see ::BuildParams).
+ * @return The constructed `navmap::NavMap`.
+ *
+ * @details
+ *  - The cloud is decoded to `pcl::PointXYZ` and processed as in ::navmap_ros::from_points.
+ *  - Non-Cartesian fields present in @p pc2 are ignored by this overload.
+ *  - The resulting NavMap is exported to @p out_msg for downstream publication or storage.
+ *
+ * @note If the message is empty or lacks the required fields, no geometry is produced.
+ * @throw std::runtime_error If decoding fails or meshing cannot be completed.
+ */
 navmap::NavMap from_pointcloud2(
   const sensor_msgs::msg::PointCloud2 & pc2,
   navmap_ros_interfaces::msg::NavMap & out_msg,