/home/runner/work/beluga/beluga/beluga_amcl/include/beluga_amcl/amcl_node.hpp Source File

/home/runner/work/beluga/beluga/beluga_amcl/include/beluga_amcl/amcl_node.hpp Source File#

Beluga AMCL: /home/runner/work/beluga/beluga/beluga_amcl/include/beluga_amcl/amcl_node.hpp Source File
Beluga AMCL
amcl_node.hpp
Go to the documentation of this file.
1 // Copyright 2022-2024 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_AMCL_AMCL_NODE_HPP
16 #define BELUGA_AMCL_AMCL_NODE_HPP
17 
18 #include <memory>
19 #include <optional>
20 #include <utility>
21 
22 #pragma GCC diagnostic push
23 #pragma GCC diagnostic ignored "-Wcpp"
24 #include <message_filters/subscriber.h>
25 #pragma GCC diagnostic pop
26 
27 #include <tf2_ros/buffer.h>
28 #include <tf2_ros/message_filter.h>
29 #include <tf2_ros/transform_broadcaster.h>
30 #include <tf2_ros/transform_listener.h>
31 #include <bondcpp/bond.hpp>
32 #include <rclcpp_lifecycle/lifecycle_node.hpp>
33 
34 #include <geometry_msgs/msg/pose_array.hpp>
35 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
36 #include <nav_msgs/msg/occupancy_grid.hpp>
37 #include <sensor_msgs/msg/laser_scan.hpp>
38 #include <std_srvs/srv/empty.hpp>
39 #include <visualization_msgs/msg/marker_array.hpp>
40 
41 #include <beluga/beluga.hpp>
42 #include <beluga_ros/amcl.hpp>
43 #include "beluga_amcl/ros2_common.hpp"
44 
50 namespace beluga_amcl {
51 
53 class AmclNode : public BaseAMCLNode {
54  public:
55  using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
56 
58  explicit AmclNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
59  ~AmclNode() override;
60 
61  protected:
63  void do_activate(const rclcpp_lifecycle::State&) override;
64 
66  void do_deactivate(const rclcpp_lifecycle::State&) override;
67 
69  void do_cleanup(const rclcpp_lifecycle::State&) override;
70 
72  auto get_initial_estimate() const -> std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>>;
73 
75  auto get_motion_model(std::string_view) const -> beluga_ros::Amcl::motion_model_variant;
76 
78  auto get_sensor_model(std::string_view, nav_msgs::msg::OccupancyGrid::SharedPtr) const
79  -> beluga_ros::Amcl::sensor_model_variant;
80 
82  auto make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr) const -> std::unique_ptr<beluga_ros::Amcl>;
83 
85  void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr);
86 
88  void do_periodic_timer_callback() override;
89 
91  void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr);
92 
94  void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override;
95 
98  std::shared_ptr<rmw_request_id_t> request_header,
99  std::shared_ptr<std_srvs::srv::Empty::Request> request,
100  std::shared_ptr<std_srvs::srv::Empty::Response> response);
101 
104  std::shared_ptr<rmw_request_id_t> request_header,
105  std::shared_ptr<std_srvs::srv::Empty::Request> req,
106  std::shared_ptr<std_srvs::srv::Empty::Response> res);
107 
109 
117  bool initialize_from_estimate(const std::pair<Sophus::SE2d, Eigen::Matrix3d>& estimate);
118 
120 
128 
130  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
132  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan, rclcpp_lifecycle::LifecycleNode>>
134 
136  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_localization_server_;
138  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_server_;
139 
141  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
143  message_filters::Connection laser_scan_connection_;
144 
146  std::unique_ptr<beluga_ros::Amcl> particle_filter_;
148  std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_;
152  bool enable_tf_broadcast_{false};
153 };
154 
155 } // namespace beluga_amcl
156 
157 #endif // BELUGA_AMCL_AMCL_NODE_HPP
2D AMCL as a ROS 2 composable lifecycle node.
Definition: amcl_node.hpp:53
void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr)
Callback for laser scan updates.
void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr)
Callback for occupancy grid map updates.
bool enable_tf_broadcast_
Whether to broadcast transforms or not.
Definition: amcl_node.hpp:152
auto get_motion_model(std::string_view) const -> beluga_ros::Amcl::motion_model_variant
Get motion model as per current parametrization.
std::optional< Sophus::SE2d > last_known_odom_transform_in_map_
Last known map to odom correction estimate, if any.
Definition: amcl_node.hpp:150
void global_localization_callback(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< std_srvs::srv::Empty::Request > request, std::shared_ptr< std_srvs::srv::Empty::Response > response)
Callback for the global relocalization service.
AmclNode(const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
Constructor.
auto get_initial_estimate() const -> std::optional< std::pair< Sophus::SE2d, Eigen::Matrix3d >>
Get initial pose estimate from parameters if set.
rclcpp::Service< std_srvs::srv::Empty >::SharedPtr nomotion_update_server_
No motion update service server.
Definition: amcl_node.hpp:138
std::unique_ptr< message_filters::Subscriber< sensor_msgs::msg::LaserScan, rclcpp_lifecycle::LifecycleNode > > laser_scan_sub_
Laser scan updates subscription.
Definition: amcl_node.hpp:133
void nomotion_update_callback(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< std_srvs::srv::Empty::Request > req, std::shared_ptr< std_srvs::srv::Empty::Response > res)
Callback for the no motion update service.
std::unique_ptr< beluga_ros::Amcl > particle_filter_
Particle filter instance.
Definition: amcl_node.hpp:146
void do_activate(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.
bool initialize_from_estimate(const std::pair< Sophus::SE2d, Eigen::Matrix3d > &estimate)
Initialize particles from an estimated pose and covariance.
rclcpp::Service< std_srvs::srv::Empty >::SharedPtr global_localization_server_
Global relocalization service server.
Definition: amcl_node.hpp:136
rclcpp::Subscription< nav_msgs::msg::OccupancyGrid >::SharedPtr map_sub_
Occupancy grid map updates subscription.
Definition: amcl_node.hpp:130
message_filters::Connection laser_scan_connection_
Connection for laser scan updates filter and callback.
Definition: amcl_node.hpp:143
void do_periodic_timer_callback() override
Callback for periodic particle cloud updates.
void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override
Callback for pose (re)initialization.
void do_deactivate(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state.
void do_cleanup(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from the INACTIVE state to the UNCONFIGURED state.
bool initialize_from_map()
Initialize particles from map.
std::unique_ptr< tf2_ros::MessageFilter< sensor_msgs::msg::LaserScan > > laser_scan_filter_
Transform synchronization filter for laser scan updates.
Definition: amcl_node.hpp:141
auto get_sensor_model(std::string_view, nav_msgs::msg::OccupancyGrid::SharedPtr) const -> beluga_ros::Amcl::sensor_model_variant
Get sensor model as per current parametrization.
std::optional< std::pair< Sophus::SE2d, Eigen::Matrix3d > > last_known_estimate_
Last known pose estimate, if any.
Definition: amcl_node.hpp:148
auto make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr) const -> std::unique_ptr< beluga_ros::Amcl >
Instantiate particle filter given an initial occupancy grid map and the current parametrization.
Definition: ros2_common.hpp:55