/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.hpp>
27 
28 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
29 
31 
32 #include <nav_msgs/msg/occupancy_grid.hpp>
33 
39 namespace beluga_ros {
40 
42 class OccupancyGrid : public beluga::BaseOccupancyGrid2<OccupancyGrid> {
43  public:
45 
48  struct ValueTraits {
50  static constexpr std::int8_t kFreeValue = 0;
52  static constexpr std::int8_t kUnknownValue = -1;
54  static constexpr std::int8_t kOccupiedValue = 100;
55 
57  [[nodiscard]] static bool is_free(std::int8_t value) { return value == kFreeValue; }
58 
60  [[nodiscard]] static bool is_unknown(std::int8_t value) { return value == kUnknownValue; }
61 
63  [[nodiscard]] static bool is_occupied(std::int8_t value) { return value == kOccupiedValue; }
64  };
65 
69  explicit OccupancyGrid(nav_msgs::msg::OccupancyGrid::ConstSharedPtr grid)
70  : grid_(std::move(grid)), origin_(make_origin_transform(grid_->info.origin)) {}
71 
73  [[nodiscard]] const Sophus::SE2d& origin() const { return origin_; }
74 
76  [[nodiscard]] std::size_t size() const { return grid_->data.size(); }
77 
79  [[nodiscard]] const auto& data() const { return grid_->data; }
80 
82  [[nodiscard]] std::size_t width() const { return grid_->info.width; }
83 
85  [[nodiscard]] std::size_t height() const { return grid_->info.height; }
86 
88  [[nodiscard]] double resolution() const { return grid_->info.resolution; }
89 
91  [[nodiscard]] static auto value_traits() { return ValueTraits{}; }
92 
93  private:
94  nav_msgs::msg::OccupancyGrid::ConstSharedPtr grid_;
95  Sophus::SE2d origin_;
96 
97  static Sophus::SE2d make_origin_transform(const geometry_msgs::msg::Pose& origin) {
98  const auto rotation = Sophus::SO2d{tf2::getYaw(origin.orientation)};
99  const auto translation = Eigen::Vector2d{origin.position.x, origin.position.y};
100  return Sophus::SE2d{rotation, translation};
101  }
102 };
103 
104 } // namespace beluga_ros
105 
106 #endif // BELUGA_ROS_OCCUPANCY_GRID_HPP
Thin wrapper type for 2D nav_msgs/OccupancyGrid messages.
Definition: occupancy_grid.hpp:42
std::size_t size() const
Get the size of the occupancy grid (width() times height()).
Definition: occupancy_grid.hpp:76
std::size_t width() const
Get the width of the occupancy grid.
Definition: occupancy_grid.hpp:82
std::size_t height() const
Get the height of the occupancy grid.
Definition: occupancy_grid.hpp:85
static auto value_traits()
Get the traits for occupancy grid value interpretation.
Definition: occupancy_grid.hpp:91
const auto & data() const
Get a reference to the underlying data storeage (ie. a row-major array).
Definition: occupancy_grid.hpp:79
OccupancyGrid(nav_msgs::msg::OccupancyGrid::ConstSharedPtr grid)
Definition: occupancy_grid.hpp:69
const Sophus::SE2d & origin() const
Get the occupancy grid origin in the occupancy grid frame.
Definition: occupancy_grid.hpp:73
double resolution() const
Get the resolution of the occupancy grid discretization, in meters.
Definition: occupancy_grid.hpp:88
The main Beluga ROS namespace.
Definition: amcl.hpp:47
Traits for occupancy grid value interpretation.
Definition: occupancy_grid.hpp:48
static bool is_occupied(std::int8_t value)
Check if the given value corresponds to that of an occupied cell.
Definition: occupancy_grid.hpp:63
static constexpr std::int8_t kUnknownValue
Unknown value in the standard ROS trinary interpretation.
Definition: occupancy_grid.hpp:52
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:60
static constexpr std::int8_t kFreeValue
Free value in the standard ROS trinary interpretation.
Definition: occupancy_grid.hpp:50
static constexpr std::int8_t kOccupiedValue
Occupied value in the standard ROS trinary interpretation.
Definition: occupancy_grid.hpp:54
static bool is_free(std::int8_t value)
Check if the given value corresponds to that of a free cell.
Definition: occupancy_grid.hpp:57