conversions.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
 
#ifndef NAVMAP_ROS__CONVERSIONS_HPP_
#define NAVMAP_ROS__CONVERSIONS_HPP_

 
#include <string>
 
#include "Eigen/Core"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "std_msgs/msg/header.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "navmap_ros_interfaces/msg/nav_map.hpp"
#include "navmap_ros_interfaces/msg/nav_map_layer.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "navmap_core/Costs.hpp"
#include "navmap_core/NavMap.hpp"

namespace navmap_ros
{
 
// Cost values (costmap_2d convention), defined in navmap_core/Costs.hpp
using navmap::NO_INFORMATION;
using navmap::LETHAL_OBSTACLE;
using navmap::INSCRIBED_INFLATED_OBSTACLE;
using navmap::MAX_NON_OBSTACLE;
using navmap::FREE_SPACE;
 
// --------- NavMap <-> ROS message ---------

navmap_ros_interfaces::msg::NavMap to_msg(
  const navmap::NavMap & nm, const std_msgs::msg::Header & header);

navmap_ros_interfaces::msg::NavMap to_msg(const navmap::NavMap & nm);

navmap::NavMap from_msg(
  const navmap_ros_interfaces::msg::NavMap & msg,
  std_msgs::msg::Header & header);

navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg);

navmap_ros_interfaces::msg::NavMapLayer to_msg(
  const navmap::NavMap & nm,
  const std::string & layer,
  const std_msgs::msg::Header & header);

navmap_ros_interfaces::msg::NavMapLayer to_msg(
  const navmap::NavMap & nm,
  const std::string & layer);

navmap_ros_interfaces::msg::NavMapLayer to_layer_update_msg(
  const navmap::NavMap & nm, const std::string & layer,
  const std_msgs::msg::Header & header = std_msgs::msg::Header(),
  float max_sparse_fraction = 0.25f);

void from_msg(
  const navmap_ros_interfaces::msg::NavMapLayer & msg,
  navmap::NavMap & nm,
  std_msgs::msg::Header & header);

void from_msg(
  const navmap_ros_interfaces::msg::NavMapLayer & msg,
  navmap::NavMap & nm);
 
// --------- OccupancyGrid <-> NavMap (shared vertices, per-NavCel layer) ---------

navmap::NavMap from_occupancy_grid(
  const nav_msgs::msg::OccupancyGrid & grid,
  std_msgs::msg::Header & header);

navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid);

nav_msgs::msg::OccupancyGrid to_occupancy_grid(
  const navmap::NavMap & nm,
  const std_msgs::msg::Header & header);

nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm);

struct BuildParams
{
  float resolution = 1.0;

  float max_edge_len = 2.0;

  float max_dz = 0.25f;

  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

  float min_clearance = 1.0f;

  int max_surfaces = 0;

  float min_surface_area = 0.0f;

  float max_hole_area = 1.0f;

  bool carve_free_space = true;

  bool drop_seen_from_below = true;

  float carve_voxel = 0.0f;

  float carve_max_range = 20.0f;

  float carve_ratio = 1.0f;
};

struct ViewpointFilterStats
{
  std::size_t input = 0;
  std::size_t carved = 0;        // in space that rays went through
  std::size_t seen_from_below = 0;
};

navmap::NavMap from_points(
  const  pcl::PointCloud<pcl::PointXYZ> & input_points,
  navmap_ros_interfaces::msg::NavMap & out_msg,
  BuildParams params);

pcl::PointCloud<pcl::PointXYZ> filter_with_viewpoints(
  const pcl::PointCloud<pcl::PointWithViewpoint> & input_points,
  const BuildParams & params,
  ViewpointFilterStats * stats = nullptr);

navmap::NavMap from_points(
  const pcl::PointCloud<pcl::PointWithViewpoint> & input_points,
  navmap_ros_interfaces::msg::NavMap & out_msg,
  BuildParams params);

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

navmap::NavMap from_regular_grid(
  const pcl::PointCloud<pcl::PointXYZ> & grid_points,
  navmap_ros_interfaces::msg::NavMap & out_msg,
  BuildParams params);
 
}  // namespace navmap_ros
 
#endif  // NAVMAP_ROS__CONVERSIONS_HPP_