/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.
82 auto make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr) const -> std::unique_ptr<beluga_ros::Amcl>;
94 void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override;
132 std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan, rclcpp_lifecycle::LifecycleNode>>
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.
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
Generated by 1.9.1