feat: added subscription to localization status topic

README.md

@@ -114,8 +114,9 @@ roslaunch place_recognition visual_pr.launch
 
 - `config_path`: путь к yaml-конфигу алгоритма. Значение по умолчанию: `/home/docker_place_recognition/catkin_ws/src/place_recognition/config/netvlad_config.yaml`.
 - `namespace`: пространство имен публикуемых узлом топиков (т.е. "приставка" в начале названия топика). Значение по умолчанию: `/visual_pr`.
-- `image_topic`: имя топика входящий query-изображений. Значение по умолчанию: `/pr_query_image/compressed`.
-- `pose_topic`: имя топика, в который будут публиковаться данные о найденной reference-позе. Значение по умолчанию: `/estimated_pose`. Полное имя топика будет `namespace` + `pose_topic`.
+- `image_topic`: имя топика входящих query-изображений. Значение по умолчанию: `/zed_node/left/image_rect_color/compressed`.
+- `pose_topic`: имя топика, в который будут публиковаться данные о найденной reference-позе. Значение по умолчанию: `/initialpose`.
+- `localization_status_topic`: топик со статусом локализации, на который будет подписываться узел. Значение по умолчанию: `/localization_status`.
 
 ## Описание узлов
 
@@ -134,9 +135,10 @@ TODO:
 - Patch-NetVLAD: [arxiv](https://arxiv.org/abs/2103.01486v1), [github.com/QVPR/Patch-NetVLAD](https://github.com/QVPR/Patch-NetVLAD)
 - CosPlace: [arxiv](https://arxiv.org/abs/2204.02287), [github.com/gmberton/CosPlace](https://github.com/gmberton/CosPlace)
 
-Узел подписывается на топики:
--  `/pr_query_image/compressed`: изображение-запрос, тип `sensor_msgs/CompressedImage`.
+#### Узел подписывается на топики:
+- `image_topic`: изображение-запрос, тип `sensor_msgs/CompressedImage`.
+- `localization_status_topic`: текущий статус локализации, тип `std_msgs/Bool`. Логика: значение `false` означает потерю локализации и необходимость определения места методом Place Recognition.
 
-Узел публикует сообщения в топики:
+#### Узел публикует сообщения в топики:
 - `/visual_pr/estimated_pose`: поза наиболее похожего изображения-места из БД, тип `geometry_msgs/PoseStamped`;
 - `/visual_pr/ref_image_ts`: timestamp наиболее похожего изображения-места из БД (может быть полезно для визуализации и отладки), тип `std_msgs/String`.

place_recognition/launch/ref_visualizer.launch

+<launch>
+
+    <!-- Параметры узла -->
+    <arg name="config_path" default="/home/docker_place_recognition/catkin_ws/src/place_recognition/config/netvlad_config.yaml" />
+
+    <!-- Nodes -->
+    <node pkg="place_recognition" type="ref_vis_node.py" name="ref_vis_node" output="screen">
+            <param name="config_path" value="$(arg config_path)" />
+    </node>
+
+
+</launch>
\ No newline at end of file

place_recognition/launch/visual_pr.launch

@@ -5,13 +5,15 @@
 
     <!-- Настройка топиков -->
     <arg name="namespace" default="/visual_pr" />
-    <arg name="image_topic" default="/pr_query_image/compressed" />
-    <arg name="pose_topic" default="/estimated_pose" />
+    <arg name="image_topic" default="/zed_node/left/image_rect_color/compressed" />
+    <arg name="pose_topic" default="/initialpose" />
+    <arg name="localization_status_topic" default="/localization_status" />
 
     <!-- Nodes -->
     <group ns="$(arg namespace)">
         <remap from="image" to="$(arg image_topic)" />
-        <remap from="estimated_pose" to="$(arg namespace)$(arg pose_topic)" />
+        <remap from="estimated_pose" to="$(arg pose_topic)" />
+        <remap from="localization_status" to="$(arg localization_status_topic)" />
 
         <node pkg="place_recognition" type="visual_pr_node.py" name="visual_pr_node" output="screen">
             <param name="config_path" value="$(arg config_path)" />

place_recognition/scripts/visual_pr_node.py

@@ -13,9 +13,9 @@ import rospy
 import message_filters
 from PIL import Image
 
-from std_msgs.msg import String
+from std_msgs.msg import String, Bool
 from sensor_msgs.msg import CompressedImage
-from geometry_msgs.msg import Pose, PoseStamped
+from geometry_msgs.msg import Pose, PoseWithCovarianceStamped
 
 from src.utils import convert_dict_to_tuple
 from src.models.model_factory import get_model
@@ -40,14 +40,34 @@ class VisualPRNode:
 
         self.db_csv = pd.read_csv(self.config.database.csv, index_col=0, dtype={'image_ts': str, 'lidar_ts': str})
 
-        self.sub_image = message_filters.Subscriber('image', CompressedImage, queue_size=1)
-        self.pub_pose = rospy.Publisher('estimated_pose', PoseStamped, queue_size=1)
+        # self.sub_image = message_filters.Subscriber('image', CompressedImage, queue_size=1)
+        self.sub_localization_status = message_filters.Subscriber('localization_status', Bool, queue_size=1)
+        self.pub_pose = rospy.Publisher('estimated_pose', PoseWithCovarianceStamped, queue_size=1)
         self.pub_ref_image = rospy.Publisher('ref_image_ts', String, queue_size=1)
 
-        self.sub_image.registerCallback(self.on_image)
+        # self.sub_image.registerCallback(self.on_image)
+        self.sub_localization_status.registerCallback(self.on_localization_status)
+    
+        self.last_update_time = rospy.Time().now() - rospy.Duration(secs=5)  # init value "5 secs ago"
 
         rospy.loginfo("visual_pr_node is ready")
 
+    def on_localization_status(self, localization_status_msg: Bool):
+        rospy.logdebug("Received localization_status message")
+        if localization_status_msg.data:
+            return  # localization_status == True  ->  localization is okay
+        else:  # the robot got lost
+            if (rospy.Time().now() - self.last_update_time) < rospy.Duration(secs=5):
+                rospy.logdebug("Last update was sent less then 5 secs ago")
+                return
+            try:
+                image_msg = rospy.wait_for_message('image', CompressedImage, timeout=5)
+            except rospy.ROSException:
+                rospy.logerr("image_msg timeout (5 secs) exeeded")
+            else:
+                self.on_image(image_msg)
+                self.last_update_time = rospy.Time().now()
+
     def on_image(self, compressed_image_msg: CompressedImage):
         rospy.logdebug("Received CompressedImage message")
         start_time = time()
@@ -88,10 +108,10 @@ class VisualPRNode:
         pose_msg.orientation.z = orientation[2]
         pose_msg.orientation.w = orientation[3]
 
-        pose_stamped_msg = PoseStamped()
+        pose_stamped_msg = PoseWithCovarianceStamped()
         pose_stamped_msg.header.stamp = stamp
         pose_stamped_msg.header.frame_id = frame_id
-        pose_stamped_msg.pose = pose_msg
+        pose_stamped_msg.pose.pose = pose_msg
 
         return pose_stamped_msg