/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/occupancy_grid.hpp Source File

/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/occupancy_grid.hpp Source File#

Beluga ROS: /home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/occupancy_grid.hpp Source File
Beluga ROS
occupancy_grid.hpp
Go to the documentation of this file.
1 // Copyright 2022-2023 Ekumen, Inc.
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 BELUGA_ROS_OCCUPANCY_GRID_HPP
16 #define BELUGA_ROS_OCCUPANCY_GRID_HPP
17 
18 #include <cmath>
19 #include <memory>
20 #include <utility>
21 #include <vector>
22 
23 #include <sophus/se2.hpp>
24 #include <sophus/so2.hpp>
25 
26 #include <tf2/utils.h>
27 
28 #if BELUGA_ROS_VERSION == 2
29 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
30 #elif BELUGA_ROS_VERSION == 1
31 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
32 #else
33 #error BELUGA_ROS_VERSION is not defined or invalid
34 #endif
35 
37 #include <beluga_ros/messages.hpp>
38 
44 namespace beluga_ros {
45 
47 class OccupancyGrid : public beluga::BaseOccupancyGrid2<OccupancyGrid> {
48  public:
50 
53  struct ValueTraits {
55  static constexpr std::int8_t kFreeValue = 0;
57  static constexpr std::int8_t kUnknownValue = -1;
59  static constexpr std::int8_t kOccupiedValue = 100;
60 
62  [[nodiscard]] static bool is_free(std::int8_t value) { return value == kFreeValue; }
63 
65  [[nodiscard]] static bool is_unknown(std::int8_t value) { return value == kUnknownValue; }
66 
68  [[nodiscard]] static bool is_occupied(std::int8_t value) { return value == kOccupiedValue; }
69  };
70 
74  explicit OccupancyGrid(beluga_ros::msg::OccupancyGridConstSharedPtr grid)
75  : grid_(std::move(grid)), origin_(make_origin_transform(grid_->info.origin)) {}
76 
78  [[nodiscard]] const Sophus::SE2d& origin() const { return origin_; }
79 
81  [[nodiscard]] std::size_t size() const { return grid_->data.size(); }
82 
84  [[nodiscard]] const auto& data() const { return grid_->data; }
85 
87  [[nodiscard]] std::size_t width() const { return grid_->info.width; }
88 
90  [[nodiscard]] std::size_t height() const { return grid_->info.height; }
91 
93  [[nodiscard]] double resolution() const { return grid_->info.resolution; }
94 
96  [[nodiscard]] static auto value_traits() { return ValueTraits{}; }
97 
98  private:
99  beluga_ros::msg::OccupancyGridConstSharedPtr grid_;
100  Sophus::SE2d origin_;
101 
102  static Sophus::SE2d make_origin_transform(const beluga_ros::msg::Pose& origin) {
103  const auto rotation = Sophus::SO2d{tf2::getYaw(origin.orientation)};
104  const auto translation = Eigen::Vector2d{origin.position.x, origin.position.y};
105  return Sophus::SE2d{rotation, translation};
106  }
107 };
108 
109 } // namespace beluga_ros
110 
111 #endif // BELUGA_ROS_OCCUPANCY_GRID_HPP
Thin wrapper type for 2D nav_msgs/OccupancyGrid messages.
Definition: occupancy_grid.hpp:47
std::size_t size() const
Get the size of the occupancy grid (width() times height()).
Definition: occupancy_grid.hpp:81
std::size_t width() const
Get the width of the occupancy grid.
Definition: occupancy_grid.hpp:87
std::size_t height() const
Get the height of the occupancy grid.
Definition: occupancy_grid.hpp:90
static auto value_traits()
Get the traits for occupancy grid value interpretation.
Definition: occupancy_grid.hpp:96
const auto & data() const
Get a reference to the underlying data storeage (ie. a row-major array).
Definition: occupancy_grid.hpp:84
OccupancyGrid(beluga_ros::msg::OccupancyGridConstSharedPtr grid)
Definition: occupancy_grid.hpp:74
const Sophus::SE2d & origin() const
Get the occupancy grid origin in the occupancy grid frame.
Definition: occupancy_grid.hpp:78
double resolution() const
Get the resolution of the occupancy grid discretization, in meters.
Definition: occupancy_grid.hpp:93
The main Beluga ROS namespace.
Definition: amcl.hpp:44
Traits for occupancy grid value interpretation.
Definition: occupancy_grid.hpp:53
static bool is_occupied(std::int8_t value)
Check if the given value corresponds to that of an occupied cell.
Definition: occupancy_grid.hpp:68
static constexpr std::int8_t kUnknownValue
Unknown value in the standard ROS trinary interpretation.
Definition: occupancy_grid.hpp:57
static bool is_unknown(std::int8_t value)
Check if the given value corresponds to that of a cell of unknown occupancy.
Definition: occupancy_grid.hpp:65
static constexpr std::int8_t kFreeValue
Free value in the standard ROS trinary interpretation.
Definition: occupancy_grid.hpp:55
static constexpr std::int8_t kOccupiedValue
Occupied value in the standard ROS trinary interpretation.
Definition: occupancy_grid.hpp:59
static bool is_free(std::int8_t value)
Check if the given value corresponds to that of a free cell.
Definition: occupancy_grid.hpp:62