vector_object_utils.hpp

// Copyright (c) 2023 Samsung R&D Institute Russia
//
// 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 NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
#define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
 
#include <uuid/uuid.h>
#include <stdexcept>
#include <string>
 
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
 
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_util/occ_grid_values.hpp"
 
namespace nav2_map_server
{
 
// ---------- Working with UUID-s ----------
 
inline std::string unparseUUID(const unsigned char * uuid)
{
  char uuid_str[37];
  uuid_unparse(uuid, uuid_str);
  return std::string(uuid_str);
}
 
// ---------- Working with shapes' overlays ----------
 
enum class OverlayType : uint8_t
{
  OVERLAY_SEQ = 0,  // Vector objects are superimposed in the order in which they have arrived
  OVERLAY_MAX = 1,  // Maximum value from vector objects and map is being chosen
  OVERLAY_MIN = 2   // Minimum value from vector objects and map is being chosen
};
 
inline void processVal(
  int8_t & map_val, const int8_t shape_val,
  const OverlayType overlay_type)
{
  switch (overlay_type) {
    case OverlayType::OVERLAY_SEQ:
      map_val = shape_val;
      return;
    case OverlayType::OVERLAY_MAX:
      if (shape_val > map_val) {
        map_val = shape_val;
      }
      return;
    case OverlayType::OVERLAY_MIN:
      if ((map_val == nav2_util::OCC_GRID_UNKNOWN || shape_val < map_val) &&
        shape_val != nav2_util::OCC_GRID_UNKNOWN)
      {
        map_val = shape_val;
      }
      return;
    default:
      throw std::runtime_error{"Unknown overlay type"};
  }
}
 
inline void processCell(
  nav_msgs::msg::OccupancyGrid::SharedPtr map,
  const unsigned int offset,
  const int8_t shape_val,
  const OverlayType overlay_type)
{
  int8_t map_val = map->data[offset];
  processVal(map_val, shape_val, overlay_type);
  map->data[offset] = map_val;
}
 
class MapAction
{
public:
  MapAction(
    nav_msgs::msg::OccupancyGrid::SharedPtr map,
    int8_t value, OverlayType overlay_type)
  : map_(map), value_(value), overlay_type_(overlay_type)
  {}
 
  inline void operator()(unsigned int offset)
  {
    processCell(map_, offset, value_, overlay_type_);
  }
 
protected:
  nav_msgs::msg::OccupancyGrid::SharedPtr map_;
  int8_t value_;
  OverlayType overlay_type_;
};
 
}  // namespace nav2_map_server
 
#endif  // NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_