From 9b09004d6fe900ae59c42a89b3f0dcb4008ba1f5 Mon Sep 17 00:00:00 2001 From: Olmer Garcia-Bedoya Date: Wed, 10 May 2023 13:51:05 -0300 Subject: [PATCH] Add service to reinitialize global localization (#157) Closes to #141. Signed-off-by: Olmer Garcia-Bedoya --- .../beluga/algorithm/particle_filter.hpp | 17 ++++++++++++ beluga_amcl/CMakeLists.txt | 4 ++- .../include/beluga_amcl/private/amcl_node.hpp | 8 ++++++ beluga_amcl/package.xml | 1 + beluga_amcl/src/amcl_node.cpp | 26 ++++++++++++++++++- 5 files changed, 54 insertions(+), 2 deletions(-) diff --git a/beluga/include/beluga/algorithm/particle_filter.hpp b/beluga/include/beluga/algorithm/particle_filter.hpp index 8f7eed35d..0bea86647 100644 --- a/beluga/include/beluga/algorithm/particle_filter.hpp +++ b/beluga/include/beluga/algorithm/particle_filter.hpp @@ -100,6 +100,14 @@ struct BaseParticleFilterInterface { * If resampling is performed, the importance weights of all particles are reset to 1.0. */ virtual void resample() = 0; + + /// Distribute the particles over all the space. + /** + * \overload + * this step will reinitialize the position of each particle based + * in some predefined distribution. + */ + virtual void reinitialize() = 0; }; /// Base implementation of a particle filter. @@ -133,6 +141,15 @@ class BootstrapParticleFilter : public Mixin { */ template explicit BootstrapParticleFilter(Args&&... args) : Mixin(std::forward(args)...) { + reinitialize(); + } + + /** + * \copydoc BaseParticleFilterInterface::reinitialize() + * + * Distribute the particles using the \ref StateGeneratorPage "StateGenerator" + */ + void reinitialize() final { this->self().initialize_particles(this->self().generate_samples(generator_) | this->self().take_samples()); } diff --git a/beluga_amcl/CMakeLists.txt b/beluga_amcl/CMakeLists.txt index fe1b8a709..f55a82eb5 100644 --- a/beluga_amcl/CMakeLists.txt +++ b/beluga_amcl/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -48,7 +49,8 @@ ament_target_dependencies(amcl_node_component PUBLIC rclcpp rclcpp_components rclcpp_lifecycle - sensor_msgs) + sensor_msgs + std_srvs) rclcpp_components_register_nodes(amcl_node_component "beluga_amcl::AmclNode") add_executable(amcl_node) diff --git a/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp index eaa77c2e1..74e860e56 100644 --- a/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include "beluga_amcl/private/execution_policy.hpp" @@ -58,6 +59,10 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode ExecutionPolicy && exec_policy, sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan); void initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + void global_localization_callback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); void reinitialize_with_pose(const Eigen::Vector3d & mean, const Eigen::Matrix3d & covariance); std::unique_ptr particle_filter_; @@ -75,6 +80,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode rclcpp::Subscription::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Service::SharedPtr global_localization_server_; std::unique_ptr tf_buffer_; std::unique_ptr tf_broadcaster_; @@ -85,6 +91,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode message_filters::Connection laser_scan_connection_; Sophus::SE2d last_odom_to_base_transform_; + + bool enable_tf_broadcast_{false}; }; } // namespace beluga_amcl diff --git a/beluga_amcl/package.xml b/beluga_amcl/package.xml index 6504c5d9e..023a5d1b2 100644 --- a/beluga_amcl/package.xml +++ b/beluga_amcl/package.xml @@ -23,6 +23,7 @@ rclcpp_components rclcpp_lifecycle sensor_msgs + std_srvs tf2 tf2_eigen tf2_geometry_msgs diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 61f396457..628b6698b 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -598,6 +598,12 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State &) rclcpp::SystemDefaultsQoS(), std::bind(&AmclNode::initial_pose_callback, this, std::placeholders::_1)); + global_localization_server_ = create_service( + "reinitialize_global_localization", + std::bind( + &AmclNode::global_localization_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + RCLCPP_INFO( get_logger(), "Subscribed to initial_pose_topic: %s", @@ -653,6 +659,7 @@ AmclNode::CallbackReturn AmclNode::on_deactivate(const rclcpp_lifecycle::State & tf_listener_.reset(); tf_broadcaster_.reset(); tf_buffer_.reset(); + global_localization_server_.reset(); bond_.reset(); return CallbackReturn::SUCCESS; } @@ -662,6 +669,8 @@ AmclNode::CallbackReturn AmclNode::on_cleanup(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Cleaning up"); particle_cloud_pub_.reset(); pose_pub_.reset(); + enable_tf_broadcast_ = false; + particle_filter_.reset(); return CallbackReturn::SUCCESS; } @@ -919,7 +928,7 @@ void AmclNode::laser_callback( pose_pub_->publish(message); } - if (get_parameter("tf_broadcast").as_bool()) { + if (enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) { auto message = geometry_msgs::msg::TransformStamped{}; // Sending a transform that is valid into the future so that odom can be used. message.header.stamp = now() + @@ -978,11 +987,26 @@ void AmclNode::reinitialize_with_pose( const auto sample = distribution(generator); return Sophus::SE2d{Sophus::SO2d{sample.z()}, Eigen::Vector2d{sample.x(), sample.y()}}; })); + enable_tf_broadcast_ = true; } catch (const std::runtime_error & error) { RCLCPP_ERROR(get_logger(), "Could not generate particles: %s", error.what()); } } +void AmclNode::global_localization_callback( + [[maybe_unused]] const std::shared_ptr request_header, + [[maybe_unused]] const std::shared_ptr req, + [[maybe_unused]] std::shared_ptr res) +{ + if (!particle_filter_) { + return; + } + RCLCPP_INFO(get_logger(), "Initializing with uniform distribution"); + particle_filter_->reinitialize(); + RCLCPP_INFO(get_logger(), "Global initialization done!"); + enable_tf_broadcast_ = true; +} + } // namespace beluga_amcl RCLCPP_COMPONENTS_REGISTER_NODE(beluga_amcl::AmclNode)