update

gzsim_run.sh

@@ -31,6 +31,17 @@ if ! ps -p "$GZSIM_PID" > /dev/null; then
   exit 1
 fi
 
+echo "[autorun] starting visual topic classifier ..."
+ros2 run bcr_bot visual_topic_classifier.py > "${LOG_DIR}/visual_topic_classifier.log" 2>&1 &
+CLASSIFIER_PID=$!
+
+sleep 2
+if ! ps -p "$CLASSIFIER_PID" > /dev/null; then
+  echo "[autorun] visual topic classifier exited early:"
+  cat "${LOG_DIR}/visual_topic_classifier.log" || true
+  exit 1
+fi
+
 WS_CONFIG="${ROOT}/src/websocket.gzlaunch"
 [[ -f "$WS_CONFIG" ]] || WS_CONFIG="/usr/share/gz/gz-launch7/configs/websocket.gzlaunch"
 

src/bcr_bot/CMakeLists.txt

@@ -2,6 +2,11 @@ cmake_minimum_required(VERSION 3.5)
 project(bcr_bot)
 
 find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+    "srv/GetVisualTopics.srv"
+)
 
 install(DIRECTORY launch meshes models urdf worlds rviz
     DESTINATION share/${PROJECT_NAME}/
@@ -11,7 +16,11 @@ install( DIRECTORY config/
          DESTINATION share/${PROJECT_NAME}/config)
          
          
-install(PROGRAMS scripts/remapper.py
+install(PROGRAMS
+        scripts/remapper.py
+        scripts/visual_topic_classifier.py
         DESTINATION lib/${PROJECT_NAME})
 
+ament_export_dependencies(rosidl_default_runtime)
+
 ament_package()

src/bcr_bot/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
 	<name>bcr_bot</name>
 	<version>1.0.2</version>
 	<description>bcr_bot</description>
@@ -10,6 +10,12 @@
 	<license>Apache License 2.0</license>
 
 	<buildtool_depend>ament_cmake</buildtool_depend>
+	<buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+	<depend>rclpy</depend>
+	<depend>sensor_msgs</depend>
+	<exec_depend>rosidl_default_runtime</exec_depend>
+	<member_of_group>rosidl_interface_packages</member_of_group>
 
 	<exec_depend>ament_index_python</exec_depend>
 	<exec_depend>launch</exec_depend>

src/bcr_bot/scripts/visual_topic_classifier.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+"""Expose visualization-compatible ROS topics for the web plugin."""
+
+from collections import defaultdict
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from sensor_msgs.msg import Image
+
+from bcr_bot.srv import GetVisualTopics
+
+
+IMAGE_TYPE = "sensor_msgs/msg/Image"
+CAMERA_INFO_TYPE = "sensor_msgs/msg/CameraInfo"
+POINT_CLOUD_TYPE = "sensor_msgs/msg/PointCloud2"
+
+DEPTH_ENCODINGS = {"16uc1", "32fc1", "mono16"}
+RGB_ENCODINGS = {
+    "rgb8",
+    "bgr8",
+    "rgba8",
+    "bgra8",
+    "8uc3",
+    "8uc4",
+    "r8g8b8",
+    "b8g8r8",
+}
+
+
+class VisualTopicClassifier(Node):
+    def __init__(self):
+        super().__init__("visual_topic_classifier")
+
+        self._image_encodings = {}
+        self._image_subscriptions = {}
+        self._topics_by_type = defaultdict(set)
+
+        self.create_service(
+            GetVisualTopics, "get_visual_topics", self._get_visual_topics_callback
+        )
+        self.create_timer(1.0, self._scan_topics)
+        self._scan_topics()
+
+        self.get_logger().info(
+            "Visual topic classifier is ready on service '/get_visual_topics'."
+        )
+
+    def _scan_topics(self):
+        topics_by_type = defaultdict(set)
+
+        for topic_name, topic_types in self.get_topic_names_and_types():
+            for topic_type in topic_types:
+                topics_by_type[topic_type].add(topic_name)
+
+            if IMAGE_TYPE in topic_types:
+                self._subscribe_to_image_encoding(topic_name)
+
+        self._topics_by_type = topics_by_type
+
+    def _subscribe_to_image_encoding(self, topic_name):
+        if topic_name in self._image_subscriptions:
+            return
+
+        qos = QoSProfile(
+            depth=1,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+        self._image_subscriptions[topic_name] = self.create_subscription(
+            Image,
+            topic_name,
+            lambda message, name=topic_name: self._cache_image_encoding(name, message),
+            qos,
+        )
+
+    def _cache_image_encoding(self, topic_name, message):
+        encoding = message.encoding.lower()
+        if self._image_encodings.get(topic_name) != encoding:
+            self._image_encodings[topic_name] = encoding
+            self.get_logger().info(
+                "Detected image encoding '%s' on '%s'." % (encoding, topic_name)
+            )
+
+    def _get_visual_topics_callback(self, request, response):
+        if request.refresh:
+            self._scan_topics()
+
+        response.depth_image_topics = self._image_topics_for(DEPTH_ENCODINGS)
+        response.rgb_image_topics = self._image_topics_for(RGB_ENCODINGS)
+        response.camera_info_topics = sorted(self._topics_by_type[CAMERA_INFO_TYPE])
+        response.point_cloud_topics = sorted(self._topics_by_type[POINT_CLOUD_TYPE])
+        return response
+
+    def _image_topics_for(self, expected_encodings):
+        return sorted(
+            topic_name
+            for topic_name in self._topics_by_type[IMAGE_TYPE]
+            if self._image_encodings.get(topic_name) in expected_encodings
+        )
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VisualTopicClassifier()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/bcr_bot/srv/GetVisualTopics.srv

@@ -0,0 +1,9 @@
+# Set true to rescan the ROS graph before returning the cached result.
+bool refresh
+
+---
+
+string[] depth_image_topics
+string[] rgb_image_topics
+string[] camera_info_topics
+string[] point_cloud_topics