(fix) Sets control callback rate to control time step

automatika-robotics · Dec 3, 2024 · 2bb9e16 · 2bb9e16
1 parent 75b25bc
commit 2bb9e16
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 22 deletions.
diff --git a/kompass/kompass/callbacks.py b/kompass/kompass/callbacks.py
@@ -411,7 +411,7 @@ def _get_output(
                     encoding=track.image.encoding,
                 )
             elif track.compressed_image.data:
-                data: np.ndarray = read_compressed_image(track.compressed_image.data)
+                data: np.ndarray = read_compressed_image(track.compressed_image)
                 img_meta = ImageMetaData(
                     frame_id=track.compressed_image.header.frame_id,
                     width=data.shape[0],

diff --git a/kompass/kompass/components/controller.py b/kompass/kompass/components/controller.py
@@ -374,17 +374,18 @@ def _publishing_callback(self):
                 frame_id=self.config.frames.world,
                 time_stamp=self.get_ros_time(),
             )
-        if (
-            self.vision_trackings
-            and self.config._mode == ControllerMode.VISION_FOLLOWER
-        ):
-            self.get_publisher(ControllerOutputs.TRACKED_POINT.key).publish(
-                self.vision_trackings,
-                frame_id=self.get_callback(
-                    ControllerInputs.VISION_DETECTIONS.key
-                ).frame_id,
-                time_stamp=self.get_ros_time(),
-            )
+        # TODO Fix publishing
+        # if (
+        #     self.vision_trackings
+        #     and self.config._mode == ControllerMode.VISION_FOLLOWER
+        # ):
+        #     self.get_publisher(ControllerOutputs.TRACKED_POINT.key).publish(
+        #         self.vision_trackings,
+        #         frame_id=self.get_callback(
+        #             ControllerInputs.VISION_DETECTIONS.key
+        #         ).frame_id,
+        #         time_stamp=self.get_ros_time(),
+        #     )
         # Publish local plan
         if self.local_plan:
             self.get_publisher(ControllerOutputs.LOCAL_PLAN.key).publish(
@@ -940,21 +941,22 @@ def _end_vision_tracking_srv_callback(
         response.success = False
         return response
 
-    def __wait_for_trackings(self, tracked_id: Optional[int] = None):
+    def __wait_for_trackings(self, tracked_id: Optional[int] = None, max_wait_time: Optional[float] = None):
         """Wait to receive vision_trackings or until timeout
 
         :param tracked_id: Check if id is available in trackings, defaults to None
         :type tracked_id: Optional[int], optional
         """
         self._update_state(vision_track_id=tracked_id)
+        max_wait_time = max_wait_time or self.config.topic_subscription_timeout
         wait_time = 0.0
         while (
             not self.vision_trackings
-            and wait_time < self.config.topic_subscription_timeout
+            and wait_time < max_wait_time
         ):
             self._update_state(vision_track_id=tracked_id)
-            wait_time += 1 / self.config.loop_rate
-            time.sleep(1 / self.config.loop_rate)
+            wait_time += self.config.control_time_step
+            time.sleep(self.config.control_time_step)
 
     def _vision_tracking_callback(self, goal_handle) -> TrackVisionTarget.Result:
         self.get_logger().info("Started vision tracking control action...")
@@ -1003,7 +1005,7 @@ def _vision_tracking_callback(self, goal_handle) -> TrackVisionTarget.Result:
 
         # While the vision tracking mode is not unset by a service call or an event action
         while self.config._mode == ControllerMode.VISION_FOLLOWER:
-            # self.__wait_for_trackings(_tracked_id)
+            # self.__wait_for_trackings(_tracked_id, max_wait_time=)
             self._update_state(vision_track_id=_tracked_id)
 
             current_tracking = copy(self.vision_trackings)
@@ -1021,11 +1023,11 @@ def _vision_tracking_callback(self, goal_handle) -> TrackVisionTarget.Result:
             self.get_logger().info(f"track: {current_tracking}")
             # Publish feedback
             feedback_msg.center_xy = (
-                current_tracking.center_xy if current_tracking else 0.0
+                current_tracking.center_xy if current_tracking else [0.0, 0.0]
             )
-            feedback_msg.size_xy = current_tracking.size_xy if current_tracking else 0.0
-            feedback_msg.distance = 0.0
-            feedback_msg.orientation = 0.0
+            feedback_msg.size_xy = current_tracking.size_xy if current_tracking else [0.0, 0.0]
+            feedback_msg.distance = 0.0     # TODO
+            feedback_msg.orientation = 0.0  # TODO
 
             # Publish control
             self._publish(
@@ -1044,7 +1046,7 @@ def _vision_tracking_callback(self, goal_handle) -> TrackVisionTarget.Result:
 
             goal_handle.publish_feedback(feedback_msg)
 
-            time.sleep(1 / self.config.loop_rate)
+            time.sleep(self.config.control_time_step)
 
         end_time = time.time()