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

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

Beluga AMCL: /home/runner/work/beluga/beluga/beluga_amcl/include/beluga_amcl/ros2_common.hpp Source File
Beluga AMCL
ros2_common.hpp
1 // Copyright 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_ROS2_COMMON_HPP
16 #define BELUGA_AMCL_ROS2_COMMON_HPP
17 
18 #include <execution>
19 #include <rclcpp/node_options.hpp>
20 #include <rclcpp_lifecycle/lifecycle_node.hpp>
21 #include <rclcpp_lifecycle/state.hpp>
22 #include <string>
23 #include <string_view>
24 
25 #include <bondcpp/bond.hpp>
26 #include <geometry_msgs/msg/pose_stamped.hpp>
27 #include <nav_msgs/msg/occupancy_grid.hpp>
28 #include <rclcpp/rclcpp.hpp>
29 #include <rclcpp/time.hpp>
30 
31 #include <tf2_ros/create_timer_ros.h>
32 #include <tf2_ros/transform_broadcaster.h>
33 #include <tf2_ros/transform_listener.h>
34 #include <tf2/convert.hpp>
35 #include <tf2/utils.hpp>
36 
38 
39 namespace beluga_amcl {
40 
42 constexpr std::string_view kDifferentialModelName = "differential_drive";
44 constexpr std::string_view kOmnidirectionalModelName = "omnidirectional_drive";
46 constexpr std::string_view kStationaryModelName = "stationary";
48 constexpr std::string_view kNav2DifferentialModelName = "nav2_amcl::DifferentialMotionModel";
50 constexpr std::string_view kNav2OmnidirectionalModelName = "nav2_amcl::OmniMotionModel";
52 using ExecutionPolicyVariant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
53 
56 class BaseAMCLNode : public rclcpp_lifecycle::LifecycleNode {
57  public:
59  explicit BaseAMCLNode(
60  const std::string& node_name = "amcl",
61  const std::string& node_namespace = "",
62  const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions{});
63 
65  ~BaseAMCLNode() override;
66 
67  protected:
69  CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
70 
72  CallbackReturn on_activate(const rclcpp_lifecycle::State&) override;
73 
75  CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override;
76 
78  CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override;
79 
81  CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override;
82 
85 
88 
91 
93  virtual void do_autostart_callback(){};
94 
95  // Configuration points for extra steps needed for the different AMCL variants.
96 
98  virtual void do_configure([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
100  virtual void do_deactivate([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
102  virtual void do_shutdown([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
104  virtual void do_cleanup([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
106  virtual void do_activate([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
108  virtual void do_periodic_timer_callback() {}
111  [[maybe_unused]] geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) {}
112 
114  void initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message);
115 
117  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr particle_cloud_pub_;
119  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr particle_markers_pub_;
121  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
123  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr likelihood_field_pub_;
125  std::unique_ptr<bond::Bond> bond_;
127  rclcpp::TimerBase::SharedPtr timer_;
129  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
131  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
133  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
135  rclcpp::TimerBase::SharedPtr autostart_timer_;
137  rclcpp::CallbackGroup::SharedPtr common_callback_group_;
139  rclcpp::SubscriptionOptions common_subscription_options_;
141  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
142 };
143 } // namespace beluga_amcl
144 #endif
Definition: ros2_common.hpp:56
void autostart_callback()
Callback for the autostart timer.
virtual void do_activate([[maybe_unused]] const rclcpp_lifecycle::State &state)
Extra steps for the on_activate callback. Defaults to no-op.
Definition: ros2_common.hpp:106
std::unique_ptr< bond::Bond > bond_
Node bond with the lifecycle manager.
Definition: ros2_common.hpp:125
virtual void do_configure([[maybe_unused]] const rclcpp_lifecycle::State &state)
Extra steps for the on_configure callback. Defaults to no-op.
Definition: ros2_common.hpp:98
CallbackReturn on_configure(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from the UNCONFIGURED state to the INACTIVE state.
virtual void do_shutdown([[maybe_unused]] const rclcpp_lifecycle::State &state)
Extra steps for the on_shutdown callback. Defaults to no-op.
Definition: ros2_common.hpp:102
virtual void do_deactivate([[maybe_unused]] const rclcpp_lifecycle::State &state)
Extra steps for the on_deactivate callback. Defaults to no-op.
Definition: ros2_common.hpp:100
std::unique_ptr< tf2_ros::TransformListener > tf_listener_
Transforms listener.
Definition: ros2_common.hpp:133
virtual void do_periodic_timer_callback()
Extra steps for the periodic updates timer callback events. Defaults to no-op.
Definition: ros2_common.hpp:108
rclcpp::SubscriptionOptions common_subscription_options_
Common subscription options.
Definition: ros2_common.hpp:139
rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray >::SharedPtr particle_markers_pub_
Particle markers publisher.
Definition: ros2_common.hpp:119
CallbackReturn on_activate(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.
CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from the INACTIVE state to the UNCONFIGURED state.
CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from most states to the FINALIZED state.
virtual void do_initial_pose_callback([[maybe_unused]] geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message)
Extra steps for (re)initialization messages.
Definition: ros2_common.hpp:110
rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PoseArray >::SharedPtr particle_cloud_pub_
Particle cloud publisher.
Definition: ros2_common.hpp:117
rclcpp::TimerBase::SharedPtr timer_
Timer for periodic particle cloud updates.
Definition: ros2_common.hpp:127
CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override
Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state.
auto get_execution_policy() const -> ExecutionPolicyVariant
Get execution policy from parameters.
std::unique_ptr< tf2_ros::TransformBroadcaster > tf_broadcaster_
Transforms broadcaster.
Definition: ros2_common.hpp:131
virtual void do_cleanup([[maybe_unused]] const rclcpp_lifecycle::State &state)
Extra steps for the on_cleanup callback. Defaults to no-op.
Definition: ros2_common.hpp:104
std::unique_ptr< tf2_ros::Buffer > tf_buffer_
Transforms buffer.
Definition: ros2_common.hpp:129
void initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message)
Callback for (re)initialization messages.
~BaseAMCLNode() override
Destructor for the base AMCL node.
rclcpp::CallbackGroup::SharedPtr common_callback_group_
Common mutually exclusive callback group.
Definition: ros2_common.hpp:137
rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr initial_pose_sub_
Pose (re)initialization subscription.
Definition: ros2_common.hpp:141
rclcpp::TimerBase::SharedPtr autostart_timer_
Timer for node to configure and activate itself.
Definition: ros2_common.hpp:135
rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr pose_pub_
Estimated pose publisher.
Definition: ros2_common.hpp:121
virtual void do_autostart_callback()
User provided extra steps for the autostart process.
Definition: ros2_common.hpp:93
rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::OccupancyGrid >::SharedPtr likelihood_field_pub_
Likelihood field publisher.
Definition: ros2_common.hpp:123
void periodic_timer_callback()
Callback for the periodic particle updates.
BaseAMCLNode(const std::string &node_name="amcl", const std::string &node_namespace="", const rclcpp::NodeOptions &node_options=rclcpp::NodeOptions{})
Constructor, following LifecycleNode signature.
std::variant< std::execution::sequenced_policy, std::execution::parallel_policy > ExecutionPolicyVariant
Supported execution policies.
Definition: ndt_amcl_node.hpp:101