ros2_common.hpp

// 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 <rclcpp/rclcpp.hpp>
#include <rclcpp/time.hpp>
 
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_ros/create_timer_ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
 
#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_;
  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