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

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

Beluga ROS: /home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/laser_scan.hpp Source File
Beluga ROS
laser_scan.hpp
Go to the documentation of this file.
1 // Copyright 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_LASER_SCAN_HPP
16 #define BELUGA_ROS_LASER_SCAN_HPP
17 
18 #include <range/v3/view/iota.hpp>
19 
22 #include <beluga_ros/messages.hpp>
23 
24 #include <sophus/se3.hpp>
25 
31 namespace beluga_ros {
32 
34 class LaserScan : public beluga::BaseLaserScan<LaserScan> {
35  public:
37  using Scalar = double;
38 
49  explicit LaserScan(
50  beluga_ros::msg::LaserScanConstSharedPtr scan,
51  Sophus::SE3d origin = Sophus::SE3d(),
52  std::size_t max_beams = std::numeric_limits<std::size_t>::max(),
53  Scalar min_range = std::numeric_limits<Scalar>::min(),
54  Scalar max_range = std::numeric_limits<Scalar>::max())
55  : scan_(std::move(scan)),
56  origin_(std::move(origin)),
57  max_beams_(max_beams),
58  min_range_(std::max(static_cast<Scalar>(scan_->range_min), min_range)),
59  max_range_(std::min(static_cast<Scalar>(scan_->range_max), max_range)) {
60  assert(scan_ != nullptr);
61  }
62 
64  [[nodiscard]] const auto& origin() const { return origin_; }
65 
67  [[nodiscard]] auto angles() const {
68  return ranges::views::iota(0, static_cast<int>(scan_->ranges.size())) | beluga::views::take_evenly(max_beams_) |
69  ranges::views::transform([this](int i) {
70  return static_cast<Scalar>(scan_->angle_min + static_cast<float>(i) * scan_->angle_increment);
71  });
72  }
73 
75  [[nodiscard]] auto ranges() const {
76  return scan_->ranges | beluga::views::take_evenly(max_beams_) |
77  ranges::views::transform([](auto value) { return static_cast<Scalar>(value); });
78  }
79 
81  [[nodiscard]] auto min_range() const { return min_range_; }
82 
84  [[nodiscard]] auto max_range() const { return max_range_; }
85 
86  private:
87  beluga_ros::msg::LaserScanConstSharedPtr scan_;
88  Sophus::SE3d origin_;
89  std::size_t max_beams_;
90  Scalar min_range_;
91  Scalar max_range_;
92 };
93 
94 } // namespace beluga_ros
95 
96 #endif // BELUGA_ROS_LASER_SCAN_HPP
Thin wrapper type for 2D sensor_msgs/LaserScan messages.
Definition: laser_scan.hpp:34
const auto & origin() const
Get the laser scan frame origin in the filter frame.
Definition: laser_scan.hpp:64
LaserScan(beluga_ros::msg::LaserScanConstSharedPtr scan, Sophus::SE3d origin=Sophus::SE3d(), std::size_t max_beams=std::numeric_limits< std::size_t >::max(), Scalar min_range=std::numeric_limits< Scalar >::min(), Scalar max_range=std::numeric_limits< Scalar >::max())
Definition: laser_scan.hpp:49
auto min_range() const
Get the minimum range measurement.
Definition: laser_scan.hpp:81
auto max_range() const
Get the maximum range measurement.
Definition: laser_scan.hpp:84
double Scalar
Range type.
Definition: laser_scan.hpp:37
auto ranges() const
Get laser scan range measurements as a range.
Definition: laser_scan.hpp:75
auto angles() const
Get laser scan measurement angles as a range.
Definition: laser_scan.hpp:67
The main Beluga ROS namespace.
Definition: amcl.hpp:44