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/NavMap.hpp"

namespace navmap_ros
{

constexpr uint8_t NO_INFORMATION = 255;
constexpr uint8_t LETHAL_OBSTACLE = 254;
constexpr uint8_t INSCRIBED_INFLATED_OBSTACLE = 253;
constexpr uint8_t MAX_NON_OBSTACLE = 252;
constexpr uint8_t FREE_SPACE = 0;  // end of Costmap value semantics group
 
// --------- 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);

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

  int max_surfaces = 0;  // 0 ó <0 => no limits. >0 => Keep only N larger
};

navmap::NavMap from_points(
  const  pcl::PointCloud<pcl::PointXYZ> & 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);
 
}  // namespace navmap_ros
 
#endif  // NAVMAP_ROS__CONVERSIONS_HPP_