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,35 @@ 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_from_config()
 
         logger.debug(f"Leaving {self.__class__.__name__} constructor")
 
+    def _configure_vision_from_config(self) -> None:
+        """
+        Configure vision components using the current config.
+        """
+        self._configure_vision(self.config)
+
     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_from_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)
 
-        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 +98,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(
-            self,
-            self._config,
-            new_config,
-            self._sub_image,
-            "ROS_img_msg_topic",
-            Image,
-            callback=self._run_vision_pipeline,
-        )
+        # For the subscriber update, handle the topic name directly
+        old_topic = getattr(self, '_last_img_topic', None)
+        current_topic = config.ROS_img_msg_topic
+
+        if old_topic != current_topic:
+            self._sub_image = self.create_subscription(
+                Image, 
+                current_topic, 
+                self._run_vision_pipeline, 
+                1
+            )
+            logger.debug(f"Registered new subscriber at {current_topic}")
+
+        # 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/yoeo/object_manager.py

@@ -72,14 +72,14 @@ 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, config) -> None:
         if not cls._package_directory_set:
             logger.error("Package directory not set!")
 
-        framework = config["yoeo_framework"]
+        framework = config.yoeo_framework
         cls._verify_framework_parameter(framework)
 
-        model_path = cls._get_full_model_path(config["yoeo_model_path"])
+        model_path = cls._get_full_model_path(config.yoeo_model_path)
         cls._verify_required_neural_network_files_exist(framework, model_path)
 
         cls._configure_yoeo_instance(config, framework, model_path)
@@ -107,7 +107,7 @@ 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, 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)
@@ -124,7 +124,7 @@ 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, config, framework: str, model_path: str) -> None:
         cls._yoeo_instance = cls._HANDLERS_BY_NAME[framework](
             config,
             model_path,
@@ -135,5 +135,15 @@ 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_config) -> bool:
+        if cls._config is None:
+            return True
+
+        # Compare YOEO-related parameters using direct attribute access
+        return (
+            cls._config.yoeo_framework != new_config.yoeo_framework or
+            cls._config.yoeo_model_path != new_config.yoeo_model_path or
+            cls._config.yoeo_nms_threshold != new_config.yoeo_nms_threshold or
+            cls._config.yoeo_conf_threshold != new_config.yoeo_conf_threshold or
+            cls._config.caching != new_config.caching
+        )

bitbots_vision/bitbots_vision/vision_modules/yoeo/vision_components.py

@@ -57,17 +57,17 @@ def __init__(
     ):
         super().__init__(node, yoeo_handler, debug_image, config)
 
-        self._publisher = self._node.create_publisher(BallArray, self._config["ROS_ball_msg_topic"], qos_profile=1)
+        self._publisher = self._node.create_publisher(BallArray, self._config.ROS_ball_msg_topic, qos_profile=1)
 
     def run(self, image: np.ndarray, header: Header) -> None:
         # Get all ball candidates from YOEO
         candidates = self._yoeo_handler.get_detection_candidates_for("ball")
 
         # Filter candidates by rating and count
         candidates = candidate.Candidate.sort_candidates(candidates)
-        top_candidates = candidates[: self._config["ball_candidate_max_count"]]
+        top_candidates = candidates[: self._config.ball_candidate_max_count]
         final_candidates = candidate.Candidate.rating_threshold(
-            top_candidates, self._config["ball_candidate_rating_threshold"]
+            top_candidates, self._config.ball_candidate_rating_threshold
         )
 
         # Publish ball candidates
@@ -95,7 +95,7 @@ def __init__(
         super().__init__(node, yoeo_handler, debug_image, config)
 
         self._publisher = self._node.create_publisher(
-            GoalpostArray, self._config["ROS_goal_posts_msg_topic"], qos_profile=1
+            GoalpostArray, self._config.ROS_goal_posts_msg_topic, qos_profile=1
         )
 
     def run(self, image: np.ndarray, header: Header) -> None:
@@ -125,7 +125,7 @@ def __init__(
         self, node: Node, yoeo_handler: yoeo_handlers.IYOEOHandler, debug_image: debug.DebugImage, config: dict
     ):
         super().__init__(node, yoeo_handler, debug_image, config)
-        self._publisher = self._node.create_publisher(Image, self._config["ROS_line_mask_msg_topic"], qos_profile=1)
+        self._publisher = self._node.create_publisher(Image, self._config.ROS_line_mask_msg_topic, qos_profile=1)
 
     def run(self, image: np.ndarray, header: Header) -> None:
         # Get line mask from YOEO
@@ -153,7 +153,7 @@ def __init__(
     ):
         super().__init__(node, yoeo_handler, debug_image, config)
         self._publisher = self._node.create_publisher(
-            Image, self._config["ROS_field_mask_image_msg_topic"], qos_profile=1
+            Image, self._config.ROS_field_mask_image_msg_topic, qos_profile=1
         )
 
     def run(self, image: np.ndarray, header: Header) -> None:
@@ -185,7 +185,7 @@ def __init__(
         super().__init__(node, yoeo_handler, debug_image, config)
 
         self._team_color_detection_supported = team_color_detection_supported
-        self._publisher = self._node.create_publisher(RobotArray, self._config["ROS_robot_msg_topic"], qos_profile=1)
+        self._publisher = self._node.create_publisher(RobotArray, self._config.ROS_robot_msg_topic, qos_profile=1)
 
     def run(self, image: np.ndarray, header: Header) -> None:
         robot_msgs: list[Robot] = []
@@ -282,7 +282,7 @@ def __init__(
     ):
         super().__init__(node, yoeo_handler, debug_image, config)
 
-        self._publisher = self._node.create_publisher(Image, self._config["ROS_debug_image_msg_topic"], qos_profile=1)
+        self._publisher = self._node.create_publisher(Image, self._config.ROS_debug_image_msg_topic, qos_profile=1)
 
     def run(self, image: np.ndarray, header: Header) -> None:
         debug_image_msg = ros_utils.build_image_msg(header, self._debug_image.get_image(), "bgr8")