Remove global cost_map scan expected rate to account for delays

src/go2_robot_sdk/calibration/front_camera_480.yaml

@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: front_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [436.92609,   0.     , 339.57048,
+           0.     , 586.78253, 283.05632,
+           0.     ,   0.     ,   1.     ]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.415971, 0.158898, -0.015395, -0.008031, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1., 0., 0.,
+         0., 1., 0.,
+         0., 0., 1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [330.75336,   0.     , 331.36332,   0.     ,
+           0.     , 542.10351, 284.02454,   0.     ,
+           0.     ,   0.     ,   1.     ,   0.     ]

src/go2_robot_sdk/config/nav2_params.yaml

@@ -1763,7 +1763,7 @@ global_costmap:
           # Expected: Sensor's actual update rate (should match increased processing rate)
           # Impact: Used for timeout detection of stale sensor data
           # 10.0Hz matches increased global costmap processing for fast obstacle clearing
-          expected_update_rate: 10.0
+          # expected_update_rate: 10.0
 
           # min_obstacle_height: Minimum height to consider as obstacle (meters)
           # Expected: 0.0-0.3 (depends on what robot should avoid)

src/go2_robot_sdk/go2_robot_sdk/application/services/robot_data_service.py

@@ -70,12 +70,19 @@ def _process_odometry_data(self, msg: Dict[str, Any], robot_data: RobotData) ->
             position = pose_data['position']
             orientation = pose_data['orientation']
 
-            # Data validation
+            # Data validation - Phase 1 optimization: type-check first for faster rejection
             pos_vals = [position['x'], position['y'], position['z']]
             rot_vals = [orientation['x'], orientation['y'], orientation['z'], orientation['w']]
+            all_vals = pos_vals + rot_vals
 
