Add service to reinitialize global localization (#157)

Closes to #141.

Signed-off-by: Olmer Garcia-Bedoya <[email protected]>

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 <class... Args>
   explicit BootstrapParticleFilter(Args&&... args) : Mixin(std::forward<Args>(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());
   }
 

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)

beluga_amcl/include/beluga_amcl/private/amcl_node.hpp

@@ -30,6 +30,7 @@
 #include <nav2_msgs/msg/particle_cloud.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_srvs/srv/empty.hpp>
 
 #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<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+    std::shared_ptr<std_srvs::srv::Empty::Response> response);
   void reinitialize_with_pose(const Eigen::Vector3d & mean, const Eigen::Matrix3d & covariance);
 
   std::unique_ptr<beluga::LaserLocalizationInterface2d> particle_filter_;
@@ -75,6 +80,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     initial_pose_sub_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_localization_server_;
 
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> 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

beluga_amcl/package.xml

@@ -23,6 +23,7 @@
   <depend>rclcpp_components</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>

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<std_srvs::srv::Empty>(
+    "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<rmw_request_id_t> request_header,
+  [[maybe_unused]] const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+  [[maybe_unused]] std::shared_ptr<std_srvs::srv::Empty::Response> 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)