Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
vector_object_utils.hpp
1 // Copyright (c) 2023 Samsung R&D Institute Russia
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
16 #define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
17 
18 #include <uuid/uuid.h>
19 #include <cstdint>
20 #include <stdexcept>
21 #include <string>
22 
23 #include "geometry_msgs/msg/transform_stamped.hpp"
24 #include "nav_msgs/msg/occupancy_grid.hpp"
25 #include "std_msgs/msg/header.hpp"
26 
27 #include "nav2_ros_common/lifecycle_node.hpp"
28 #include "nav2_ros_common/tf2_factories.hpp"
29 #include "nav2_ros_common/node_utils.hpp"
30 #include "nav2_util/occ_grid_values.hpp"
31 
32 namespace nav2_map_server
33 {
34 
44 bool lookupShapeTransform(
45  const std_msgs::msg::Header & header,
46  const std::string & target_frame,
47  const nav2::TransformBuffer::SharedPtr & tf_buffer,
48  const double transform_tolerance,
49  geometry_msgs::msg::TransformStamped & transform);
50 
51 // ---------- Working with UUID-s ----------
52 
58 inline std::string unparseUUID(const unsigned char * uuid)
59 {
60  char uuid_str[37];
61  uuid_unparse(uuid, uuid_str);
62  return std::string(uuid_str);
63 }
64 
65 // ---------- Working with shapes' overlays ----------
66 
68 enum class OverlayType : uint8_t
69 {
70  OVERLAY_SEQ = 0, // Vector objects are superimposed in the order in which they have arrived
71  OVERLAY_MAX = 1, // Maximum value from vector objects and map is being chosen
72  OVERLAY_MIN = 2 // Minimum value from vector objects and map is being chosen
73 };
74 
82 inline void processVal(
83  int8_t & map_val, const int8_t shape_val,
84  const OverlayType overlay_type)
85 {
86  switch (overlay_type) {
87  case OverlayType::OVERLAY_SEQ:
88  map_val = shape_val;
89  return;
90  case OverlayType::OVERLAY_MAX:
91  if (shape_val > map_val) {
92  map_val = shape_val;
93  }
94  return;
95  case OverlayType::OVERLAY_MIN:
96  if ((map_val == nav2_util::OCC_GRID_UNKNOWN || shape_val < map_val) &&
97  shape_val != nav2_util::OCC_GRID_UNKNOWN)
98  {
99  map_val = shape_val;
100  }
101  return;
102  default:
103  throw std::runtime_error{"Unknown overlay type"};
104  }
105 }
106 
114 inline void processCell(
115  const nav_msgs::msg::OccupancyGrid::SharedPtr & map,
116  const unsigned int offset,
117  const int8_t shape_val,
118  const OverlayType overlay_type)
119 {
120  processVal(map->data[offset], shape_val, overlay_type);
121 }
122 
125 {
126 public:
134  nav_msgs::msg::OccupancyGrid::SharedPtr map,
135  int8_t value, OverlayType overlay_type)
136  : map_(map), value_(value), overlay_type_(overlay_type)
137  {}
138 
143  inline void operator()(unsigned int offset)
144  {
145  processCell(map_, offset, value_, overlay_type_);
146  }
147 
148 protected:
150  nav_msgs::msg::OccupancyGrid::SharedPtr map_;
152  int8_t value_;
154  OverlayType overlay_type_;
155 };
156 
157 } // namespace nav2_map_server
158 
159 #endif // NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
Functor class used in raytraceLine algorithm.
OverlayType overlay_type_
Overlay type.
void operator()(unsigned int offset)
Map' cell updating operator.
MapAction(nav_msgs::msg::OccupancyGrid::SharedPtr map, int8_t value, OverlayType overlay_type)
MapAction constructor.
nav_msgs::msg::OccupancyGrid::SharedPtr map_
Output map pointer.
int8_t value_
Value to put on map.