Feature/comm plate scan revisit v2

ros2_ws/src/comm/comm/local_planner_node.py

@@ -11,6 +11,7 @@
 from rclpy.time import Time
 from geometry_msgs.msg import PoseStamped
 from nav_msgs.msg import OccupancyGrid
+from std_msgs.msg import Bool
 import numpy as np
 
 
@@ -21,6 +22,8 @@
     durability=DurabilityPolicy.VOLATILE,
 )
 
+BLOCKED_LOG_PERIOD_SEC = 1.0
+
 
 class LocalPlannerNode(Node):
     """
@@ -84,9 +87,14 @@ def __init__(self):
         self.last_passthrough_reason = None
         self.last_debug_log_time = None
         self.last_plan_mode = None
+        self.enabled = False
+        self._blocked_reason = None
+        self._last_blocked_log_time = None
 
         self.occ_sub = self.create_subscription(
             OccupancyGrid, 'occupancy_node/occupancy_grid', self._occ_cb, OCCUPANCY_QOS)
+        self.enable_sub = self.create_subscription(
+            Bool, '/local_planner/enabled', self._enable_cb, 10)
         self.pose_sub = self.create_subscription(
             PoseStamped, 'mavros/local_position/pose', self._pose_cb, qos_profile_sensor_data)
         # Subscribing here triggers CommNode's routing logic:
@@ -104,17 +112,31 @@ def __init__(self):
     def _occ_cb(self, msg: OccupancyGrid):
         self.grid_msg = msg
 
+    def _enable_cb(self, msg: Bool):
+        self.enabled = msg.data
+
     def _pose_cb(self, msg: PoseStamped):
         self.current_pose = msg
 
     def _cmd_cb(self, msg: PoseStamped):
         self.desired_pose = msg
+        if not self.enabled:
+            return
         # If planner inputs are not ready yet, keep MAVROS fed with the raw target.
         if self.grid_msg is None or self.current_pose is None:
             self._publish_passthrough(msg)
 
     # ------------------------------------------------------------------ main loop
     def _plan(self):
+        if not self.enabled:
+            self._log_blocked_state(
+                'disabled',
+                'Planner idle: /local_planner/enabled = false; early-returning from _plan().',
+            )
+            return
+
+        self._clear_blocked_state('Planner resumed: /local_planner/enabled = true; _plan() active again.')
+
         if self.desired_pose is None:
             return
 
@@ -133,8 +155,8 @@ def _plan(self):
         info = self.grid_msg.info
         res = info.resolution
         W, D = info.width, info.height
-        safety_cells = max(1, int(self.safety_r / res))
-        min_gap_cells = max(1, int(self.min_gap_w / res))
+        safety_cells = max(1, int(np.ceil(self.safety_r / res - 1e-9)))
+        min_gap_cells = max(1, int(np.ceil(self.min_gap_w / res - 1e-9)))
 
         # --- 1. Intended heading -> grid column ----------------------------
         target_col, target_fwd = self._target_in_grid(W, res)
@@ -151,9 +173,10 @@ def _plan(self):
         ])
 
         # --- 4. Direct path check -----------------------------------------
-        c0 = max(0, int(target_col) - safety_cells)
-        c1 = min(W - 1, int(target_col) + safety_cells)
-        direct_clear = bool(np.all(inflated[c0: c1 + 1] >= self.lookahead))
+        center_col = int(np.clip(np.round(target_col), 0, W - 1))
+        c0 = max(0, center_col - safety_cells)
+        c1 = min(W - 1, center_col + safety_cells)
+        direct_clear = bool(np.all(inflated[c0:c1 + 1] >= self.lookahead))
 
         if direct_clear and target_fwd > 0:
             self._log_plan(
@@ -199,6 +222,31 @@ def _publish_passthrough(self, cmd: PoseStamped):
         passthrough.pose = cmd.pose
         self.setpoint_pub.publish(passthrough)
 
+    def _log_blocked_state(self, reason: str, message: str):
+        now = self.get_clock().now()
+        if self._blocked_reason != reason:
+            self._blocked_reason = reason
+            self._last_blocked_log_time = now
+            self.get_logger().info(message)
+            return
+
+        if self._last_blocked_log_time is None:
+            self._last_blocked_log_time = now
+            self.get_logger().info(message)
+            return
+
+        elapsed_s = (now - self._last_blocked_log_time).nanoseconds / 1e9
+        if elapsed_s >= BLOCKED_LOG_PERIOD_SEC:
+            self._last_blocked_log_time = now
+            self.get_logger().info(message)
+
+    def _clear_blocked_state(self, message: str):
+        if self._blocked_reason is None:
+            return
+        self._blocked_reason = None
+        self._last_blocked_log_time = None
+        self.get_logger().info(message)
+
     def _log_passthrough(self, reason: str):
         if self.last_passthrough_reason == reason:
             return

ros2_ws/src/comm/comm/occupancy_node.py

@@ -13,6 +13,7 @@
 from sensor_msgs.msg import CameraInfo, Image
 from stereo_msgs.msg import DisparityImage
 from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Bool
 import numpy as np
 from cv_bridge import CvBridge
 from scipy.ndimage import binary_erosion
@@ -25,6 +26,8 @@
     durability=DurabilityPolicy.VOLATILE,
 )
 
+BLOCKED_LOG_PERIOD_SEC = 1.0
+
 MIN_VALID_PX = 8
 PERCENTILE = 90
 
@@ -81,11 +84,16 @@ def __init__(self):
         self.last_occ_rate_hz = None
         self.current_pose = None
         self.target_pose = None
+        self.enabled = False
+        self._blocked_reason = None
+        self._last_blocked_log_time = None
 
         self.cam_info_sub = self.create_subscription(
             CameraInfo, 'disparity/camera_info', self._cam_info_cb, qos_profile_sensor_data)
         self.disparity_sub = self.create_subscription(
             DisparityImage, 'disparity', self.disparity_callback, qos_profile_sensor_data)
+        self.enable_sub = self.create_subscription(
+            Bool, '/occupancy_node/enabled', self._enable_cb, 10)
         self.pose_sub = self.create_subscription(
             PoseStamped, 'mavros/local_position/pose', self._pose_cb, qos_profile_sensor_data)
         self.target_sub = self.create_subscription(
@@ -110,8 +118,22 @@ def _pose_cb(self, msg: PoseStamped):
     def _target_cb(self, msg: PoseStamped):
         self.target_pose = msg
 
+    def _enable_cb(self, msg: Bool):
+        self.enabled = msg.data
+
     # ------------------------------------------------------------------
     def disparity_callback(self, msg: DisparityImage):
+        if not self.enabled:
+            self._log_blocked_state(
+                'disabled',
+                'Occupancy idle: /occupancy_node/enabled = false; early-returning from disparity_callback().',
+            )
+            return
+
+        self._clear_blocked_state(
+            'Occupancy resumed: /occupancy_node/enabled = true; disparity_callback() active again.'
+        )
+
         f = float(msg.f)
         B = abs(float(msg.t))
         if f == 0.0 or B == 0.0:
@@ -181,6 +203,31 @@ def disparity_callback(self, msg: DisparityImage):
 
         self._log_occupancy_summary(grid, valid_cols.size, W)
 
+    def _log_blocked_state(self, reason: str, message: str):
+        now = self.get_clock().now()
+        if self._blocked_reason != reason:
+            self._blocked_reason = reason
+            self._last_blocked_log_time = now
+            self.get_logger().info(message)
+            return
+
+        if self._last_blocked_log_time is None:
+            self._last_blocked_log_time = now
+            self.get_logger().info(message)
+            return
+
+        elapsed_s = (now - self._last_blocked_log_time).nanoseconds / 1e9
+        if elapsed_s >= BLOCKED_LOG_PERIOD_SEC:
+            self._last_blocked_log_time = now
+            self.get_logger().info(message)
+
+    def _clear_blocked_state(self, message: str):
+        if self._blocked_reason is None:
+            return
+        self._blocked_reason = None
+        self._last_blocked_log_time = None
+        self.get_logger().info(message)
+
     # ------------------------------------------------------------------
     def _log_occupancy_summary(self, grid: np.ndarray, valid_col_count: int, image_width: int):
         if not self._should_log_debug():