From fc83627e83aa3f20b5bd4113ec42c1c53771492f Mon Sep 17 00:00:00 2001 From: Huang77 <2366619700@qq.com> Date: Mon, 10 Aug 2026 15:48:34 +0800 Subject: [PATCH] update --- gzsim_run.sh | 11 ++ src/bcr_bot/CMakeLists.txt | 11 +- src/bcr_bot/package.xml | 8 +- .../scripts/visual_topic_classifier.py | 118 ++++++++++++++++++ src/bcr_bot/srv/GetVisualTopics.srv | 9 ++ 5 files changed, 155 insertions(+), 2 deletions(-) create mode 100644 src/bcr_bot/scripts/visual_topic_classifier.py create mode 100644 src/bcr_bot/srv/GetVisualTopics.srv diff --git a/gzsim_run.sh b/gzsim_run.sh index b14984b..bf828ed 100755 --- a/gzsim_run.sh +++ b/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" diff --git a/src/bcr_bot/CMakeLists.txt b/src/bcr_bot/CMakeLists.txt index 5ef2cd9..4656380 100644 --- a/src/bcr_bot/CMakeLists.txt +++ b/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() diff --git a/src/bcr_bot/package.xml b/src/bcr_bot/package.xml index 4f3afc9..f1ad468 100644 --- a/src/bcr_bot/package.xml +++ b/src/bcr_bot/package.xml @@ -1,5 +1,5 @@ - + bcr_bot 1.0.2 bcr_bot @@ -10,6 +10,12 @@ Apache License 2.0 ament_cmake + rosidl_default_generators + + rclpy + sensor_msgs + rosidl_default_runtime + rosidl_interface_packages ament_index_python launch diff --git a/src/bcr_bot/scripts/visual_topic_classifier.py b/src/bcr_bot/scripts/visual_topic_classifier.py new file mode 100644 index 0000000..d4fa701 --- /dev/null +++ b/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() diff --git a/src/bcr_bot/srv/GetVisualTopics.srv b/src/bcr_bot/srv/GetVisualTopics.srv new file mode 100644 index 0000000..12c0ca6 --- /dev/null +++ b/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