-            if not all(isinstance(v, (int, float)) and math.isfinite(v) for v in pos_vals + rot_vals):
-                logger.warning("Invalid odometry data - skipping")
+            # Fast path: check types first (cheaper operation)
+            if not all(isinstance(v, (int, float)) for v in all_vals):
+                logger.warning("Invalid odometry data types - skipping")
+                return
+
+            # Only check finite values if types are correct (more expensive operation)
+            if not all(math.isfinite(v) for v in all_vals):
+                logger.warning("Invalid odometry data values - skipping")
                 return
 
             robot_data.odometry_data = OdometryData(
@@ -90,16 +97,23 @@ def _process_sport_mode_state(self, msg: Dict[str, Any], robot_data: RobotData)
         try:
             data = msg["data"]
 
-            # Data validation
-            if not self._validate_float_list(data.get("position", [])):
-                return
-            if not self._validate_float_list(data.get("range_obstacle", [])):
-                return
-            if not self._validate_float_list(data.get("foot_position_body", [])):
-                return
-            if not self._validate_float_list(data.get("foot_speed_body", [])):
-                return
+            # Data validation - Phase 1 optimization: consolidated validation to reduce function call overhead
+            validation_fields = [
+                ("position", data.get("position", [])),
+                ("range_obstacle", data.get("range_obstacle", [])),
+                ("foot_position_body", data.get("foot_position_body", [])),
+                ("foot_speed_body", data.get("foot_speed_body", []))
+            ]
+
+            # Validate all float lists in one pass
+            for field_name, field_data in validation_fields:
+                if not self._validate_float_list(field_data):
+                    logger.warning(f"Invalid sport mode state data in field '{field_name}' - skipping")
+                    return
+
+            # Validate single float value
             if not self._validate_float(data.get("body_height")):
+                logger.warning("Invalid sport mode state body_height - skipping")
                 return
 
             robot_data.robot_state = RobotState(
@@ -144,9 +158,17 @@ def _process_low_state(self, msg: Dict[str, Any], robot_data: RobotData) -> None
             logger.error(f"Error processing low state: {e}")
 
     def _validate_float_list(self, data: list) -> bool:
-        """Validate a list of float values"""
-        return all(isinstance(x, (int, float)) and math.isfinite(x) for x in data)
+        """Validate a list of float values - Phase 1 optimization: type-check first"""
+        # Fast path: check types first (cheaper operation)
+        if not all(isinstance(x, (int, float)) for x in data):
+            return False
+        # Only check finite values if types are correct (more expensive operation)
+        return all(math.isfinite(x) for x in data)
 
     def _validate_float(self, value: Any) -> bool:
-        """Validate a float value"""
-        return isinstance(value, (int, float)) and math.isfinite(value) 
+        """Validate a float value - Phase 1 optimization: type-check first"""
+        # Fast path: check type first (cheaper operation)
+        if not isinstance(value, (int, float)):
+            return False
+        # Only check finite if type is correct (more expensive operation)
+        return math.isfinite(value) 

src/go2_robot_sdk/go2_robot_sdk/infrastructure/ros2/ros2_publisher.py

@@ -42,19 +42,22 @@ def publish_odometry(self, robot_data: RobotData) -> None:
         try:
             robot_idx = int(robot_data.robot_id)
 
+            # Single timestamp for both transform and odometry to ensure logical consistency
+            current_time = self.node.get_clock().now().to_msg()
+
             # Publish transform
-            self._publish_transform(robot_data, robot_idx)
+            self._publish_transform(robot_data, robot_idx, current_time)
 
             # Publish odometry topic
-            self._publish_odometry_topic(robot_data, robot_idx)
+            self._publish_odometry_topic(robot_data, robot_idx, current_time)
 
         except Exception as e:
             logger.error(f"Error publishing odometry: {e}")
 
-    def _publish_transform(self, robot_data: RobotData, robot_idx: int) -> None:
+    def _publish_transform(self, robot_data: RobotData, robot_idx: int, current_time) -> None:
         """Publish TF transform"""
         odom_trans = TransformStamped()
-        odom_trans.header.stamp = self.node.get_clock().now().to_msg()
+        odom_trans.header.stamp = current_time
         odom_trans.header.frame_id = 'odom'
 
         if self.config.conn_mode == 'single':
@@ -76,10 +79,10 @@ def _publish_transform(self, robot_data: RobotData, robot_idx: int) -> None:
 
         self.broadcaster.sendTransform(odom_trans)
 
-    def _publish_odometry_topic(self, robot_data: RobotData, robot_idx: int) -> None:
+    def _publish_odometry_topic(self, robot_data: RobotData, robot_idx: int, current_time) -> None:
         """Publish Odometry topic"""
         odom_msg = Odometry()
-        odom_msg.header.stamp = self.node.get_clock().now().to_msg()
+        odom_msg.header.stamp = current_time
         odom_msg.header.frame_id = 'odom'
 
         if self.config.conn_mode == 'single':
@@ -108,8 +111,11 @@ def publish_joint_state(self, robot_data: RobotData) -> None:
 
         try:
             robot_idx = int(robot_data.robot_id)
+            # Phase 1 optimization: single timestamp call per message to reduce system call overhead
+            current_time = self.node.get_clock().now().to_msg()
+
             joint_state = JointState()
-            joint_state.header.stamp = self.node.get_clock().now().to_msg()
+            joint_state.header.stamp = current_time
 
             # Define joint names
             if self.config.conn_mode == 'single':
@@ -196,9 +202,12 @@ def publish_lidar_data(self, robot_data: RobotData) -> None:
                 0
             )
 
+            # Phase 1 optimization: single timestamp call per message to reduce system call overhead
+            current_time = self.node.get_clock().now().to_msg()
+
             point_cloud = PointCloud2()
             point_cloud.header = Header(frame_id="odom")
-            point_cloud.header.stamp = self.node.get_clock().now().to_msg()
+            point_cloud.header.stamp = current_time
 
             fields = [
                 PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
@@ -213,6 +222,16 @@ def publish_lidar_data(self, robot_data: RobotData) -> None:
         except Exception as e:
             logger.error(f"Error publishing lidar data: {e}")
 
+    def has_camera_subscribers(self, robot_id: str) -> bool:
+        """Check if there are active subscribers to camera topics"""
+        try:
+            robot_idx = int(robot_id)
+            camera_subs = self.publishers['camera'][robot_idx].get_subscription_count()
+            camera_info_subs = self.publishers['camera_info'][robot_idx].get_subscription_count()
+            return camera_subs > 0 or camera_info_subs > 0
+        except Exception:
+            return True  # Default to processing if check fails
+
     def publish_camera_data(self, robot_data: RobotData) -> None:
         """Publish camera data"""
         if not robot_data.camera_data:
@@ -222,13 +241,23 @@ def publish_camera_data(self, robot_data: RobotData) -> None:
             robot_idx = int(robot_data.robot_id)
             camera = robot_data.camera_data
 
+            # Phase 1 optimization: single timestamp call per message to reduce system call overhead
+            current_time = self.node.get_clock().now().to_msg()
+
             # Convert to ROS Image
             ros_image = self.bridge.cv2_to_imgmsg(camera.image, encoding=camera.encoding)
-            ros_image.header.stamp = self.node.get_clock().now().to_msg()
-
-            # Camera info
+            ros_image.header.stamp = current_time
+
+            # Camera info - reuse same timestamp for consistency
+            # Phase 1: Direct lookup with explicit error handling (no fallback)
+            if camera.height not in self.camera_info:
+                available = list(self.camera_info.keys())
+                raise ValueError(f"No camera calibration found for {camera.height}p resolution. "
+                               f"Available resolutions: {available}. "
+                               f"Ensure front_camera_{camera.height}.yaml exists in calibration directory.")
+
             camera_info = self.camera_info[camera.height]
-            camera_info.header.stamp = ros_image.header.stamp
+            camera_info.header.stamp = current_time
 
             if self.config.conn_mode == 'single':
                 camera_info.header.frame_id = 'front_camera'