Migrate bitbots_vision to use generate_parameter_library with optimized parameter passing

.gitignore

@@ -215,6 +215,9 @@ log/
 .pyenv2/*
 .pyenv3/*
 
+# Generated parameter library files
+**/vision_parameters.py
+
 ansible_robots/*
 doc_internal/*
 doku/*

bitbots_vision/bitbots_vision/vision.py

@@ -1,18 +1,15 @@
 #! /usr/bin/env python3
-from copy import deepcopy
 from typing import Optional
 
 import rclpy
 from ament_index_python.packages import get_package_share_directory
 from cv_bridge import CvBridge
 from rcl_interfaces.msg import SetParametersResult
-from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from sensor_msgs.msg import Image
 
 from bitbots_vision.vision_modules import debug, ros_utils, yoeo
-
-from .params import gen
+from bitbots_vision.vision_parameters import bitbots_vision as parameters
 
 logger = rclpy.logging.get_logger("bitbots_vision")
 
@@ -39,20 +36,22 @@ def __init__(self) -> None:
 
         logger.debug(f"Entering {self.__class__.__name__} constructor")
 
+        # Setup parameter listener directly
+        self.param_listener = parameters.ParamListener(self)
+        self.config = self.param_listener.get_params()
+
         self._package_path = get_package_share_directory("bitbots_vision")
 
         yoeo.YOEOObjectManager.set_package_directory(self._package_path)
 
-        self._config: dict = {}
         self._cv_bridge = CvBridge()
 
         self._sub_image = None
 
         self._vision_components: list[yoeo.AbstractVisionComponent] = []
         self._debug_image: Optional[debug.DebugImage] = None
 
-        # Setup reconfiguration
-        gen.declare_params(self)
+        # Setup reconfiguration callback
         self.add_on_set_parameters_callback(self._dynamic_reconfigure_callback)
 
         # Add general params
@@ -61,32 +60,29 @@ def __init__(self) -> None:
         # Update team color
         ros_utils.update_own_team_color(self)
 
-        self._dynamic_reconfigure_callback(self.get_parameters_by_prefix("").values())
+        # Configure vision with initial parameters
+        self._configure_vision(self.config)
 
         logger.debug(f"Leaving {self.__class__.__name__} constructor")
 
     def _dynamic_reconfigure_callback(self, params) -> SetParametersResult:
         """
         Callback for the dynamic reconfigure configuration.
 
-        :param dict params: new config
+        :param params: list of changed parameters
         """
-        new_config = self._get_updated_config_with(params)
-        self._configure_vision(new_config)
-        self._config = new_config
+        # Update the config from the parameter listener
+        self.config = self.param_listener.get_params()
+
+        # Configure vision with the updated config
+        self._configure_vision(self.config)
 
         return SetParametersResult(successful=True)
 
-    def _get_updated_config_with(self, params) -> dict:
-        new_config = deepcopy(self._config)
-        for param in params:
-            new_config[param.name] = param.value
-        return new_config
-
-    def _configure_vision(self, new_config: dict) -> None:
-        yoeo.YOEOObjectManager.configure(new_config)
+    def _configure_vision(self, config) -> None:
+        yoeo.YOEOObjectManager.configure(config.yoeo)
 
-        debug_image = debug.DebugImage(new_config["component_debug_image_active"])
+        debug_image = debug.DebugImage(config.component_debug_image_active)
         self._debug_image = debug_image
 
         def make_vision_component(
@@ -96,39 +92,45 @@ def make_vision_component(
                 node=self,
                 yoeo_handler=yoeo.YOEOObjectManager.get(),
                 debug_image=debug_image,
-                config=new_config,
+                config=config,  # Now passing config object directly
                 **kwargs,
             )
 
         self._vision_components = [make_vision_component(yoeo.YOEOComponent)]
 
-        if new_config["component_ball_detection_active"]:
+        if config.component_ball_detection_active:
             self._vision_components.append(make_vision_component(yoeo.BallDetectionComponent))
-        if new_config["component_robot_detection_active"]:
+        if config.component_robot_detection_active:
             self._vision_components.append(
                 make_vision_component(
                     yoeo.RobotDetectionComponent,
                     team_color_detection_supported=yoeo.YOEOObjectManager.is_team_color_detection_supported(),
                 )
             )
-        if new_config["component_goalpost_detection_active"]:
+        if config.component_goalpost_detection_active:
             self._vision_components.append(make_vision_component(yoeo.GoalpostDetectionComponent))
-        if new_config["component_line_detection_active"]:
+        if config.component_line_detection_active:
             self._vision_components.append(make_vision_component(yoeo.LineDetectionComponent))
-        if new_config["component_field_detection_active"]:
+        if config.component_field_detection_active:
             self._vision_components.append(make_vision_component(yoeo.FieldDetectionComponent))
-        if new_config["component_debug_image_active"]:
+        if config.component_debug_image_active:
             self._vision_components.append(make_vision_component(yoeo.DebugImageComponent))
 
-        self._sub_image = ros_utils.create_or_update_subscriber(
+        # For the subscriber update, use the improved ros_utils function
+        old_topic = getattr(self, '_last_img_topic', None)
+        current_topic = config.ROS_img_msg_topic
+
+        self._sub_image = ros_utils.create_or_update_subscriber_with_config(
             self,
-            self._config,
-            new_config,
+            old_topic,
+            current_topic,
             self._sub_image,
-            "ROS_img_msg_topic",
             Image,
-            callback=self._run_vision_pipeline,
+            self._run_vision_pipeline,
         )
+
+        # Remember this topic for next time
+        self._last_img_topic = current_topic
 
     @profile
     def _run_vision_pipeline(self, image_msg: Image) -> None:

bitbots_vision/bitbots_vision/vision_modules/ros_utils.py

@@ -46,6 +46,32 @@ class RobotColor(Enum):
 own_team_color: RobotColor = RobotColor.UNKNOWN
 
 
+def create_or_update_subscriber_with_config(
+    node, old_topic, new_topic, subscriber_object, data_class, callback, qos_profile=1, callback_group=None
+):
+    """
+    Creates or updates a subscriber using direct topic names instead of config dicts
+
+    :param node: ROS node to which the publisher is bound
+    :param old_topic: Previous topic name
+    :param new_topic: New topic name  
+    :param subscriber_object: The python object, that represents the subscriber
+    :param data_class: Data type class for ROS messages of the topic we want to subscribe
+    :param callback: The subscriber callback function
+    :param qos_profile: A QoSProfile or a history depth to apply to the subscription.
+    :param callback_group: The callback group for the subscription.
+    :return: adjusted subscriber object
+    """
+    # Check if topic has changed
+    if old_topic != new_topic:
+        # Create the new subscriber
+        subscriber_object = node.create_subscription(
+            data_class, new_topic, callback, qos_profile, callback_group=callback_group
+        )
+        logger.debug("Registered new subscriber at " + str(new_topic))
+    return subscriber_object
+
+
 def create_or_update_subscriber(
     node, old_config, new_config, subscriber_object, topic_key, data_class, callback, qos_profile=1, callback_group=None
 ):

bitbots_vision/bitbots_vision/vision_modules/yoeo/object_manager.py

@@ -4,6 +4,7 @@
 import rclpy
 
 from bitbots_vision.vision_modules import ros_utils
+from bitbots_vision.vision_parameters import bitbots_vision as parameters
 
 from . import yoeo_handlers
 from .model_config import ModelConfig, ModelConfigLoader
@@ -72,19 +73,19 @@ def is_team_color_detection_supported(cls) -> bool:
         return cls._model_config.team_colors_are_provided()
 
     @classmethod
-    def configure(cls, config: dict) -> None:
+    def configure(cls, yoeo_config) -> None:
         if not cls._package_directory_set:
             logger.error("Package directory not set!")
 
-        framework = config["yoeo_framework"]
+        framework = yoeo_config.framework
         cls._verify_framework_parameter(framework)
 
-        model_path = cls._get_full_model_path(config["yoeo_model_path"])
+        model_path = cls._get_full_model_path(yoeo_config.model_path)
         cls._verify_required_neural_network_files_exist(framework, model_path)
 
-        cls._configure_yoeo_instance(config, framework, model_path)
+        cls._configure_yoeo_instance(yoeo_config, framework, model_path)
 
-        cls._config = config
+        cls._config = yoeo_config
         cls._framework = framework
         cls._model_path = model_path
 
@@ -107,13 +108,13 @@ def _model_files_exist(cls, framework: str, model_path: str) -> bool:
         return cls._HANDLERS_BY_NAME[framework].model_files_exist(model_path)
 
     @classmethod
-    def _configure_yoeo_instance(cls, config: dict, framework: str, model_path: str) -> None:
+    def _configure_yoeo_instance(cls, yoeo_config, framework: str, model_path: str) -> None:
         if cls._new_yoeo_handler_is_needed(framework, model_path):
             cls._load_model_config(model_path)
             cls._instantiate_new_yoeo_handler(config, framework, model_path)
-        elif cls._yoeo_parameters_have_changed(config):
+        elif cls._yoeo_parameters_have_changed(yoeo_config):
             assert cls._yoeo_instance is not None, "YOEO handler instance not set!"
-            cls._yoeo_instance.configure(config)
+            cls._yoeo_instance.configure(yoeo_config)
 
     @classmethod
     def _new_yoeo_handler_is_needed(cls, framework: str, model_path: str) -> bool:
@@ -124,9 +125,9 @@ def _load_model_config(cls, model_path: str) -> None:
         cls._model_config = ModelConfigLoader.load_from(model_path)
 
     @classmethod
-    def _instantiate_new_yoeo_handler(cls, config: dict, framework: str, model_path: str) -> None:
+    def _instantiate_new_yoeo_handler(cls, yoeo_config, framework: str, model_path: str) -> None:
         cls._yoeo_instance = cls._HANDLERS_BY_NAME[framework](
-            config,
+            yoeo_config,
             model_path,
             cls._model_config.get_detection_classes(),
             cls._model_config.get_robot_class_ids(),
@@ -135,5 +136,9 @@ def _instantiate_new_yoeo_handler(cls, config: dict, framework: str, model_path:
         logger.info(f"Using {cls._yoeo_instance.__class__.__name__}")
 
     @classmethod
-    def _yoeo_parameters_have_changed(cls, new_config: dict) -> bool:
-        return ros_utils.config_param_change(cls._config, new_config, r"yoeo_")
+    def _yoeo_parameters_have_changed(cls, new_yoeo_config) -> bool:
+        if cls._config is None:
+            return True
+
+        # Compare YOEO parameters using the hierarchical structure
+        return cls._config != new_yoeo_config