amcl_node.hpp

Go to the documentation of this file.

// Copyright 2022-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_AMCL_NODE_HPP
#define BELUGA_AMCL_AMCL_NODE_HPP
 
#include <memory>
#include <optional>
#include <type_traits>
#include <utility>
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcpp"
#include <message_filters/subscriber.hpp>
#pragma GCC diagnostic pop
 
#include <tf2_ros/buffer.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <bondcpp/bond.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
 
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_srvs/srv/empty.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
 
#include <beluga/beluga.hpp>
#include <beluga_ros/amcl.hpp>
#include "beluga_amcl/message_filters.hpp"
#include "beluga_amcl/ros2_common.hpp"
 
namespace beluga_amcl {
 
class AmclNode : public BaseAMCLNode {
 public:
  using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
  explicit AmclNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
  ~AmclNode() override;
 
 protected:
  void do_activate(const rclcpp_lifecycle::State&) override;
 
  void do_deactivate(const rclcpp_lifecycle::State&) override;
 
  void do_cleanup(const rclcpp_lifecycle::State&) override;
 
  auto get_initial_estimate() const -> std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>>;
 
  auto get_motion_model(std::string_view) const -> beluga_ros::Amcl::motion_model_variant;
 
  auto get_sensor_model(std::string_view, nav_msgs::msg::OccupancyGrid::SharedPtr) const
      -> beluga_ros::Amcl::sensor_model_variant;
 
  auto make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr) const -> std::unique_ptr<beluga_ros::Amcl>;
 
  void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr);
 
  void do_periodic_timer_callback() override;
 
  template <typename TransformT>
  std::optional<TransformT>
  lookup_transform(const std::string& target_frame_id, const std::string& source_frame_id, const tf2::TimePoint& stamp);
 
  std::optional<beluga_ros::LaserScan> wrap_sensor_data(const sensor_msgs::msg::LaserScan::ConstSharedPtr& sensor_msg);
 
  std::optional<beluga_ros::SparsePointCloud3f> wrap_sensor_data(
      const sensor_msgs::msg::PointCloud2::ConstSharedPtr& sensor_msg);
 
  template <typename MessageT>
  void sensor_callback(const std::shared_ptr<const MessageT>& sensor_msg);
 
  void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override;
 
  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);
 
  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);
 
 
  bool initialize_from_estimate(const std::pair<Sophus::SE2d, Eigen::Matrix3d>& estimate);
 
 
  bool initialize_from_map();
 
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
 
  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;
 
  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> point_cloud_sub_;
 
  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr likelihood_field_pub_;
 
  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_localization_server_;
  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_server_;
 
  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
  ::message_filters::Connection laser_scan_connection_;
 
  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> point_cloud_filter_;
 
  ::message_filters::Connection point_cloud_connection_;
 
  std::unique_ptr<beluga_ros::Amcl> particle_filter_;
  std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_;
  std::optional<Sophus::SE2d> last_known_odom_transform_in_map_;
  bool enable_tf_broadcast_{false};
};
 
}  // namespace beluga_amcl
 
#endif  // BELUGA_AMCL_AMCL_NODE_HPP