ros2bridge: handle /initialpose topic -> relocalize service

MOLAorg · Dec 13, 2024 · f8ddad9 · f8ddad9
1 parent dc623c5
commit f8ddad9
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 0 deletions.
diff --git a/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h b/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h
@@ -134,6 +134,8 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer
 
         bool publish_tf_from_robot_pose_observations = true;
 
+        std::string relocalize_from_topic = "/initialpose";  //!< Default in RViz
+
         /// If true, the original dataset timestamps will be used to publish.
         /// Otherwise, the wallclock time will be used.
         bool publish_in_sim_time = false;
@@ -182,6 +184,8 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer
 
     std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr> subsGNSS_;
 
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subInitPose_;
+
     void callbackOnPointCloud2(
         const sensor_msgs::msg::PointCloud2& o, const std::string& outSensorLabel,
         const std::optional<mrpt::poses::CPose3D>& fixedSensorPose);
@@ -200,6 +204,8 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer
 
     void callbackOnOdometry(const nav_msgs::msg::Odometry& o, const std::string& outSensorLabel);
 
+    void callbackOnRelocalizeTopic(const geometry_msgs::msg::PoseWithCovarianceStamped& o);
+
     bool waitForTransform(
         mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame,
         bool printErrors);

diff --git a/mola_bridge_ros2/src/BridgeROS2.cpp b/mola_bridge_ros2/src/BridgeROS2.cpp
@@ -132,6 +132,16 @@ void BridgeROS2::ros_node_thread_main(Yaml cfg)
                 static_cast<unsigned int>(1e6 * params_.period_publish_static_tfs)),
             [this]() { publishStaticTFs(); });
 
+        //
+        if (!params_.relocalize_from_topic.empty())
+        {
+            subInitPose_ =
+                rosNode()->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+                    params_.relocalize_from_topic, rclcpp::SystemDefaultsQoS(),
+                    [this](const geometry_msgs::msg::PoseWithCovarianceStamped& o)
+                    { this->callbackOnRelocalizeTopic(o); });
+        }
+
         // Spin:
         rclcpp::spin(rosNode_);
 
@@ -1062,6 +1072,17 @@ void BridgeROS2::service_relocalize_near_pose(
     response->accepted = true;
 }
 
+void BridgeROS2::callbackOnRelocalizeTopic(const geometry_msgs::msg::PoseWithCovarianceStamped& o)
+{
+    auto lck = mrpt::lockHelper(rosPubsMtx_);
+
+    for (auto m : molaSubs_.relocalization)
+    {
+        const mrpt::poses::CPose3DPDFGaussian p = mrpt::ros2bridge::fromROS(o.pose);
+        m->relocalize_near_pose_pdf(p);
+    }
+}
+
 void BridgeROS2::service_map_load(
     const std::shared_ptr<mola_msgs::srv::MapLoad::Request> request,
     std::shared_ptr<mola_msgs::srv::MapLoad::Response>      response)