/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
 // Copyright 2024 Ekumen, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
 //
 //     http://www.apache.org/licenses/LICENSE-2.0
 //
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
  
 #ifndef BELUGA_AMCL_ROS2_COMMON_HPP
 #define BELUGA_AMCL_ROS2_COMMON_HPP
  
 #include <execution>
 #include <rclcpp/node_options.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 #include <string>
 #include <string_view>
  
 #include <bondcpp/bond.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/time.hpp>
  
 #include <tf2_ros/create_timer_ros.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2/convert.hpp>
 #include <tf2/utils.hpp>
  
 #include <beluga_ros/tf2_sophus.hpp>
  
 namespace beluga_amcl {
  
 constexpr std::string_view kDifferentialModelName = "differential_drive";
 constexpr std::string_view kOmnidirectionalModelName = "omnidirectional_drive";
 constexpr std::string_view kStationaryModelName = "stationary";
 constexpr std::string_view kNav2DifferentialModelName = "nav2_amcl::DifferentialMotionModel";
 constexpr std::string_view kNav2OmnidirectionalModelName = "nav2_amcl::OmniMotionModel";
 using ExecutionPolicyVariant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
  
 class BaseAMCLNode : public rclcpp_lifecycle::LifecycleNode {
  public:
   explicit BaseAMCLNode(
       const std::string& node_name = "amcl",
       const std::string& node_namespace = "",
       const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions{});
  
   ~BaseAMCLNode() override;
  
  protected:
   CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
  
   CallbackReturn on_activate(const rclcpp_lifecycle::State&) override;
  
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override;
  
   CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override;
  
   CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override;
  
   auto get_execution_policy() const -> ExecutionPolicyVariant;
  
   void periodic_timer_callback();
  
   void autostart_callback();
  
   virtual void do_autostart_callback(){};
  
   // Configuration points for extra steps needed for the different AMCL variants.
  
   virtual void do_configure([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
   virtual void do_deactivate([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
   virtual void do_shutdown([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
   virtual void do_cleanup([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
   virtual void do_activate([[maybe_unused]] const rclcpp_lifecycle::State& state) {}
   virtual void do_periodic_timer_callback() {}
   virtual void do_initial_pose_callback(
       [[maybe_unused]] geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) {}
  
   void initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message);
  
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr particle_cloud_pub_;
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr particle_markers_pub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr likelihood_field_pub_;
   std::unique_ptr<bond::Bond> bond_;
   rclcpp::TimerBase::SharedPtr timer_;
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
   std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
   rclcpp::TimerBase::SharedPtr autostart_timer_;
   rclcpp::CallbackGroup::SharedPtr common_callback_group_;
   rclcpp::SubscriptionOptions common_subscription_options_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
 };
 }  // namespace beluga_amcl
 #endif