diff --git a/ros2_ws/src/comm/comm/comm_node.py b/ros2_ws/src/comm/comm/comm_node.py index 385a428..057c13d 100644 --- a/ros2_ws/src/comm/comm/comm_node.py +++ b/ros2_ws/src/comm/comm/comm_node.py @@ -1,23 +1,45 @@ import math +import json +from pathlib import Path import rclpy from rclpy.node import Node -from std_srvs.srv import Empty, Trigger +from std_srvs.srv import Trigger from mavros_msgs.msg import State from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import OccupancyGrid +from stereo_msgs.msg import DisparityImage from mavros_msgs.srv import CommandBool, SetMode, CommandLong from rclpy.qos import qos_profile_sensor_data -from geometry_msgs.msg import PoseArray, Pose +from geometry_msgs.msg import PoseArray +from std_msgs.msg import Bool, Int32MultiArray, String import numpy as np -STATES = ['LAUNCH, ARMING'] MODES = ['OFFBOARD', 'ALTCTL', 'STABILIZED'] -LAUNCH_ALT = 2.0 Z_OFFSET = -0.125 RADIUS = 0.2 YAW_RADIUS = 0.5 +# Scanning parameters +TRANSIT_ALT = 2.0 +SCAN_ALT = 0.8 +ASCEND_DESCEND_TOL = 0.15 +SCAN_TIMEOUT_SEC = 15.0 +FRESHNESS_TIMEOUT_SEC = 1.0 +WAIT_LOG_PERIOD_SEC = 1.0 + +# FSM States +STATE_IDLE = 'IDLE' +STATE_TRANSIT = 'TRANSIT' +STATE_DESCEND = 'DESCEND' +STATE_SCAN = 'SCAN' +STATE_ASCEND = 'ASCEND' + +WAYPOINT_TOPIC = 'rob498_drone_8/comm/waypoints' +WAYPOINT_SCAN_FLAGS_TOPIC = 'rob498_drone_8/comm/waypoint_scan_flags' + + class CommNode(Node): def __init__(self): super().__init__('rob498_drone_8') @@ -35,16 +57,34 @@ def __init__(self): self.state_sub = self.create_subscription(State, 'mavros/state', self._state_callback, 10) self.pos_sub = self.create_subscription(PoseStamped, 'mavros/local_position/pose', self._pos_callback, qos_profile_sensor_data) self.local_pos_pub = self.create_publisher(PoseStamped, 'mavros/setpoint_position/local', 10) - self.waypoint_sub = self.create_subscription(PoseArray,'rob498_drone_8/comm/waypoints',self._waypoint_callback, 10) + self.waypoint_sub = self.create_subscription(PoseArray, WAYPOINT_TOPIC, self._waypoint_callback, 10) + self.waypoint_scan_flags_sub = self.create_subscription(Int32MultiArray, WAYPOINT_SCAN_FLAGS_TOPIC, self._waypoint_scan_flags_callback, 10) # Routed to local_planner_node when it is running; planner then publishes to MAVROS. self.cmd_pose_pub = self.create_publisher(PoseStamped, 'rob498_drone_8/cmd_pose', 10) + # Plate reader gating + self.plate_confirmed_sub = self.create_subscription(String, '/plate_reader/confirmed', self._plate_confirmed_callback, 10,) + self.plate_ready_sub = self.create_subscription(Bool, '/plate_reader/ready', self._plate_ready_callback, 10) + self.plate_enable_pub = self.create_publisher(Bool, '/plate_reader/enabled', 10) + + # Compute gating for planner stack + self.planner_enable_pub = self.create_publisher(Bool, '/local_planner/enabled', 10) + self.occupancy_enable_pub = self.create_publisher(Bool, '/occupancy_node/enabled', 10) + self.depth_enable_pub = self.create_publisher(Bool, '/perception/depth_enabled', 10) + + # Freshness monitors used as safety interlock before resuming transit + self.depth_fresh_sub = self.create_subscription(DisparityImage, 'disparity', self._depth_fresh_callback, qos_profile_sensor_data) + self.occupancy_fresh_sub = self.create_subscription(OccupancyGrid, 'occupancy_node/occupancy_grid', self._occupancy_fresh_callback, qos_profile_sensor_data) + self.planner_fresh_sub = self.create_subscription(PoseStamped, 'mavros/setpoint_position/local', self._planner_fresh_callback, 10) + # Timer self.timer = self.create_timer(0.05, self._timer_callback) self.last_target_log_time = self.get_clock().now() self.last_state_log_time = self.get_clock().now() self.last_pos_log_time = self.get_clock().now() self.last_nav_log_time = self.get_clock().now() + self.last_scan_wait_log_time = self.get_clock().now() + self.last_stack_wait_log_time = self.get_clock().now() # Poses self.current_pos = PoseStamped() @@ -56,18 +96,49 @@ def __init__(self): # Waypoints self.waypoints = [] self.waypoints_received = False + self.waypoint_scan_flags = [] + self.waypoint_scan_flags_received = False self.current_wp_index = 0 self.target_radius = RADIUS self.test_active = False self.return_home_active = False self.prev_waypoint = None + # FSM State + self.state = STATE_IDLE + + # License plate scanning + self.confirmed_plate = None + + # Scan state tracking + self.scan_start_time = None + self.scan_wp_index = None + self.scan_hold_orientation = None + self.plate_reader_ready = False + self.plate_reader_enabled = False + self.nav_stack_enabled = False + self.waiting_for_stack_ready = False + self.last_depth_msg_time = None + self.last_occupancy_msg_time = None + self.last_planner_msg_time = None + + # Live mission logging (JSON) + self.mission_log_path = None + self.mission_log_entries = [] + self.logged_plate_events = set() + self.mission_log_fail_count = 0 + self._MISSION_LOG_MAX_FAILS = 3 + self.get_logger().info('Waiting for MAVROS services...') self.arming_client.wait_for_service() self.set_mode_client.wait_for_service() self.command_client.wait_for_service() self.get_logger().info('Initialization complete!') + # Keep heavy stack idle until mission test starts. + self._set_plate_reader_enabled(False) + self._set_nav_stack_enabled(False) + def callback_launch(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: """Takeoff in place to LAUNCH_ALT""" @@ -78,7 +149,7 @@ def callback_launch(self, request: Trigger.Request, response: Trigger.Response) self.target_pose.pose.position.x = self.current_pos.pose.position.x self.target_pose.pose.position.y = self.current_pos.pose.position.y - self.target_pose.pose.position.z = self.current_pos.pose.position.z + LAUNCH_ALT + self.target_pose.pose.position.z = self.current_pos.pose.position.z + TRANSIT_ALT self.target_pose.pose.orientation = self.current_pos.pose.orientation self.get_logger().info(f"[LAUNCH]: Takeoff to target | X: {self.target_pose.pose.position.x:.3f} | Y: {self.target_pose.pose.position.y:.3f} | Z: {self.target_pose.pose.position.z:.3f}") @@ -96,14 +167,42 @@ def callback_launch(self, request: Trigger.Request, response: Trigger.Response) def callback_test(self, request, response): if not self.waypoints_received: - self.get_logger().warn("[TEST]: Cannot start test — no waypoints received") + self.get_logger().warn("[TEST]: Cannot start test - no waypoints received") response.success = False response.message = "No waypoints received" return response + if not self.waypoint_scan_flags_received: + self.get_logger().warn("[TEST]: Cannot start test - no waypoint scan flags received") + response.success = False + response.message = "No waypoint scan flags received" + return response + + if len(self.waypoints) != len(self.waypoint_scan_flags): + self.get_logger().warn( + "[TEST]: Cannot start test - waypoint count does not match scan-flag count" + ) + response.success = False + response.message = ( + f"Waypoint count ({len(self.waypoints)}) does not match " + f"scan-flag count ({len(self.waypoint_scan_flags)})" + ) + return response + self.prev_waypoint = np.array([self.current_pos.pose.position.x, self.current_pos.pose.position.y, self.current_pos.pose.position.z]) self.current_wp_index = 0 self.test_active = True + self.state = STATE_TRANSIT + self.confirmed_plate = None + self.scan_start_time = None + self.scan_wp_index = None + self.scan_hold_orientation = None + self.plate_reader_ready = False + self.waiting_for_stack_ready = False + self._set_plate_reader_enabled(False) + self._set_nav_stack_enabled(True) + self.logged_plate_events.clear() + self._start_mission_log() self.get_logger().info("[TEST]: Starting waypoint navigation") @@ -112,8 +211,6 @@ def callback_test(self, request, response): return response def callback_land(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: - """RTL and land""" - # Set target to home pose exactly """Return toward home XY/heading, then auto-land.""" if self.home_set is False: @@ -126,29 +223,21 @@ def callback_land(self, request: Trigger.Request, response: Trigger.Response) -> response.message = "Return home not active" return response - self.get_logger().info("[LAND]: Entered callback_land") # Step 1: Lock target XY and yaw to home, but keep current Z self.target_pose.pose.position.x = self.home_pose.pose.position.x self.target_pose.pose.position.y = self.home_pose.pose.position.y self.target_pose.pose.position.z = self.current_pos.pose.position.z - yaw = math.atan2(self.home_pose.pose.position.y - self.current_pos.pose.position.y, self.home_pose.pose.position.x - self.current_pos.pose.position.x) - self.target_pose.pose.orientation.x = 0.0 - self.target_pose.pose.orientation.y = 0.0 - self.target_pose.pose.orientation.z = math.sin(yaw / 2.0) - self.target_pose.pose.orientation.w = math.cos(yaw / 2.0) - dx = self.home_pose.pose.position.x - self.current_pos.pose.position.x dy = self.home_pose.pose.position.y - self.current_pos.pose.position.y - dist = np.sqrt(dx**2 + dy**2) + xy_dist = np.sqrt(dx**2 + dy**2) - if dist < self.target_radius: + if xy_dist < self.target_radius: self.get_logger().info("[MISSION]: Home reached. Initiating landing.") self.target_pose.pose.orientation = self.home_pose.pose.orientation - req = SetMode.Request() req.custom_mode = 'AUTO.LAND' self.get_logger().info("Requesting AUTO.LAND...") @@ -157,12 +246,117 @@ def callback_land(self, request: Trigger.Request, response: Trigger.Response) -> response.message = "Sent AUTO.LAND command to MAVROS." self.return_home_active = False + self.state = STATE_IDLE + self._set_plate_reader_enabled(False) + self._set_nav_stack_enabled(False) + self._append_mission_log({ + 'event': 'home_reached', + 'timestamp_s': round(self.get_clock().now().nanoseconds / 1e9, 3), + 'state': self.state, + }) return response + response.success = True + response.message = 'Returning to home position.' + return response + + def _start_mission_log(self): + log_dir = Path(__file__).resolve().parent + self.mission_log_path = log_dir / 'confirmed_plates.json' + self.mission_log_entries = [] + self._append_mission_log({ + 'event': 'mission_start', + 'timestamp_s': round(self.get_clock().now().nanoseconds / 1e9, 3), + 'state': self.state, + }) + self.get_logger().info(f'[LOG]: Writing mission plate log to {self.mission_log_path}') + + def _append_mission_log(self, entry): + if self.mission_log_path is None: + return + self.mission_log_entries.append(entry) + payload = { + 'entries': self.mission_log_entries, + } + try: + with self.mission_log_path.open('w', encoding='utf-8') as handle: + json.dump(payload, handle, indent=2) + self.mission_log_fail_count = 0 + except OSError as exc: + self.mission_log_fail_count += 1 + self.get_logger().error( + f'[LOG]: Failed to write mission log ({self.mission_log_fail_count}/' + f'{self._MISSION_LOG_MAX_FAILS}): {exc}' + ) + if self.mission_log_fail_count >= self._MISSION_LOG_MAX_FAILS: + self.get_logger().error('[LOG]: Disabling mission log after repeated failures.') + self.mission_log_path = None + + def _set_plate_reader_enabled(self, enabled: bool): + if self.plate_reader_enabled == enabled: + return + self.plate_reader_enabled = enabled + msg = Bool() + msg.data = enabled + self.plate_enable_pub.publish(msg) + + def _set_nav_stack_enabled(self, enabled: bool, reset_freshness: bool = False): + if self.nav_stack_enabled != enabled: + msg = Bool() + msg.data = enabled + self.planner_enable_pub.publish(msg) + self.occupancy_enable_pub.publish(msg) + self.depth_enable_pub.publish(msg) + self.nav_stack_enabled = enabled + + if enabled and reset_freshness: + self.last_depth_msg_time = None + self.last_occupancy_msg_time = None + self.last_planner_msg_time = None + + def _publish_control_heartbeat(self): + """Republish current control states so late subscribers converge quickly.""" + plate_msg = Bool() + plate_msg.data = self.plate_reader_enabled + self.plate_enable_pub.publish(plate_msg) + + nav_msg = Bool() + nav_msg.data = self.nav_stack_enabled + self.planner_enable_pub.publish(nav_msg) + self.occupancy_enable_pub.publish(nav_msg) + self.depth_enable_pub.publish(nav_msg) + + def _plate_ready_callback(self, msg: Bool): + self.plate_reader_ready = msg.data + + def _depth_fresh_callback(self, msg: DisparityImage): + self.last_depth_msg_time = self.get_clock().now() + + def _occupancy_fresh_callback(self, msg: OccupancyGrid): + self.last_occupancy_msg_time = self.get_clock().now() + + def _planner_fresh_callback(self, msg: PoseStamped): + if self.nav_stack_enabled: + self.last_planner_msg_time = self.get_clock().now() + + def _is_fresh(self, timestamp, now): + if timestamp is None: + return False + return (now - timestamp).nanoseconds / 1e9 <= FRESHNESS_TIMEOUT_SEC + + def _planner_stack_ready(self, now): + return ( + self._is_fresh(self.last_depth_msg_time, now) + and self._is_fresh(self.last_occupancy_msg_time, now) + and self._is_fresh(self.last_planner_msg_time, now) + ) + def callback_abort(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: """Land using PX4 land mode""" + self._set_plate_reader_enabled(False) + self._set_nav_stack_enabled(False) req = SetMode.Request() req.custom_mode = 'AUTO.LAND' self.get_logger().info("Requesting AUTO.LAND...") @@ -171,60 +365,38 @@ def callback_abort(self, request: Trigger.Request, response: Trigger.Response) - response.message = "Sent AUTO.LAND command to MAVROS." return response - def update_waypoint_navigation(self): - if not self.test_active: - return - - if self.current_wp_index >= len(self.waypoints): - self.test_active = False - self.return_home_active = True - self.get_logger().info("[MISSION]: All waypoints reached. Returning home.") - return + def update_waypoint_navigation(self, wp_index, target_altitude, hold_orientation=None): + """Navigate to a waypoint at a requested altitude and return nav metrics.""" + wp = self.waypoints[wp_index] + yaw = 0.0 - wp = self.waypoints[self.current_wp_index] + # Set target position + self.target_pose.pose.position.x = wp[0] + self.target_pose.pose.position.y = wp[1] + self.target_pose.pose.position.z = target_altitude - # Distance to waypoint + # Calculate distances dx = wp[0] - self.current_pos.pose.position.x dy = wp[1] - self.current_pos.pose.position.y - dz = wp[2] - self.current_pos.pose.position.z - - # Set target position and yaw to face direction of travel. - self.target_pose.pose.position.x = wp[0] - self.target_pose.pose.position.y = wp[1] - self.target_pose.pose.position.z = wp[2] + dz = target_altitude - self.current_pos.pose.position.z xy_dist = np.sqrt(dx**2 + dy**2) dist = np.sqrt(dx**2 + dy**2 + dz**2) - - #wp = np.array([pose.position.x, pose.position.y, pose.position.z]) - yaw = math.atan2(wp[1] - self.prev_waypoint[1], wp[0] - self.prev_waypoint[0]) - - if xy_dist > YAW_RADIUS: - self.target_pose.pose.orientation.x = 0.0 - self.target_pose.pose.orientation.y = 0.0 - self.target_pose.pose.orientation.z = math.sin(yaw / 2.0) - self.target_pose.pose.orientation.w = math.cos(yaw / 2.0) - - now = self.get_clock().now() - - if (now - self.last_nav_log_time).nanoseconds > 1e9: - self.get_logger().info(f"[NAV]: WP {self.current_wp_index+1} | "f"Dist {dist:.3f} m") - self.last_nav_log_time = now - self.get_logger().info(f"[ORIENTATION]: Yaw to target: {math.degrees(yaw):.1f} degrees") - - if dist < self.target_radius: - - self.get_logger().info(f"[WAYPOINT]: {self.current_wp_index+1} reached "f"(distance {dist:.3f} m)") - - self.current_wp_index += 1 - self.prev_waypoint = wp - - if self.current_wp_index < len(self.waypoints): + # Set orientation + if hold_orientation is not None: + self.target_pose.pose.orientation = hold_orientation + else: + # Calculate yaw toward waypoint + yaw = math.atan2(wp[1] - self.prev_waypoint[1], wp[0] - self.prev_waypoint[0]) - next_wp = self.waypoints[self.current_wp_index] + if xy_dist > YAW_RADIUS: + self.target_pose.pose.orientation.x = 0.0 + self.target_pose.pose.orientation.y = 0.0 + self.target_pose.pose.orientation.z = math.sin(yaw / 2.0) + self.target_pose.pose.orientation.w = math.cos(yaw / 2.0) - self.get_logger().info(f"[NAV]: Moving to waypoint {self.current_wp_index+1} | "f"X {next_wp[0]:.3f} | Y {next_wp[1]:.3f} | Z {next_wp[2]:.3f}") + return dist, xy_dist, dz, yaw def arming(self, val: bool): arm_req = CommandBool.Request() @@ -323,7 +495,15 @@ def _timer_callback(self): """Heartbeat for the drone""" # WAYPOINT NAVIGATION if self.test_active: - self.update_waypoint_navigation() + # FSM state dispatch + if self.state == STATE_TRANSIT: + self._update_transit() + elif self.state == STATE_DESCEND: + self._update_descend() + elif self.state == STATE_SCAN: + self._update_scan() + elif self.state == STATE_ASCEND: + self._update_ascend() # RETURN HOME elif self.return_home_active: @@ -334,9 +514,12 @@ def _timer_callback(self): self.callback_land(req, res) + # Keep enable-state topics fresh for nodes that (re)subscribe mid-mission. + self._publish_control_heartbeat() + self.target_pose.header.stamp = self.get_clock().now().to_msg() - # Route through local_planner_node when it is active; otherwise go direct. - if self.cmd_pose_pub.get_subscription_count() > 0 and self.test_active == True: + # Route through local_planner_node when planner enabled; otherwise go direct to MAVROS. + if self.nav_stack_enabled and self.cmd_pose_pub.get_subscription_count() > 0: self.cmd_pose_pub.publish(self.target_pose) else: self.local_pos_pub.publish(self.target_pose) @@ -363,6 +546,173 @@ def _waypoint_callback(self, msg): self.get_logger().info(f"[WAYPOINTS]: Stored {len(self.waypoints)} waypoints successfully") + def _waypoint_scan_flags_callback(self, msg: Int32MultiArray): + if self.waypoint_scan_flags_received: + return + + self.waypoint_scan_flags = [bool(flag) for flag in msg.data] + self.waypoint_scan_flags_received = True + + self.get_logger().info( + f"[WAYPOINT FLAGS]: Stored {len(self.waypoint_scan_flags)} scan flags successfully" + ) + for i, scan_plate in enumerate(self.waypoint_scan_flags): + waypoint_type = 'scan' if scan_plate else 'nav-only' + self.get_logger().info(f"[WAYPOINT FLAG {i+1}]: {waypoint_type}") + + def _plate_confirmed_callback(self, msg: String): + """Receive confirmed license plate from plate_reader_node""" + plate = msg.data.strip() + if plate and self.state == STATE_SCAN: + self.confirmed_plate = plate + self.get_logger().info(f"[PLATE CONFIRMED]: {self.confirmed_plate}") + wp_index = self.scan_wp_index + 1 if self.scan_wp_index is not None else self.current_wp_index + 1 + event_key = (wp_index, self.confirmed_plate) + if event_key not in self.logged_plate_events: + self.logged_plate_events.add(event_key) + self._append_mission_log({ + 'event': 'plate_confirmed', + 'timestamp_s': round(self.get_clock().now().nanoseconds / 1e9, 3), + 'waypoint_index': wp_index, + 'plate': self.confirmed_plate, + 'state': self.state, + }) + + def _update_transit(self): + """Navigate waypoints at TRANSIT_ALT with planner enabled""" + self._set_plate_reader_enabled(False) + self._set_nav_stack_enabled(True) + + # Waypoint have already been checked that they exist in Test service callback. + if self.current_wp_index >= len(self.waypoints): + # All waypoints done, return home + self.test_active = False + self.return_home_active = True + self._set_plate_reader_enabled(False) + self._append_mission_log({ + 'event': 'all_waypoints_complete', + 'timestamp_s': round(self.get_clock().now().nanoseconds / 1e9, 3), + 'state': self.state, + }) + self.get_logger().info("[MISSION]: All waypoints reached, returning to home") + return + + # Navigate to waypoint at transit altitude + dist, xy_dist, dz, yaw = self.update_waypoint_navigation(self.current_wp_index, TRANSIT_ALT) + + now = self.get_clock().now() + if (now - self.last_nav_log_time).nanoseconds > 1e9: + self.get_logger().info(f"[NAV]: WP {self.current_wp_index+1} | Dist {dist:.3f} m | Yaw {math.degrees(yaw):.1f}°") + self.last_nav_log_time = now + + # Waypoint reached, descend for scan + if dist < self.target_radius: + self.get_logger().info(f"[TRANSIT]: Waypoint {self.current_wp_index+1} reached") + if not self.waypoint_scan_flags[self.current_wp_index]: + reached_wp_index = self.current_wp_index + wp = self.waypoints[reached_wp_index] + self.prev_waypoint = np.array([wp[0], wp[1], TRANSIT_ALT]) + self.current_wp_index += 1 + self.scan_wp_index = None + self.scan_hold_orientation = None + self.scan_start_time = None + self.plate_reader_ready = False + self._append_mission_log({ + 'event': 'waypoint_nav_only', + 'timestamp_s': round(self.get_clock().now().nanoseconds / 1e9, 3), + 'waypoint_index': reached_wp_index + 1, + 'state': self.state, + }) + if self.current_wp_index < len(self.waypoints): + self.get_logger().info( + f"[TRANSIT]: Waypoint {reached_wp_index+1} is nav-only, skipping scan and continuing to waypoint {self.current_wp_index+1}" + ) + else: + self.get_logger().info( + f"[TRANSIT]: Waypoint {reached_wp_index+1} is nav-only, skipping scan and preparing to return home" + ) + return + + self.scan_wp_index = self.current_wp_index + self.scan_hold_orientation = self.target_pose.pose.orientation + self.scan_start_time = None + self.plate_reader_ready = False + self._set_nav_stack_enabled(False) + self._set_plate_reader_enabled(True) + self.state = STATE_DESCEND + + def _update_descend(self): + """Descend to SCAN_ALT before enabling camera""" + self._set_nav_stack_enabled(False) + self._set_plate_reader_enabled(True) + + # Navigate to scan altitude, holding orientation from transit phase + dist, xy_dist, dz, yaw = self.update_waypoint_navigation(self.scan_wp_index, SCAN_ALT, self.scan_hold_orientation) + + if abs(dz) < ASCEND_DESCEND_TOL: # Reached scan altitude + if self.plate_reader_ready: + self.state = STATE_SCAN + self.scan_start_time = self.get_clock().now() + self.confirmed_plate = None + self.get_logger().info(f"[SCAN]: At scan altitude {SCAN_ALT} m, camera enabled") + else: + now = self.get_clock().now() + if (now - self.last_scan_wait_log_time).nanoseconds / 1e9 > WAIT_LOG_PERIOD_SEC: + self.get_logger().warn('[SCAN]: Waiting for plate_reader ready before starting timeout') + self.last_scan_wait_log_time = now + + def _update_scan(self): + """Hold scanning phase at current waypoint""" + self._set_nav_stack_enabled(False) + self._set_plate_reader_enabled(True) + + # Navigate to hold position at scan altitude + dist, xy_dist, dz, yaw = self.update_waypoint_navigation(self.scan_wp_index, SCAN_ALT, self.scan_hold_orientation) + + if self.scan_start_time is None: + self.scan_start_time = self.get_clock().now() + + now = self.get_clock().now() + scan_elapsed = (now - self.scan_start_time).nanoseconds / 1e9 + + # Exit SCAN on either successful read or timeout, then run one shared ascent transition. + timed_out = scan_elapsed > SCAN_TIMEOUT_SEC + if self.confirmed_plate or timed_out: + if timed_out: + self.get_logger().warn(f"[SCAN]: Timeout at waypoint {self.current_wp_index+1}, ascending") + + self._set_plate_reader_enabled(False) + self.state = STATE_ASCEND + return + + def _update_ascend(self): + """Ascend back to TRANSIT_ALT and enable planner""" + self._set_plate_reader_enabled(False) + + # Navigate to transit altitude, holding orientation from transit phase + dist, xy_dist, dz, yaw = self.update_waypoint_navigation(self.scan_wp_index, TRANSIT_ALT, self.scan_hold_orientation) + + if abs(dz) < ASCEND_DESCEND_TOL: # Reached transit altitude + if not self.waiting_for_stack_ready: + self._set_nav_stack_enabled(True, reset_freshness=True) + self.waiting_for_stack_ready = True + + now = self.get_clock().now() + if not self._planner_stack_ready(now): + if (now - self.last_stack_wait_log_time).nanoseconds / 1e9 > WAIT_LOG_PERIOD_SEC: + self.get_logger().warn('[ASCEND]: Holding for fresh depth/occupancy/planner publishes before transit') + self.last_stack_wait_log_time = now + return + + self.waiting_for_stack_ready = False + self.current_wp_index += 1 + wp = self.waypoints[self.scan_wp_index] + self.prev_waypoint = np.array([wp[0], wp[1], TRANSIT_ALT]) + self.state = STATE_TRANSIT + if self.confirmed_plate: + self.get_logger().info(f"[WAYPOINT {self.scan_wp_index+1}]: Plate scanned: {self.confirmed_plate}") + self.get_logger().info(f"[ASCEND]: Reached transit altitude, moving to waypoint {self.current_wp_index+1}") + def main(args=None): rclpy.init(args=args) @@ -372,4 +722,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/ros2_ws/src/comm/comm/local_planner_node.py b/ros2_ws/src/comm/comm/local_planner_node.py index 716b38b..8626176 100644 --- a/ros2_ws/src/comm/comm/local_planner_node.py +++ b/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 diff --git a/ros2_ws/src/comm/comm/occupancy_node.py b/ros2_ws/src/comm/comm/occupancy_node.py index 52ed70d..d269bc6 100644 --- a/ros2_ws/src/comm/comm/occupancy_node.py +++ b/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(): diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/plate_reader_node.py b/ros2_ws/src/comm/comm/plate_reader_node.py similarity index 69% rename from ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/plate_reader_node.py rename to ros2_ws/src/comm/comm/plate_reader_node.py index 29a295f..63c7020 100644 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/plate_reader_node.py +++ b/ros2_ws/src/comm/comm/plate_reader_node.py @@ -1,13 +1,3 @@ -#!/usr/bin/env python3 -"""ROS wrapper for the TensorRT license-plate pipeline. - -This node stays idle until the mission node enables scanning. During scans it -runs the detector/recognizer, applies multi-frame voting, and publishes the -current candidate plus the confirmed 7-character plate string. -""" - -from __future__ import annotations - from collections import Counter, deque import cv2 @@ -17,60 +7,50 @@ from rclpy.node import Node from sensor_msgs.msg import Image from std_msgs.msg import Bool, String - -from parking_enforcement.config import ( - BLANK, - CHARS, - DETECT_EVERY, - DRONE_NS, - LPD_ENGINE_PATH, - LPD_H, - LPD_W, - LPR_ENGINE_PATH, - LPR_H, - LPR_W, - MIN_DET_CONF, - MIN_READ_CONF, - MIN_VOTES, - PAD_BOTTOM, - PAD_LEFT, - PAD_RIGHT, - PAD_TOP, - PLATE_CONFIRMED_TOPIC, - PLATE_CURRENT_TOPIC, - PLATE_DEBUG_IMAGE_TOPIC, - PLATE_ENABLED_TOPIC, - RGB_IMAGE_TOPIC, - VOTE_WINDOW, -) - -try: - import tensorrt as trt - import pycuda.autoinit # noqa: F401 # Creates the CUDA context once at import time. - import pycuda.driver as cuda -except Exception as exc: # pragma: no cover - depends on Jetson runtime. - trt = None - cuda = None - TRT_IMPORT_ERROR = exc -else: - TRT_IMPORT_ERROR = None - -TRT_LOGGER = trt.Logger(trt.Logger.WARNING) if trt is not None else None +import tensorrt as trt +import pycuda.autoinit # noqa: F401 +import pycuda.driver as cuda + +# Engine paths +LPD_ENGINE_PATH = '/home/jetson/engines/lpdnet_fp32.engine' +LPR_ENGINE_PATH = '/home/jetson/engines/lprnet_fp32.engine' + +# Model / decode defaults +LPD_W, LPD_H = 640, 480 +LPR_W, LPR_H = 96, 48 +CHARS = '0123456789ABCDEFGHIJKLMNPQRSTUVWXYZ' +BLANK = 35 +DETECT_EVERY = 15 +PAD_LEFT = 15 +PAD_RIGHT = 0 +PAD_TOP = 10 +PAD_BOTTOM = 10 + +# Confirmation behavior +REQUIRED_PLATE_LEN = 7 +MIN_VOTES = 10 +VOTE_WINDOW = 40 +MIN_DET_CONF = 0.60 +MIN_READ_CONF = 0.45 + +# Topics +DRONE_NS = 'rob498_drone_8' +RGB_IMAGE_TOPIC = '/camera/image_raw' +PLATE_ENABLED_TOPIC = '/plate_reader/enabled' +PLATE_READY_TOPIC = '/plate_reader/ready' +PLATE_CONFIRMED_TOPIC = '/plate_reader/confirmed' +PLATE_CURRENT_TOPIC = '/plate_reader/current' +# PLATE_DEBUG_IMAGE_TOPIC = '/plate_reader/debug_image' # disabled by default to save compute + +TRT_LOGGER = trt.Logger(trt.Logger.WARNING) class TRTModel: - """Minimal TensorRT runner for the static-shape engines used in the demo.""" - def __init__(self, engine_path: str): with open(engine_path, 'rb') as handle: runtime = trt.Runtime(TRT_LOGGER) self.engine = runtime.deserialize_cuda_engine(handle.read()) - if self.engine is None: - raise RuntimeError(f'Could not deserialize TensorRT engine: {engine_path}') - self.context = self.engine.create_execution_context() - if self.context is None: - raise RuntimeError(f'Could not create TensorRT execution context: {engine_path}') self.inputs = [] self.outputs = [] @@ -90,8 +70,6 @@ def __init__(self, engine_path: str): self.outputs.append(payload) def infer(self, *input_arrays): - """Copy inputs to the device, execute once, and return reshaped outputs.""" - for index, arr in enumerate(input_arrays): np.copyto(self.inputs[index]['host'], arr.ravel()) cuda.memcpy_htod(self.inputs[index]['device'], self.inputs[index]['host']) @@ -106,21 +84,15 @@ def infer(self, *input_arrays): class PlateVoter: - """Confirm a plate only after it has been seen repeatedly in a short window.""" - def __init__(self, min_votes: int = MIN_VOTES, window: int = VOTE_WINDOW): self.min_votes = min_votes self.window = deque(maxlen=window) def clear(self): - """Reset the vote history when a new scan starts.""" - self.window.clear() def update(self, plate_text: str, det_conf: float, read_conf: float): - """Return a confirmed plate once the same reading has enough votes.""" - - if len(plate_text) != 7: + if len(plate_text) != REQUIRED_PLATE_LEN: return None if det_conf < MIN_DET_CONF or read_conf < MIN_READ_CONF: return None @@ -134,8 +106,6 @@ def update(self, plate_text: str, det_conf: float, read_conf: float): class PlateReaderNode(Node): - """Run the plate detector/recognizer only when the mission enters a scan state.""" - def __init__(self): super().__init__(f'{DRONE_NS}_plate_reader') self.bridge = CvBridge() @@ -144,32 +114,25 @@ def __init__(self): self.last_detections = [] self.last_confirmed_plate = '' self.voter = PlateVoter() + self.ready_latched = False + self.ready_pub = self.create_publisher(Bool, PLATE_READY_TOPIC, 10) self.confirmed_pub = self.create_publisher(String, PLATE_CONFIRMED_TOPIC, 10) self.current_pub = self.create_publisher(String, PLATE_CURRENT_TOPIC, 10) - self.debug_pub = self.create_publisher(Image, PLATE_DEBUG_IMAGE_TOPIC, 10) self.create_subscription(Image, RGB_IMAGE_TOPIC, self._image_cb, 10) self.create_subscription(Bool, PLATE_ENABLED_TOPIC, self._enable_cb, 10) self.models_ready = False - if trt is None or cuda is None: - self.get_logger().error(f'TensorRT/PyCUDA import failed: {TRT_IMPORT_ERROR}') - return - - try: - self.get_logger().info(f'Loading LPD engine from {LPD_ENGINE_PATH}') - self.lpd_model = TRTModel(LPD_ENGINE_PATH) - self.get_logger().info(f'Loading LPR engine from {LPR_ENGINE_PATH}') - self.lpr_model = TRTModel(LPR_ENGINE_PATH) - self.models_ready = True - self.get_logger().info('Plate reader ready') - except Exception as exc: - self.get_logger().error(f'Failed to initialize plate reader: {exc}') + self.get_logger().info(f'Loading LPD engine from {LPD_ENGINE_PATH}') + self.lpd_model = TRTModel(LPD_ENGINE_PATH) + self.get_logger().info(f'Loading LPR engine from {LPR_ENGINE_PATH}') + self.lpr_model = TRTModel(LPR_ENGINE_PATH) + self.models_ready = True + self._publish_ready(False) + self.get_logger().info(f'[READY] Plate reader online. Listening on {RGB_IMAGE_TOPIC}, gated by {PLATE_ENABLED_TOPIC}') def _enable_cb(self, msg: Bool): - """Turn the reader on only during mission scan phases.""" - if msg.data == self.enabled: return @@ -177,25 +140,30 @@ def _enable_cb(self, msg: Bool): self.frame_id = 0 self.last_detections = [] self.last_confirmed_plate = '' + self.ready_latched = False self.voter.clear() + self._publish_ready(False) state = 'enabled' if self.enabled else 'disabled' self.get_logger().info(f'Plate reader {state}') - def _image_cb(self, msg: Image): - """Process a color frame when the mission node has enabled plate scanning.""" + def _publish_ready(self, ready: bool): + ready_msg = Bool() + ready_msg.data = ready + self.ready_pub.publish(ready_msg) + def _image_cb(self, msg: Image): if not self.enabled or not self.models_ready: return frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') self.frame_id += 1 - # Run the detector every few frames, then re-read the strongest recent box. if self.frame_id % DETECT_EVERY == 0: lpd_tensor, scale_x, scale_y = preprocess_lpd(frame) lpd_out = self.lpd_model.infer(lpd_tensor) self.last_detections = decode_lpd(lpd_out[0], lpd_out[1], scale_x, scale_y) + # DEBUG-ONLY: keep overlay drawing for visual diagnostics during testing. overlay = frame.copy() current_plate = '' @@ -219,6 +187,9 @@ def _image_cb(self, msg: Image): char_confs = lpr_out[0][0].astype(np.float32) read_conf = float(char_confs.mean()) current_plate = plate_text + + if len(plate_text) == REQUIRED_PLATE_LEN: + self.get_logger().debug(f'[PLATE] Detected: {plate_text} (det_conf={det_conf:.2f}, read_conf={read_conf:.2f})') cv2.rectangle(overlay, (x1, y1), (x2, y2), (0, 255, 0), 2) cv2.putText( @@ -238,21 +209,18 @@ def _image_cb(self, msg: Image): confirmed_msg.data = confirmed self.confirmed_pub.publish(confirmed_msg) self.last_confirmed_plate = confirmed - self.get_logger().info(f'Confirmed plate: {confirmed}') + self.get_logger().info(f'[SCAN_COMPLETE] Confirmed plate: {confirmed} (votes={MIN_VOTES}/{VOTE_WINDOW})') current_msg = String() current_msg.data = current_plate self.current_pub.publish(current_msg) - if self.debug_pub.get_subscription_count() > 0: - debug_msg = self.bridge.cv2_to_imgmsg(overlay, encoding='bgr8') - debug_msg.header = msg.header - self.debug_pub.publish(debug_msg) + if not self.ready_latched: + self._publish_ready(True) + self.ready_latched = True def ctc_decode(pred): - """Collapse repeated class IDs and remove the CTC blank token.""" - plate = [] prev = BLANK for token in pred: @@ -263,8 +231,6 @@ def ctc_decode(pred): def preprocess_lpd(frame): - """Resize and normalize an RGB image for the plate detector engine.""" - orig_h, orig_w = frame.shape[:2] scale_x = orig_w / LPD_W scale_y = orig_h / LPD_H @@ -277,8 +243,6 @@ def preprocess_lpd(frame): def decode_lpd(cov, bbox, scale_x, scale_y, conf_thresh=0.50): - """Decode the detector heatmap into pixel-space boxes, then apply NMS.""" - cov = cov.squeeze() bbox = bbox.squeeze() if bbox.shape[-1] == 4: @@ -322,8 +286,6 @@ def decode_lpd(cov, bbox, scale_x, scale_y, conf_thresh=0.50): def preprocess_lpr(crop_bgr): - """Resize and normalize a plate crop for the LPR engine.""" - img = cv2.cvtColor(crop_bgr, cv2.COLOR_BGR2RGB) img = cv2.resize(img, (LPR_W, LPR_H)) x = img.astype(np.float32) / 255.0 @@ -333,8 +295,6 @@ def preprocess_lpr(crop_bgr): def main(args=None): - """Spin the plate reader node until shutdown.""" - rclpy.init(args=args) node = PlateReaderNode() try: diff --git a/ros2_ws/src/comm/package.xml b/ros2_ws/src/comm/package.xml index 034f24c..b53ae95 100644 --- a/ros2_ws/src/comm/package.xml +++ b/ros2_ws/src/comm/package.xml @@ -7,6 +7,15 @@ jetson TODO: License declaration + cv_bridge + geometry_msgs + mavros_msgs + nav_msgs + rclpy + sensor_msgs + std_msgs + std_srvs + ament_copyright ament_flake8 ament_pep257 diff --git a/ros2_ws/src/comm/setup.py b/ros2_ws/src/comm/setup.py index 7dd8bfd..29a7595 100644 --- a/ros2_ws/src/comm/setup.py +++ b/ros2_ws/src/comm/setup.py @@ -22,7 +22,8 @@ 'console_scripts': [ 'comm_node = comm.comm_node:main', 'occupancy_node = comm.occupancy_node:main', - 'local_planner_node = comm.local_planner_node:main' + 'local_planner_node = comm.local_planner_node:main', + 'plate_reader_node = comm.plate_reader_node:main' ], }, ) diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/README.md b/ros2_ws/src/parking_enforcement_package/parking_enforcement/README.md deleted file mode 100644 index 8c7b570..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/README.md +++ /dev/null @@ -1,202 +0,0 @@ -# `parking_enforcement` ROS 2 package - -This package contains the parking-enforcement mission stack for the ROB498 capstone demo. -It stays separate from the older `flight3` package, but it preserves the same service interface: - -- `/rob498_drone_8/comm/launch` -- `/rob498_drone_8/comm/test` -- `/rob498_drone_8/comm/land` -- `/rob498_drone_8/comm/abort` - -The package is split into four main pieces: - -- `stereo_depth_node.py` - - subscribes to the RealSense fisheye stereo pair - - rectifies stereo images and crops the invalid left strip - - publishes `/stereo/depth`, `/stereo/depth/camera_info`, and `/stereo/disp_vis` -- `parking_mission_node.py` - - first-pass waypoint mission - - hardcoded parking waypoints from `config.py` - - obstacle sidestep / back-up behavior from `/avoid_dir` - - yaw-right + vertical scan at each spot - - writes first-pass results to `~/.ros/parking_enforcement/plate_log.json` -- `parking_revisit_node.py` - - same mission flow for the revisit pass - - loads `plate_log.json` - - rescans each parking spot - - writes revisit results to `~/.ros/parking_enforcement/revisit_log.json` -- `local_costmap_node.py` - - consumes `/stereo/depth` and `/stereo/depth/camera_info` - - publishes `/local_grid` - - makes decisions only from forward `left`, `center`, and `right` sectors - - publishes `/avoid_dir` as `CLEAR`, `LEFT`, `RIGHT`, or `BLOCKED` -- `plate_reader_node.py` - - wraps the TensorRT license-plate detector and recognizer - - stays disabled outside scan states - - confirms a plate only after 10 matching 7-character reads - -## Depth pipeline - -This package runs its own stereo depth node inside `parking_enforcement`. -That node publishes: - -- `/stereo/depth` -- `/stereo/depth/camera_info` -- `/stereo/disp_vis` - -## Where to edit things first - -Edit [config.py](/home/jetson/rob498_2026/capstone2026/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/config.py) before building. - -Important fields there: - -- `DRONE_NS` -- `PARKING_WAYPOINTS` -- `RGB_IMAGE_TOPIC` -- `LPD_ENGINE_PATH` -- `LPR_ENGINE_PATH` -- `REVISIT_OVERSTAY_THRESHOLD_SEC` - -### Hardcoded waypoints - -The demo now uses hardcoded waypoints instead of a runtime `PoseArray` topic. -Edit them here: - -```python -PARKING_WAYPOINTS = [ - (-1.0, -1.0, 1.0), - (1.0, -1.0, 2.0), - (1.0, 1.0, 1.0), - (-1.0, 1.0, 0.5), -] -``` - -## Build - -```bash -cd /home/jetson/rob498_2026/capstone2026/ros2_ws -source /opt/ros/foxy/setup.bash -colcon build --packages-select perception parking_enforcement -source install/setup.bash -``` - -## Run order - -### Terminal 1: MAVROS - -Launch MAVROS the same way you already do for flight. - -Example: - -```bash -ros2 launch px4_autonomy_modules mavros.launch.py -``` - -### Terminal 2: External pose / vision bridge - -Launch your external pose bridge separately if your flight stack needs it. - -### Terminal 3: Camera publishers - -Run the stereo cameras and your front RGB camera publisher. - -### Terminal 4: First pass - -```bash -cd /home/jetson/rob498_2026/capstone2026/ros2_ws -source /opt/ros/foxy/setup.bash -source install/setup.bash -ros2 launch parking_enforcement parking_first_pass.launch.py -``` - -### Start the mission - -```bash -ros2 service call /rob498_drone_8/comm/launch std_srvs/srv/Trigger '{}' -ros2 service call /rob498_drone_8/comm/test std_srvs/srv/Trigger '{}' -``` - -### Revisit pass - -After the first pass writes `~/.ros/parking_enforcement/plate_log.json`, run: - -```bash -cd /home/jetson/rob498_2026/capstone2026/ros2_ws -source /opt/ros/foxy/setup.bash -source install/setup.bash -ros2 launch parking_enforcement parking_revisit.launch.py -``` - -Then call the same services: - -```bash -ros2 service call /rob498_drone_8/comm/launch std_srvs/srv/Trigger '{}' -ros2 service call /rob498_drone_8/comm/test std_srvs/srv/Trigger '{}' -``` - -## Topics to check before flight - -```bash -ros2 topic echo /avoid_dir -ros2 topic echo /plate_reader/current -ros2 topic echo /plate_reader/confirmed -ros2 topic echo /stereo/depth/camera_info -``` - -You should also see: - -- `/local_grid` -- `/stereo/depth` -- `/stereo/disp_vis` -- `/plate_reader/debug_image` - -## Stereo crop check - -Your stereo disparity output has a left-side invalid strip caused by rectification / disparity overlap. -That strip should be cropped before the occupancy node uses the depth image. - -To verify it: - -1. View `/stereo/disp_vis` -2. Confirm the persistent dark-blue left strip is removed from the published image -3. Echo `/stereo/depth/camera_info` -4. Confirm the width is smaller than the raw rectified width and the principal point `cx` has shifted left accordingly - -If the crop needs tuning, edit the crop constants in [config.py](/home/jetson/rob498_2026/capstone2026/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/config.py). - -## Mission behavior - -1. `launch` - - enters `OFFBOARD` - - arms - - climbs to `LAUNCH_ALT` -2. `test` - - starts the waypoint mission using the hardcoded parking list -3. waypoint flight - - if the center sector is clear, the mission continues normally - - if the center sector is blocked, it chooses the freer side from left/right - - if left, center, and right are all blocked, it backs up and re-evaluates -4. at each parking spot - - holds position - - yaws right - - scans vertically - - waits for a confirmed plate or timeout -5. end of mission - - sends `AUTO.LAND` - -## RViz debug topics - -Recommended displays: - -- `Map` -> `/local_grid` -- `Image` -> `/stereo/disp_vis` -- `Image` -> `/plate_reader/debug_image` -- `Pose` -> `/mavros/local_position/pose` - -## Notes - -- `land` is a normal `AUTO.LAND` request. -- `abort` also switches to `AUTO.LAND` for this demo. -- `home_pose` initializes from the first valid local pose sample. -- The occupancy map is local and reactive, not global SLAM. -- The planner uses only the forward `left`, `center`, and `right` sectors for decisions, even though the full 2D grid is still published for debugging. diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_demo.launch.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_demo.launch.py deleted file mode 100644 index e3e7e61..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_demo.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -"""Backward-compatible alias for the first-pass parking demo launch.""" - -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare - - - -def generate_launch_description(): - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('parking_enforcement'), - 'launch', - 'parking_first_pass.launch.py', - ]) - ) - ) - ]) diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_first_pass.launch.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_first_pass.launch.py deleted file mode 100644 index 862c394..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_first_pass.launch.py +++ /dev/null @@ -1,39 +0,0 @@ -"""Launch the parking-enforcement first pass. - -MAVROS, the external pose bridge, and the raw camera publishers are expected to -be running separately. This launch file brings up the parking package's stereo -depth, mission, obstacle-avoidance, and plate-reading nodes. -""" - -from launch import LaunchDescription -from launch_ros.actions import Node - - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='parking_enforcement', - executable='stereo_depth_node', - name='stereo_depth_node', - output='screen', - ), - Node( - package='parking_enforcement', - executable='local_costmap_node', - name='local_costmap_node', - output='screen', - ), - Node( - package='parking_enforcement', - executable='plate_reader_node', - name='plate_reader_node', - output='screen', - ), - Node( - package='parking_enforcement', - executable='parking_mission_node', - name='parking_mission_node', - output='screen', - ), - ]) diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_revisit.launch.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_revisit.launch.py deleted file mode 100644 index 5aa194e..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/launch/parking_revisit.launch.py +++ /dev/null @@ -1,38 +0,0 @@ -"""Launch the revisit pass for the parking-enforcement demo. - -Like the first-pass launch, this assumes MAVROS, the external pose bridge, and -the raw camera publishers already exist in the wider system. -""" - -from launch import LaunchDescription -from launch_ros.actions import Node - - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='parking_enforcement', - executable='stereo_depth_node', - name='stereo_depth_node', - output='screen', - ), - Node( - package='parking_enforcement', - executable='local_costmap_node', - name='local_costmap_node', - output='screen', - ), - Node( - package='parking_enforcement', - executable='plate_reader_node', - name='plate_reader_node', - output='screen', - ), - Node( - package='parking_enforcement', - executable='parking_revisit_node', - name='parking_revisit_node', - output='screen', - ), - ]) diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/package.xml b/ros2_ws/src/parking_enforcement_package/parking_enforcement/package.xml deleted file mode 100644 index 0026491..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - parking_enforcement - 0.1.0 - ROS2 nodes for ROB498 parking enforcement demo. - OpenAI - MIT - - ament_python - - rclpy - std_msgs - std_srvs - sensor_msgs - geometry_msgs - nav_msgs - mavros_msgs - cv_bridge - message_filters - tf2_ros - python3-numpy - python3-opencv - python3-scipy - - - ament_python - - diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/__init__.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/config.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/config.py deleted file mode 100644 index 248b4ef..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/config.py +++ /dev/null @@ -1,141 +0,0 @@ -"""Shared configuration for the parking_enforcement ROS 2 package. - -Edit this file first when you move between demo setups. The package keeps the -parking mission separate from the old flight3 package, but it preserves the same -service interface and general offboard-control flow. -""" - -from pathlib import Path - -# Drone namespace and frame assumptions. -DRONE_NS = 'rob498_drone_8' -FRAME_ID = 'map' -GRID_FRAME = 'base_link' -STEREO_FRAME_ID = 'camera_fisheye1_optical_frame' -CAMERA_BODY_ALIGNED = True - -# Hardcoded parking waypoints for the current demo phase. -# Replace these tuples with your real parking-spot coordinates before flight. -PARKING_WAYPOINTS = [ - (-1.0, -1.0, 1.0), - (1.0, -1.0, 2.0), - (1.0, 1.0, 1.0), - (-1.0, 1.0, 0.5), -] - -# Mission behavior. -LAUNCH_ALT = 0.5 -TARGET_RADIUS = 0.20 -POSITION_HOLD_RADIUS = 0.12 -WAYPOINT_SETTLE_TIME = 1.0 -MISSION_RATE_HZ = 20.0 -Z_OFFSET = 0.0 - -# Reactive avoidance behavior. -AVOID_STEP = 0.60 -AVOID_FORWARD_STEP = 0.20 -BACK_UP_STEP = 0.25 -AVOID_TIMEOUT_SEC = 4.0 - -# Scan behavior at each parking spot. -SCAN_YAW_RIGHT_DEG = 90.0 -SCAN_AMPLITUDE = 0.18 -SCAN_PERIOD_SEC = 4.0 -SCAN_TIMEOUT_SEC = 12.0 - -# Revisit / overstay behavior. -REVISIT_OVERSTAY_THRESHOLD_SEC = 120.0 - -# Shared log locations. -LOG_DIR = Path.home() / '.ros' / 'parking_enforcement' -FIRST_PASS_LOG_PATH = LOG_DIR / 'plate_log.json' -REVISIT_LOG_PATH = LOG_DIR / 'revisit_log.json' - -# MAVROS topics and services. -LOCAL_POSE_TOPIC = 'mavros/local_position/pose' -SETPOINT_TOPIC = 'mavros/setpoint_position/local' -MAVROS_STATE_TOPIC = 'mavros/state' -MAVROS_ARM_TOPIC = 'mavros/cmd/arming' -MAVROS_SET_MODE_TOPIC = 'mavros/set_mode' - -# Stereo camera topics. -LEFT_CAMERA_INFO_TOPIC = '/camera/fisheye1/camera_info' -RIGHT_CAMERA_INFO_TOPIC = '/camera/fisheye2/camera_info' -LEFT_IMAGE_TOPIC = '/camera/fisheye1/image_raw' -RIGHT_IMAGE_TOPIC = '/camera/fisheye2/image_raw' - -# Depth topics produced by the parking package stereo node. -DEPTH_TOPIC = '/stereo/depth' -DEPTH_INFO_TOPIC = '/stereo/depth/camera_info' -DISPARITY_VIS_TOPIC = '/stereo/disp_vis' - -# Plate-reader topics. -RGB_IMAGE_TOPIC = '/camera/image_raw' -PLATE_ENABLED_TOPIC = '/plate_reader/enabled' -PLATE_CONFIRMED_TOPIC = '/plate_reader/confirmed' -PLATE_CURRENT_TOPIC = '/plate_reader/current' -PLATE_DEBUG_IMAGE_TOPIC = '/plate_reader/debug_image' - -# Local grid topics. -LOCAL_GRID_TOPIC = '/local_grid' -AVOID_TOPIC = '/avoid_dir' - -# TensorRT engine locations. -LPD_ENGINE_PATH = '/home/jetson/engines/lpdnet_fp32.engine' -LPR_ENGINE_PATH = '/home/jetson/engines/lprnet_fp32.engine' - -# Stereo depth settings. -MIN_DEPTH_METERS = 0.20 -MAX_DEPTH_METERS = 4.00 -SKIP_FRAMES = 3 -NUM_DISPARITIES = 64 -VERTICAL_OFFSET_PIXELS = -2 -DOWNSCALE = 0.5 - -# Left-side crop handling. -# Set LEFT_CROP_OVERRIDE_PIXELS to a positive integer if you want to force the -# crop width instead of estimating it from the rectification overlap. -LEFT_CROP_OVERRIDE_PIXELS = None -LEFT_CROP_MIN_OVERLAP_RATIO = 0.85 -LEFT_CROP_EXTRA_PADDING_PIXELS = 2 -MIN_PUBLISHED_WIDTH_PIXELS = 32 - -# Occupancy-grid settings. -GRID_X_MIN = 0.0 -GRID_X_MAX = 3.0 -GRID_Y_MIN = -1.5 -GRID_Y_MAX = 1.5 -GRID_RESOLUTION = 0.10 -POINT_MIN_DEPTH = 0.25 -POINT_MAX_DEPTH = 3.0 -POINT_STRIDE = 6 -POINT_Z_MIN = -0.80 -POINT_Z_MAX = 0.80 - -# Sector windows used for avoidance decisions. -SECTOR_X_MIN = 0.40 -SECTOR_X_MAX = 1.40 -CENTER_HALF_WIDTH = 0.35 -SIDE_HALF_WIDTH = 1.20 -CENTER_OCC_THRESHOLD = 0.10 -SIDE_OCC_THRESHOLD = 0.12 -CENTER_MIN_KNOWN_RATIO = 0.35 -SIDE_MIN_KNOWN_RATIO = 0.20 -HYSTERESIS_LEN = 5 - -# Plate-reader behavior. -LPD_W = 640 -LPD_H = 480 -LPR_W = 96 -LPR_H = 48 -CHARS = '0123456789ABCDEFGHIJKLMNPQRSTUVWXYZ' -BLANK = 35 -DETECT_EVERY = 6 -PAD_LEFT = 15 -PAD_RIGHT = 0 -PAD_TOP = 10 -PAD_BOTTOM = 10 -MIN_DET_CONF = 0.60 -MIN_READ_CONF = 0.45 -MIN_VOTES = 10 -VOTE_WINDOW = 40 diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/local_costmap_node.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/local_costmap_node.py deleted file mode 100644 index 93e4ed1..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/local_costmap_node.py +++ /dev/null @@ -1,331 +0,0 @@ -#!/usr/bin/env python3 -"""Build a local 2D occupancy grid from stereo depth. - -This node keeps the full 2D grid for RViz/debugging, but the flight decision is -based only on three forward sectors: left, center, and right. Vertical up/down -structure is ignored outside a fixed body-height slice. -""" - -from __future__ import annotations - -from collections import deque - -import numpy as np -import rclpy -from cv_bridge import CvBridge -from nav_msgs.msg import OccupancyGrid -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from sensor_msgs.msg import CameraInfo, Image -from std_msgs.msg import String - -from parking_enforcement.config import ( - AVOID_TOPIC, - CENTER_HALF_WIDTH, - CENTER_MIN_KNOWN_RATIO, - CENTER_OCC_THRESHOLD, - GRID_FRAME, - GRID_RESOLUTION, - GRID_X_MAX, - GRID_X_MIN, - GRID_Y_MAX, - GRID_Y_MIN, - HYSTERESIS_LEN, - LOCAL_GRID_TOPIC, - POINT_MAX_DEPTH, - POINT_MIN_DEPTH, - POINT_STRIDE, - POINT_Z_MAX, - POINT_Z_MIN, - SECTOR_X_MAX, - SECTOR_X_MIN, - SIDE_HALF_WIDTH, - SIDE_MIN_KNOWN_RATIO, - SIDE_OCC_THRESHOLD, - DEPTH_INFO_TOPIC, - DEPTH_TOPIC, -) - - -class LocalCostmapNode(Node): - """Convert cropped stereo depth into a rolling occupancy grid and sector decision.""" - - def __init__(self): - super().__init__('rob498_drone_8_local_costmap') - self.bridge = CvBridge() - - # Camera intrinsics come from the cropped depth camera_info topic. - self.fx = None - self.fy = None - self.cx = None - self.cy = None - - # Precompute grid geometry once so callbacks stay lightweight. - self.grid_width = int(round((GRID_X_MAX - GRID_X_MIN) / GRID_RESOLUTION)) - self.grid_height = int(round((GRID_Y_MAX - GRID_Y_MIN) / GRID_RESOLUTION)) - self.origin_row = int(round((0.0 - GRID_Y_MIN) / GRID_RESOLUTION)) - - # Hysteresis prevents the avoid direction from flipping every frame. - self.last_cmds = deque(maxlen=HYSTERESIS_LEN) - self.last_dir = 'BLOCKED' - self.last_sector_log_time = self.get_clock().now() - - self.grid_pub = self.create_publisher(OccupancyGrid, LOCAL_GRID_TOPIC, 10) - self.avoid_pub = self.create_publisher(String, AVOID_TOPIC, 10) - self.create_subscription(CameraInfo, DEPTH_INFO_TOPIC, self._camera_info_cb, 10) - self.create_subscription(Image, DEPTH_TOPIC, self._depth_cb, qos_profile_sensor_data) - - self.get_logger().info('Local costmap node started') - - def _camera_info_cb(self, msg: CameraInfo): - """Cache the current depth intrinsics for fast back-projection.""" - - self.fx = float(msg.k[0]) - self.fy = float(msg.k[4]) - self.cx = float(msg.k[2]) - self.cy = float(msg.k[5]) - - def _depth_cb(self, msg: Image): - """Convert depth into a 2D grid and a left/center/right avoid command.""" - - if self.fx is None: - return - - depth = self.bridge.imgmsg_to_cv2(msg, desired_encoding='32FC1') - pts_body = self._depth_to_body_points(depth) - grid = self._build_grid(pts_body) - avoid_dir, center_stats, left_stats, right_stats = self._choose_direction(grid) - self._publish_grid(grid, msg.header.stamp) - self._publish_direction(avoid_dir) - self._log_sector_stats(avoid_dir, center_stats, left_stats, right_stats) - - def _depth_to_body_points(self, depth: np.ndarray) -> np.ndarray: - """Back-project depth pixels into a body-frame slice used for 2D occupancy. - - With the stereo pair mounted on the front, we treat camera forward as body - forward and keep only points near the drone body height. - """ - - height, width = depth.shape - vs, us = np.mgrid[0:height:POINT_STRIDE, 0:width:POINT_STRIDE] - - z = depth[vs, us] - valid = np.isfinite(z) & (z > POINT_MIN_DEPTH) & (z < POINT_MAX_DEPTH) - if not np.any(valid): - return np.empty((0, 3), dtype=np.float32) - - us = us[valid].astype(np.float32) - vs = vs[valid].astype(np.float32) - z = z[valid].astype(np.float32) - - # OpenCV camera frame is x-right, y-down, z-forward. - x_cam = (us - self.cx) * z / self.fx - y_cam = (vs - self.cy) * z / self.fy - - # Convert to a simple body frame: x-forward, y-left, z-up. - x_body = z - y_body = -x_cam - z_body = -y_cam - pts_body = np.stack([x_body, y_body, z_body], axis=1) - - keep = ( - (pts_body[:, 0] >= GRID_X_MIN) & (pts_body[:, 0] < GRID_X_MAX) & - (pts_body[:, 1] >= GRID_Y_MIN) & (pts_body[:, 1] < GRID_Y_MAX) & - (pts_body[:, 2] >= POINT_Z_MIN) & (pts_body[:, 2] <= POINT_Z_MAX) - ) - return pts_body[keep] - - def _build_grid(self, pts_body: np.ndarray) -> np.ndarray: - """Rasterize the filtered 3D points into a local 2D occupancy grid. - - Unknown cells remain -1, free cells become 0 through raytracing, and hit - cells become 100. - """ - - grid = -np.ones((self.grid_height, self.grid_width), dtype=np.int8) - if pts_body.size == 0: - return grid - - cols = ((pts_body[:, 0] - GRID_X_MIN) / GRID_RESOLUTION).astype(np.int32) - rows = ((pts_body[:, 1] - GRID_Y_MIN) / GRID_RESOLUTION).astype(np.int32) - keep = ( - (rows >= 0) & (rows < self.grid_height) & - (cols >= 0) & (cols < self.grid_width) - ) - rows = rows[keep] - cols = cols[keep] - if rows.size == 0: - return grid - - unique_cells = np.unique(np.stack([rows, cols], axis=1), axis=0) - for row, col in unique_cells: - # Mark the observed line of sight as free, then the hit cell as occupied. - ray = self._bresenham(self.origin_row, 0, int(row), int(col)) - for ray_row, ray_col in ray[:-1]: - if 0 <= ray_row < self.grid_height and 0 <= ray_col < self.grid_width: - if grid[ray_row, ray_col] == -1: - grid[ray_row, ray_col] = 0 - grid[int(row), int(col)] = 100 - return grid - - @staticmethod - def _bresenham(r0: int, c0: int, r1: int, c1: int): - """Return the grid cells along a ray so we can carve free space.""" - - points = [] - dr = abs(r1 - r0) - dc = abs(c1 - c0) - sr = 1 if r0 < r1 else -1 - sc = 1 if c0 < c1 else -1 - err = dc - dr - row, col = r0, c0 - - while True: - points.append((row, col)) - if row == r1 and col == c1: - break - e2 = 2 * err - if e2 > -dr: - err -= dr - col += sc - if e2 < dc: - err += dc - row += sr - return points - - def _choose_direction(self, grid: np.ndarray): - """Use only the left/center/right forward sectors for avoidance decisions.""" - - center_stats = self._window_stats(grid, SECTOR_X_MIN, SECTOR_X_MAX, -CENTER_HALF_WIDTH, CENTER_HALF_WIDTH) - left_stats = self._window_stats(grid, SECTOR_X_MIN, SECTOR_X_MAX, CENTER_HALF_WIDTH, SIDE_HALF_WIDTH) - right_stats = self._window_stats(grid, SECTOR_X_MIN, SECTOR_X_MAX, -SIDE_HALF_WIDTH, -CENTER_HALF_WIDTH) - - center_clear = ( - center_stats['known_ratio'] >= CENTER_MIN_KNOWN_RATIO - and center_stats['occ_ratio'] < CENTER_OCC_THRESHOLD - ) - left_open = ( - left_stats['known_ratio'] >= SIDE_MIN_KNOWN_RATIO - and left_stats['occ_ratio'] < SIDE_OCC_THRESHOLD - ) - right_open = ( - right_stats['known_ratio'] >= SIDE_MIN_KNOWN_RATIO - and right_stats['occ_ratio'] < SIDE_OCC_THRESHOLD - ) - - if center_clear: - raw = 'CLEAR' - elif left_open and right_open: - # When both sides are usable, keep the currently active side if it is - # still competitive. This gives smoother sidesteps around obstacles. - if self.last_dir in ('LEFT', 'RIGHT'): - if self.last_dir == 'LEFT' and left_stats['occ_ratio'] <= right_stats['occ_ratio'] + 0.02: - raw = 'LEFT' - elif self.last_dir == 'RIGHT' and right_stats['occ_ratio'] <= left_stats['occ_ratio'] + 0.02: - raw = 'RIGHT' - else: - raw = 'LEFT' if left_stats['occ_ratio'] <= right_stats['occ_ratio'] else 'RIGHT' - else: - raw = 'LEFT' if left_stats['occ_ratio'] <= right_stats['occ_ratio'] else 'RIGHT' - elif left_open: - raw = 'LEFT' - elif right_open: - raw = 'RIGHT' - else: - # If center is not clear and neither side has enough known free space, - # the mission node will stop or back up before re-evaluating. - raw = 'BLOCKED' - - self.last_cmds.append(raw) - if len(self.last_cmds) < self.last_cmds.maxlen: - self.last_dir = raw - return raw, center_stats, left_stats, right_stats - - counts = {cmd: self.last_cmds.count(cmd) for cmd in set(self.last_cmds)} - stable = max(counts, key=counts.get) - self.last_dir = stable - return stable, center_stats, left_stats, right_stats - - def _window_stats(self, grid: np.ndarray, x0: float, x1: float, y0: float, y1: float) -> dict[str, float]: - """Compute how much of a sector is known and how much of it is occupied.""" - - c0 = int((x0 - GRID_X_MIN) / GRID_RESOLUTION) - c1 = int((x1 - GRID_X_MIN) / GRID_RESOLUTION) - r0 = int((y0 - GRID_Y_MIN) / GRID_RESOLUTION) - r1 = int((y1 - GRID_Y_MIN) / GRID_RESOLUTION) - - c0 = max(0, min(grid.shape[1], c0)) - c1 = max(0, min(grid.shape[1], c1)) - r0 = max(0, min(grid.shape[0], r0)) - r1 = max(0, min(grid.shape[0], r1)) - if r1 <= r0 or c1 <= c0: - return {'known_ratio': 0.0, 'occ_ratio': 1.0} - - window = grid[r0:r1, c0:c1] - total = float(window.size) - if total <= 0.0: - return {'known_ratio': 0.0, 'occ_ratio': 1.0} - - known = window != -1 - known_count = float(np.count_nonzero(known)) - known_ratio = known_count / total - if known_count <= 0.0: - return {'known_ratio': 0.0, 'occ_ratio': 1.0} - - occ_ratio = float(np.mean(window[known] == 100)) - return {'known_ratio': known_ratio, 'occ_ratio': occ_ratio} - - def _publish_grid(self, grid: np.ndarray, stamp): - """Publish the local occupancy grid for RViz and debugging.""" - - msg = OccupancyGrid() - msg.header.stamp = stamp - msg.header.frame_id = GRID_FRAME - msg.info.resolution = GRID_RESOLUTION - msg.info.width = self.grid_width - msg.info.height = self.grid_height - msg.info.origin.position.x = GRID_X_MIN - msg.info.origin.position.y = GRID_Y_MIN - msg.info.origin.position.z = 0.0 - msg.info.origin.orientation.w = 1.0 - msg.data = grid.reshape(-1).tolist() - self.grid_pub.publish(msg) - - def _publish_direction(self, avoid_dir: str): - """Publish the discrete avoidance result for the mission node.""" - - msg = String() - msg.data = avoid_dir - self.avoid_pub.publish(msg) - - def _log_sector_stats(self, avoid_dir: str, center_stats: dict[str, float], left_stats: dict[str, float], right_stats: dict[str, float]): - """Log sector occupancy at a low rate so tuning stays observable in flight tests.""" - - now = self.get_clock().now() - if (now - self.last_sector_log_time).nanoseconds <= 1e9: - return - - self.get_logger().info( - '[SECTORS] ' - f'center(k={center_stats["known_ratio"]:.2f}, o={center_stats["occ_ratio"]:.2f}) ' - f'left(k={left_stats["known_ratio"]:.2f}, o={left_stats["occ_ratio"]:.2f}) ' - f'right(k={right_stats["known_ratio"]:.2f}, o={right_stats["occ_ratio"]:.2f}) ' - f'-> {avoid_dir}' - ) - self.last_sector_log_time = now - - -def main(args=None): - """Spin the local costmap node until shutdown.""" - - rclpy.init(args=args) - node = LocalCostmapNode() - try: - rclpy.spin(node) - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/mission_utils.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/mission_utils.py deleted file mode 100644 index 6ac82b3..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/mission_utils.py +++ /dev/null @@ -1,111 +0,0 @@ -"""Utility helpers shared by the parking mission nodes. - -These helpers keep the first-pass mission node and the revisit mission node -consistent in how they handle poses, yaw, JSON logs, and small temporary -avoidance targets. -""" - -from __future__ import annotations - -import json -import math -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -from geometry_msgs.msg import Pose, Quaternion - - -@dataclass -class TemporaryTarget: - """A short-lived setpoint used for sidestep or back-up behavior.""" - - x: float - y: float - z: float - yaw: float - created_time: float - - -def ensure_parent_dir(path: Path) -> None: - """Create the parent directory for a log file if it does not exist yet.""" - - path.parent.mkdir(parents=True, exist_ok=True) - - -def write_json(path: Path, payload: Any) -> None: - """Write a JSON payload with stable formatting for easy inspection.""" - - ensure_parent_dir(path) - path.write_text(json.dumps(payload, indent=2, sort_keys=True) + '\n', encoding='utf-8') - - -def read_json(path: Path, default: Any) -> Any: - """Load a JSON file, or return the provided default if the file is missing.""" - - if not path.exists(): - return default - - try: - return json.loads(path.read_text(encoding='utf-8')) - except json.JSONDecodeError: - return default - - -def copy_pose(src: Pose) -> Pose: - """Make a deep-enough copy of a ROS pose for local mission state.""" - - pose = Pose() - pose.position.x = src.position.x - pose.position.y = src.position.y - pose.position.z = src.position.z - pose.orientation = copy_quaternion(src.orientation) - return pose - - -def copy_quaternion(src: Quaternion) -> Quaternion: - """Copy a quaternion so later edits do not mutate the original message.""" - - q = Quaternion() - q.x = src.x - q.y = src.y - q.z = src.z - q.w = src.w - return q - - -def quaternion_from_yaw(yaw: float) -> Quaternion: - """Create a flat-yaw quaternion for MAVROS position setpoints.""" - - q = Quaternion() - q.x = 0.0 - q.y = 0.0 - q.z = math.sin(yaw / 2.0) - q.w = math.cos(yaw / 2.0) - return q - - -def yaw_from_quaternion(q: Quaternion) -> float: - """Extract yaw from a quaternion using the standard ENU formula.""" - - siny_cosp = 2.0 * (q.w * q.z + q.x * q.y) - cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) - return math.atan2(siny_cosp, cosy_cosp) - - -def wrap_angle(angle: float) -> float: - """Wrap an angle to [-pi, pi] so yaw comparisons stay numerically stable.""" - - return math.atan2(math.sin(angle), math.cos(angle)) - - -def serialize_waypoints(waypoints: list[tuple[float, float, float]]) -> list[dict[str, Any]]: - """Convert the hardcoded waypoint list into JSON-safe dictionaries.""" - - return [ - { - 'waypoint_index': index + 1, - 'position': [float(x), float(y), float(z)], - } - for index, (x, y, z) in enumerate(waypoints) - ] diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/parking_mission_node.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/parking_mission_node.py deleted file mode 100644 index 73511f8..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/parking_mission_node.py +++ /dev/null @@ -1,612 +0,0 @@ -#!/usr/bin/env python3 -"""First-pass parking mission node. - -This node keeps the same service interface as the lab mission code, but it now -loads hardcoded parking waypoints from the shared config, uses the sector-based -avoidance result from `/avoid_dir`, scans for plates at each waypoint, and logs -confirmed plates to disk for the later revisit pass. -""" - -from __future__ import annotations - -import math - -import rclpy -from geometry_msgs.msg import PoseStamped -from mavros_msgs.msg import State -from mavros_msgs.srv import CommandBool, SetMode -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from std_msgs.msg import Bool, String -from std_srvs.srv import Trigger - -from parking_enforcement.config import ( - AVOID_FORWARD_STEP, - AVOID_STEP, - AVOID_TIMEOUT_SEC, - AVOID_TOPIC, - BACK_UP_STEP, - DRONE_NS, - FIRST_PASS_LOG_PATH, - FRAME_ID, - LAUNCH_ALT, - LOCAL_POSE_TOPIC, - MAVROS_ARM_TOPIC, - MAVROS_SET_MODE_TOPIC, - MAVROS_STATE_TOPIC, - MISSION_RATE_HZ, - PARKING_WAYPOINTS, - PLATE_CONFIRMED_TOPIC, - PLATE_ENABLED_TOPIC, - POSITION_HOLD_RADIUS, - SCAN_AMPLITUDE, - SCAN_PERIOD_SEC, - SCAN_TIMEOUT_SEC, - SCAN_YAW_RIGHT_DEG, - SETPOINT_TOPIC, - TARGET_RADIUS, - WAYPOINT_SETTLE_TIME, - Z_OFFSET, -) -from parking_enforcement.mission_utils import ( - TemporaryTarget, - copy_pose, - copy_quaternion, - quaternion_from_yaw, - serialize_waypoints, - write_json, - wrap_angle, - yaw_from_quaternion, -) - -STATE_IDLE = 'IDLE' -STATE_TAKEOFF = 'TAKEOFF' -STATE_WAIT_FOR_TEST = 'WAIT_FOR_TEST' -STATE_NAVIGATE = 'NAVIGATE' -STATE_AVOID = 'AVOID' -STATE_SCAN_YAW = 'SCAN_YAW' -STATE_SCAN_VERTICAL = 'SCAN_VERTICAL' -STATE_LANDING = 'LANDING' - - -class ParkingMissionNode(Node): - """Fly the first parking-enforcement pass and save confirmed plates to disk.""" - - def __init__(self, node_name: str | None = None): - super().__init__(node_name or f'{DRONE_NS}_mission') - - self.current_pos = PoseStamped() - self.current_state = State() - self.target_pose = PoseStamped() - self.target_pose.header.frame_id = FRAME_ID - self.home_pose = None - self.pose_received = False - - # Mission state and command flags follow the same pattern as the lab code: - # service callbacks only set flags, while the timer loop does the work. - self.state = STATE_IDLE - self.launch_requested = False - self.test_requested = False - self.land_requested = False - self.abort_requested = False - self.auto_land_sent = False - - # The parking spots are defined once in config.py for this demo phase. - self.waypoints = [tuple(map(float, waypoint)) for waypoint in PARKING_WAYPOINTS] - self.current_wp_index = 0 - self.current_wp_arrival_time = None - - # Avoidance and scan state. - self.avoid_dir = 'BLOCKED' - self.avoid_target = None - self.scan_center_pose = None - self.scan_yaw = 0.0 - self.scan_start_time = None - self.confirmed_plate = '' - - # First-pass log data is kept in memory and written to disk whenever it changes. - self.plate_log = {} - self.mission_start_time = None - - self.srv_launch = self.create_service(Trigger, f'{DRONE_NS}/comm/launch', self.callback_launch) - self.srv_test = self.create_service(Trigger, f'{DRONE_NS}/comm/test', self.callback_test) - self.srv_land = self.create_service(Trigger, f'{DRONE_NS}/comm/land', self.callback_land) - self.srv_abort = self.create_service(Trigger, f'{DRONE_NS}/comm/abort', self.callback_abort) - - self.arming_client = self.create_client(CommandBool, MAVROS_ARM_TOPIC) - self.set_mode_client = self.create_client(SetMode, MAVROS_SET_MODE_TOPIC) - - self.create_subscription(State, MAVROS_STATE_TOPIC, self._state_callback, 10) - self.create_subscription(PoseStamped, LOCAL_POSE_TOPIC, self._pos_callback, qos_profile_sensor_data) - self.create_subscription(String, AVOID_TOPIC, self._avoid_callback, 10) - self.create_subscription(String, PLATE_CONFIRMED_TOPIC, self._plate_confirmed_callback, 10) - - self.local_pos_pub = self.create_publisher(PoseStamped, SETPOINT_TOPIC, 10) - self.plate_enable_pub = self.create_publisher(Bool, PLATE_ENABLED_TOPIC, 10) - - self.timer = self.create_timer(1.0 / MISSION_RATE_HZ, self._timer_callback) - self.last_target_log_time = self.get_clock().now() - self.last_state_log_time = self.get_clock().now() - self.last_pos_log_time = self.get_clock().now() - self.last_nav_log_time = self.get_clock().now() - - self.get_logger().info('Waiting for MAVROS services...') - self.arming_client.wait_for_service() - self.set_mode_client.wait_for_service() - self._log_loaded_waypoints() - self.get_logger().info('First-pass parking mission node ready') - - def callback_launch(self, request, response): - """Arm and climb to the configured launch altitude.""" - - self.launch_requested = True - response.success = True - response.message = 'Launch flag set' - return response - - def callback_test(self, request, response): - """Start the waypoint mission after takeoff is complete.""" - - self.test_requested = True - response.success = len(self.waypoints) > 0 - response.message = 'Mission start flag set' if self.waypoints else 'No waypoints configured' - return response - - def callback_land(self, request, response): - """Normal operator-requested land uses PX4 AUTO.LAND.""" - - self.land_requested = True - response.success = True - response.message = 'Autonomous landing flag set' - return response - - def callback_abort(self, request, response): - """Abort also uses AUTO.LAND instead of an in-air kill for this demo.""" - - self.abort_requested = True - response.success = True - response.message = 'Abort flag set' - return response - - def _state_callback(self, msg: State): - """Track the current FCU state for operator visibility.""" - - self.current_state = msg - now = self.get_clock().now() - if (now - self.last_state_log_time).nanoseconds > 1e9: - self.get_logger().info( - f'[STATE] connected={msg.connected} armed={msg.armed} mode={msg.mode} mission={self.state}' - ) - self.last_state_log_time = now - - def _pos_callback(self, msg: PoseStamped): - """Initialize home pose from the first valid local-position sample.""" - - self.current_pos = msg - self.current_pos.pose.position.z += Z_OFFSET - self.pose_received = True - - if self.home_pose is None: - self.home_pose = PoseStamped() - self.home_pose.header = msg.header - self.home_pose.pose = copy_pose(msg.pose) - self.target_pose.pose = copy_pose(msg.pose) - self.get_logger().info( - '[HOME] Initialized at ' - f'({msg.pose.position.x:.2f}, {msg.pose.position.y:.2f}, {msg.pose.position.z:.2f})' - ) - - now = self.get_clock().now() - if (now - self.last_pos_log_time).nanoseconds > 1e9: - self.get_logger().info( - f'[POSE] x={msg.pose.position.x:.2f} y={msg.pose.position.y:.2f} z={msg.pose.position.z:.2f}' - ) - self.last_pos_log_time = now - - def _avoid_callback(self, msg: String): - """Receive the discrete left/center/right avoidance result from the costmap.""" - - self.avoid_dir = msg.data - - def _plate_confirmed_callback(self, msg: String): - """Store the latest confirmed plate from the TensorRT plate reader.""" - - self.confirmed_plate = msg.data - - def _timer_callback(self): - """Run the mission state machine and continuously stream MAVROS setpoints.""" - - if not self.pose_received: - return - - self._handle_command_flags() - - if self.state == STATE_TAKEOFF: - self._update_takeoff() - elif self.state == STATE_WAIT_FOR_TEST: - self._update_wait_for_test() - elif self.state == STATE_NAVIGATE: - self._update_navigation() - elif self.state == STATE_AVOID: - self._update_avoidance() - elif self.state == STATE_SCAN_YAW: - self._update_scan_yaw() - elif self.state == STATE_SCAN_VERTICAL: - self._update_scan_vertical() - elif self.state == STATE_LANDING: - pass - elif self.home_pose is not None: - self.target_pose.pose = copy_pose(self.home_pose.pose) - - self.target_pose.header.stamp = self.get_clock().now().to_msg() - self.target_pose.header.frame_id = FRAME_ID - self.local_pos_pub.publish(self.target_pose) - self._publish_plate_enable(self.state in (STATE_SCAN_YAW, STATE_SCAN_VERTICAL)) - - now = self.get_clock().now() - if (now - self.last_target_log_time).nanoseconds > 1e9: - position = self.target_pose.pose.position - self.get_logger().info( - f'[TARGET] x={position.x:.2f} y={position.y:.2f} z={position.z:.2f} state={self.state}' - ) - self.last_target_log_time = now - - def _handle_command_flags(self): - """Translate service flags into one-time mission state changes.""" - - if self.abort_requested: - self.abort_requested = False - self.get_logger().warn('[ABORT] Switching to AUTO.LAND') - self._command_auto_land() - return - - if self.land_requested: - self.land_requested = False - self.get_logger().warn('[LAND] Switching to AUTO.LAND') - self._command_auto_land() - return - - if self.launch_requested: - self.launch_requested = False - self._start_takeoff() - - if self.test_requested and self.state == STATE_WAIT_FOR_TEST and self.waypoints: - self.test_requested = False - self.current_wp_index = 0 - self.current_wp_arrival_time = None - self.confirmed_plate = '' - self.mission_start_time = self._now_sec() - self.state = STATE_NAVIGATE - self.get_logger().info('[MISSION] Test started, entering NAVIGATE state') - - def _start_takeoff(self): - """Enter OFFBOARD, arm, and climb straight up from home.""" - - if self.home_pose is None: - self.get_logger().warn('[LAUNCH] No pose yet, cannot launch') - return - - self.target_pose.pose = copy_pose(self.current_pos.pose) - self.target_pose.pose.position.z = self.home_pose.pose.position.z + LAUNCH_ALT - self.state = STATE_TAKEOFF - self.auto_land_sent = False - self._set_mode('OFFBOARD') - self._arming(True) - self.get_logger().info(f'[LAUNCH] Takeoff target z={self.target_pose.pose.position.z:.2f}') - - def _update_takeoff(self): - """Hold XY at home while climbing to the launch altitude.""" - - target_z = self.home_pose.pose.position.z + LAUNCH_ALT - self.target_pose.pose.position.x = self.home_pose.pose.position.x - self.target_pose.pose.position.y = self.home_pose.pose.position.y - self.target_pose.pose.position.z = target_z - self.target_pose.pose.orientation = copy_quaternion(self.home_pose.pose.orientation) - - if abs(self.current_pos.pose.position.z - target_z) < 0.12: - self.state = STATE_WAIT_FOR_TEST - self.get_logger().info('[LAUNCH] Takeoff complete, waiting for TEST command') - - def _update_wait_for_test(self): - """Hold a steady hover above home until the mission starts.""" - - self.target_pose.pose.position.x = self.home_pose.pose.position.x - self.target_pose.pose.position.y = self.home_pose.pose.position.y - self.target_pose.pose.position.z = self.home_pose.pose.position.z + LAUNCH_ALT - self.target_pose.pose.orientation = copy_quaternion(self.home_pose.pose.orientation) - - def _update_navigation(self): - """Fly toward the active waypoint unless the local costmap requests avoidance.""" - - if self.current_wp_index >= len(self.waypoints): - self.get_logger().info('[MISSION] All waypoints done, landing in place') - self._command_auto_land() - return - - waypoint = self.waypoints[self.current_wp_index] - yaw = yaw_from_quaternion(self.current_pos.pose.orientation) - - self.target_pose.pose.position.x = waypoint[0] - self.target_pose.pose.position.y = waypoint[1] - self.target_pose.pose.position.z = waypoint[2] - self.target_pose.pose.orientation = quaternion_from_yaw(yaw) - - dist = self._distance_to_target(*waypoint) - now = self.get_clock().now() - if (now - self.last_nav_log_time).nanoseconds > 1e9: - self.get_logger().info( - f'[NAV] waypoint={self.current_wp_index + 1} dist={dist:.2f} avoid={self.avoid_dir}' - ) - self.last_nav_log_time = now - - if self.avoid_dir in ('LEFT', 'RIGHT', 'BLOCKED') and dist > POSITION_HOLD_RADIUS: - self._start_avoidance(self.avoid_dir) - return - - if dist < TARGET_RADIUS: - if self.current_wp_arrival_time is None: - self.current_wp_arrival_time = self._now_sec() - self.get_logger().info(f'[WAYPOINT] Reached waypoint {self.current_wp_index + 1}, settling') - elif self._now_sec() - self.current_wp_arrival_time > WAYPOINT_SETTLE_TIME: - self._start_scan() - else: - self.current_wp_arrival_time = None - - def _start_avoidance(self, direction: str): - """Generate a temporary sidestep or back-up target from the sector decision.""" - - current = self.current_pos.pose.position - yaw = yaw_from_quaternion(self.current_pos.pose.orientation) - - if direction == 'LEFT': - forward = AVOID_FORWARD_STEP - lateral = AVOID_STEP - elif direction == 'RIGHT': - forward = AVOID_FORWARD_STEP - lateral = -AVOID_STEP - else: - # BLOCKED means no safe left/right sector is known, so back up and - # let the costmap re-evaluate before committing laterally. - forward = -BACK_UP_STEP - lateral = 0.0 - - x = current.x + forward * math.cos(yaw) - lateral * math.sin(yaw) - y = current.y + forward * math.sin(yaw) + lateral * math.cos(yaw) - z = current.z - - self.avoid_target = TemporaryTarget(x=x, y=y, z=z, yaw=yaw, created_time=self._now_sec()) - self.state = STATE_AVOID - self.get_logger().warn( - f'[AVOID] direction={direction} temporary target=({x:.2f}, {y:.2f}, {z:.2f})' - ) - - def _update_avoidance(self): - """Fly to the temporary avoid target, then resume normal navigation.""" - - if self.avoid_target is None: - self.state = STATE_NAVIGATE - return - - self.target_pose.pose.position.x = self.avoid_target.x - self.target_pose.pose.position.y = self.avoid_target.y - self.target_pose.pose.position.z = self.avoid_target.z - self.target_pose.pose.orientation = quaternion_from_yaw(self.avoid_target.yaw) - - dist = self._distance_to_target(self.avoid_target.x, self.avoid_target.y, self.avoid_target.z) - timed_out = (self._now_sec() - self.avoid_target.created_time) > AVOID_TIMEOUT_SEC - if self.avoid_dir == 'CLEAR' or dist < TARGET_RADIUS or timed_out: - self.state = STATE_NAVIGATE - self.avoid_target = None - self.current_wp_arrival_time = None - self.get_logger().info('[AVOID] Returning to waypoint navigation') - - def _start_scan(self): - """Freeze over the parking spot, yaw right, and enable the plate reader.""" - - self.scan_center_pose = PoseStamped() - self.scan_center_pose.header = self.current_pos.header - self.scan_center_pose.pose = copy_pose(self.current_pos.pose) - current_yaw = yaw_from_quaternion(self.current_pos.pose.orientation) - self.scan_yaw = wrap_angle(current_yaw - math.radians(SCAN_YAW_RIGHT_DEG)) - self.scan_start_time = None - self.confirmed_plate = '' - self.state = STATE_SCAN_YAW - self.get_logger().info(f'[SCAN] Turning right at waypoint {self.current_wp_index + 1}') - - def _update_scan_yaw(self): - """Rotate in place before starting the small vertical scan motion.""" - - if self.scan_center_pose is None: - self.state = STATE_NAVIGATE - return - - position = self.scan_center_pose.pose.position - self.target_pose.pose.position.x = position.x - self.target_pose.pose.position.y = position.y - self.target_pose.pose.position.z = position.z - self.target_pose.pose.orientation = quaternion_from_yaw(self.scan_yaw) - - yaw_now = yaw_from_quaternion(self.current_pos.pose.orientation) - if abs(wrap_angle(yaw_now - self.scan_yaw)) < 0.15: - self.scan_start_time = self._now_sec() - self.state = STATE_SCAN_VERTICAL - self.get_logger().info('[SCAN] Yaw reached, scanning vertically') - - def _update_scan_vertical(self): - """Oscillate in Z until a plate is confirmed or the scan times out.""" - - if self.scan_center_pose is None or self.scan_start_time is None: - self.state = STATE_NAVIGATE - return - - elapsed = self._now_sec() - self.scan_start_time - position = self.scan_center_pose.pose.position - z_cmd = position.z + SCAN_AMPLITUDE * math.sin(2.0 * math.pi * elapsed / SCAN_PERIOD_SEC) - - self.target_pose.pose.position.x = position.x - self.target_pose.pose.position.y = position.y - self.target_pose.pose.position.z = z_cmd - self.target_pose.pose.orientation = quaternion_from_yaw(self.scan_yaw) - - if self.confirmed_plate: - self._record_plate_for_current_waypoint(self.confirmed_plate) - self._advance_waypoint() - return - - if elapsed > SCAN_TIMEOUT_SEC: - self.get_logger().warn(f'[SCAN] Timeout at waypoint {self.current_wp_index + 1}, moving on') - self._advance_waypoint() - - def _record_plate_for_current_waypoint(self, plate: str): - """Store the confirmed plate and write the first-pass log file to disk.""" - - waypoint = self.waypoints[self.current_wp_index] - self.plate_log[str(self.current_wp_index + 1)] = { - 'waypoint_index': self.current_wp_index + 1, - 'position': [float(waypoint[0]), float(waypoint[1]), float(waypoint[2])], - 'plate': plate, - 'first_seen_time_sec': round(self._now_sec(), 2), - } - self.get_logger().info(f'[PLATE] waypoint={self.current_wp_index + 1} plate={plate}') - self._write_first_pass_log() - - def _advance_waypoint(self): - """Leave the current parking spot and continue to the next mission target.""" - - self.confirmed_plate = '' - self.scan_center_pose = None - self.scan_start_time = None - self.current_wp_arrival_time = None - self.current_wp_index += 1 - self.state = STATE_NAVIGATE - if self.current_wp_index < len(self.waypoints): - self.get_logger().info(f'[MISSION] Moving to waypoint {self.current_wp_index + 1}') - else: - self.get_logger().info('[MISSION] Final waypoint complete') - self._write_first_pass_log() - - def _command_auto_land(self): - """Switch PX4 into AUTO.LAND once and keep the node in LANDING state.""" - - if self.auto_land_sent: - self.state = STATE_LANDING - return - - self.auto_land_sent = True - self.state = STATE_LANDING - self._write_first_pass_log() - req = SetMode.Request() - req.custom_mode = 'AUTO.LAND' - future = self.set_mode_client.call_async(req) - future.add_done_callback(self._auto_land_callback) - - def _auto_land_callback(self, future): - """Report whether PX4 accepted AUTO.LAND.""" - - try: - result = future.result() - if result.mode_sent: - self.get_logger().warn('[LAND] AUTO.LAND sent to PX4') - else: - self.get_logger().error('[LAND] PX4 rejected AUTO.LAND') - except Exception as exc: - self.get_logger().error(f'[LAND] AUTO.LAND service call failed: {exc}') - - def _arming(self, value: bool): - """Send the MAVROS arm or disarm command.""" - - req = CommandBool.Request() - req.value = value - future = self.arming_client.call_async(req) - future.add_done_callback(lambda f: self._arming_callback(f, value)) - - def _arming_callback(self, future, value: bool): - """Log the result of the arm/disarm request.""" - - try: - result = future.result() - if result.success: - self.get_logger().info(f'[ARM] set to {value}') - else: - self.get_logger().warn(f'[ARM] command failed for {value}') - except Exception as exc: - self.get_logger().error(f'[ARM] service call failed: {exc}') - - def _set_mode(self, mode_string: str): - """Send a PX4 mode change request through MAVROS.""" - - req = SetMode.Request() - req.custom_mode = mode_string - future = self.set_mode_client.call_async(req) - future.add_done_callback(lambda f: self._set_mode_callback(f, mode_string)) - - def _set_mode_callback(self, future, mode_string: str): - """Log the result of the mode change request.""" - - try: - result = future.result() - if result.mode_sent: - self.get_logger().info(f'[MODE] {mode_string}') - else: - self.get_logger().warn(f'[MODE] failed to enter {mode_string}') - except Exception as exc: - self.get_logger().error(f'[MODE] service call failed: {exc}') - - def _publish_plate_enable(self, enabled: bool): - """Only run TensorRT plate inference during the scan states.""" - - msg = Bool() - msg.data = enabled - self.plate_enable_pub.publish(msg) - - def _distance_to_target(self, x: float, y: float, z: float) -> float: - """Euclidean distance from the current local pose to a target setpoint.""" - - dx = float(x) - self.current_pos.pose.position.x - dy = float(y) - self.current_pos.pose.position.y - dz = float(z) - self.current_pos.pose.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - def _now_sec(self) -> float: - """Current ROS clock time in seconds.""" - - return self.get_clock().now().nanoseconds * 1e-9 - - def _log_loaded_waypoints(self): - """Print the hardcoded parking waypoint list at startup for quick review.""" - - if not self.waypoints: - self.get_logger().warn('No parking waypoints are configured in config.py') - return - - for index, waypoint in enumerate(self.waypoints, start=1): - self.get_logger().info( - f'[WAYPOINT LOAD] {index}: x={waypoint[0]:.2f} y={waypoint[1]:.2f} z={waypoint[2]:.2f}' - ) - - def _write_first_pass_log(self): - """Persist the first-pass mission results for the separate revisit node.""" - - payload = { - 'drone_ns': DRONE_NS, - 'mission_start_time_sec': None if self.mission_start_time is None else round(self.mission_start_time, 2), - 'waypoints': serialize_waypoints(self.waypoints), - 'entries': [self.plate_log[key] for key in sorted(self.plate_log, key=lambda item: int(item))], - } - write_json(FIRST_PASS_LOG_PATH, payload) - - -def main(args=None): - """Spin the first-pass mission node until shutdown.""" - - rclpy.init(args=args) - node = ParkingMissionNode() - try: - rclpy.spin(node) - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/parking_revisit_node.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/parking_revisit_node.py deleted file mode 100644 index 38c3938..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/parking_revisit_node.py +++ /dev/null @@ -1,171 +0,0 @@ -#!/usr/bin/env python3 -"""Revisit mission node for the parking-enforcement demo. - -This node reuses the same hardcoded waypoint mission flow as the first pass, but -it loads the first-pass plate log, revisits the same parking spots, rescans the -plate at each spot, and records whether the same plate has overstayed. -""" - -from __future__ import annotations - -import math - -import rclpy - -from parking_enforcement.config import ( - DRONE_NS, - FIRST_PASS_LOG_PATH, - REVISIT_LOG_PATH, - REVISIT_OVERSTAY_THRESHOLD_SEC, - SCAN_AMPLITUDE, - SCAN_PERIOD_SEC, - SCAN_TIMEOUT_SEC, -) -from parking_enforcement.mission_utils import quaternion_from_yaw, read_json, serialize_waypoints, write_json -from parking_enforcement.parking_mission_node import ParkingMissionNode, STATE_NAVIGATE - - -class ParkingRevisitNode(ParkingMissionNode): - """Fly the revisit pass and compare each parking spot against the first pass.""" - - def __init__(self): - super().__init__(node_name=f'{DRONE_NS}_revisit_mission') - self.first_pass_payload = read_json(FIRST_PASS_LOG_PATH, {}) - self.first_pass_entries = self._index_first_pass_entries(self.first_pass_payload) - self.revisit_log = {} - - if self.first_pass_entries: - self.get_logger().info( - f'Loaded {len(self.first_pass_entries)} first-pass plate entries from {FIRST_PASS_LOG_PATH}' - ) - else: - self.get_logger().warn( - f'No first-pass entries found at {FIRST_PASS_LOG_PATH}. Revisit will still scan and log results.' - ) - - @staticmethod - def _index_first_pass_entries(payload: dict) -> dict[int, dict]: - """Key first-pass entries by waypoint index for quick revisit lookup.""" - - indexed = {} - for entry in payload.get('entries', []): - try: - waypoint_index = int(entry['waypoint_index']) - except (KeyError, TypeError, ValueError): - continue - indexed[waypoint_index] = entry - return indexed - - def _update_scan_vertical(self): - """Scan vertically until we confirm a plate or time out at the parking spot.""" - - if self.scan_center_pose is None or self.scan_start_time is None: - self.state = STATE_NAVIGATE - return - - elapsed = self._now_sec() - self.scan_start_time - position = self.scan_center_pose.pose.position - z_cmd = position.z + SCAN_AMPLITUDE * math.sin(2.0 * math.pi * elapsed / SCAN_PERIOD_SEC) - - self.target_pose.pose.position.x = position.x - self.target_pose.pose.position.y = position.y - self.target_pose.pose.position.z = z_cmd - self.target_pose.pose.orientation = quaternion_from_yaw(self.scan_yaw) - - if self.confirmed_plate: - self._record_plate_for_current_waypoint(self.confirmed_plate) - self._advance_waypoint() - return - - if elapsed > SCAN_TIMEOUT_SEC: - self.get_logger().warn(f'[REVISIT] Timeout at waypoint {self.current_wp_index + 1}, moving on') - self._record_timeout_for_current_waypoint() - self._advance_waypoint() - - def _record_plate_for_current_waypoint(self, plate: str): - """Compare the current plate reading against the first-pass result.""" - - entry = self._build_revisit_entry(plate, scan_status='confirmed') - self.revisit_log[str(self.current_wp_index + 1)] = entry - self.get_logger().info( - f"[REVISIT] waypoint={entry['waypoint_index']} expected={entry['expected_plate'] or 'NONE'} " - f"observed={entry['observed_plate']} same={entry['same_plate']} overstayed={entry['overstayed']}" - ) - self._write_revisit_log() - - def _record_timeout_for_current_waypoint(self): - """Persist a timeout result so missed plates still show up in the revisit log.""" - - entry = self._build_revisit_entry('', scan_status='timeout') - self.revisit_log[str(self.current_wp_index + 1)] = entry - self._write_revisit_log() - - def _build_revisit_entry(self, observed_plate: str, scan_status: str) -> dict: - """Create one revisit-log record for the active waypoint.""" - - waypoint_index = self.current_wp_index + 1 - waypoint = self.waypoints[self.current_wp_index] - first_pass_entry = self.first_pass_entries.get(waypoint_index, {}) - expected_plate = str(first_pass_entry.get('plate', '')) - first_seen_time = first_pass_entry.get('first_seen_time_sec') - - elapsed_sec = None - same_plate = False - overstayed = False - if first_seen_time is not None: - try: - elapsed_sec = round(self._now_sec() - float(first_seen_time), 2) - except (TypeError, ValueError): - elapsed_sec = None - - if observed_plate and expected_plate: - same_plate = observed_plate == expected_plate - overstayed = bool( - same_plate and elapsed_sec is not None and elapsed_sec >= REVISIT_OVERSTAY_THRESHOLD_SEC - ) - - return { - 'waypoint_index': waypoint_index, - 'position': [float(waypoint[0]), float(waypoint[1]), float(waypoint[2])], - 'expected_plate': expected_plate, - 'observed_plate': observed_plate, - 'same_plate': same_plate, - 'overstayed': overstayed, - 'elapsed_sec': elapsed_sec, - 'first_seen_time_sec': first_seen_time, - 'revisit_time_sec': round(self._now_sec(), 2), - 'scan_status': scan_status, - } - - def _write_first_pass_log(self): - """Redirect the inherited write hook to the revisit output file instead.""" - - self._write_revisit_log() - - def _write_revisit_log(self): - """Write the revisit results so the demo can be inspected after flight.""" - - payload = { - 'drone_ns': DRONE_NS, - 'source_first_pass_log': str(FIRST_PASS_LOG_PATH), - 'overstay_threshold_sec': REVISIT_OVERSTAY_THRESHOLD_SEC, - 'waypoints': serialize_waypoints(self.waypoints), - 'entries': [self.revisit_log[key] for key in sorted(self.revisit_log, key=lambda item: int(item))], - } - write_json(REVISIT_LOG_PATH, payload) - - -def main(args=None): - """Spin the revisit mission node until shutdown.""" - - rclpy.init(args=args) - node = ParkingRevisitNode() - try: - rclpy.spin(node) - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/stereo_depth_node.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/stereo_depth_node.py deleted file mode 100644 index f691100..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/parking_enforcement/stereo_depth_node.py +++ /dev/null @@ -1,324 +0,0 @@ -#!/usr/bin/env python3 -"""Self-contained stereo depth node for the parking package. - -This node computes cropped stereo depth directly inside the parking package so -the local costmap can consume `/stereo/depth` and `/stereo/depth/camera_info` -without depending on the separate perception package. -""" - -from __future__ import annotations - -import cv2 -import numpy as np -import rclpy -from cv_bridge import CvBridge -from message_filters import ApproximateTimeSynchronizer, Subscriber -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from scipy.spatial.transform import Rotation -from sensor_msgs.msg import CameraInfo, Image -from tf2_ros import Buffer, TransformListener - -from parking_enforcement.config import ( - DEPTH_INFO_TOPIC, - DEPTH_TOPIC, - DISPARITY_VIS_TOPIC, - DOWNSCALE, - LEFT_CAMERA_INFO_TOPIC, - LEFT_CROP_EXTRA_PADDING_PIXELS, - LEFT_CROP_MIN_OVERLAP_RATIO, - LEFT_CROP_OVERRIDE_PIXELS, - LEFT_IMAGE_TOPIC, - MAX_DEPTH_METERS, - MIN_DEPTH_METERS, - MIN_PUBLISHED_WIDTH_PIXELS, - NUM_DISPARITIES, - RIGHT_CAMERA_INFO_TOPIC, - RIGHT_IMAGE_TOPIC, - SKIP_FRAMES, - STEREO_FRAME_ID, - VERTICAL_OFFSET_PIXELS, -) - - -class StereoDepthNode(Node): - """Produce cropped stereo depth, disparity visualization, and camera info.""" - - def __init__(self): - super().__init__('rob498_drone_8_stereo_depth') - self.bridge = CvBridge() - - # Intrinsics and rectification data are filled once the camera drivers and - # TF tree are online. - self.K1 = None - self.D1 = None - self.K2 = None - self.D2 = None - self.image_size = None - - self.left_xmap = None - self.left_ymap = None - self.right_xmap = None - self.right_ymap = None - self.depth_camera_info = None - - self.scale = DOWNSCALE - self.vertical_offset = VERTICAL_OFFSET_PIXELS - self.crop_start_col = 0 - self.output_width = 0 - self.output_height = 0 - self.baseline = None - self.fx_scaled = None - self.fy_scaled = None - self.cx_scaled = None - self.cy_scaled = None - self.min_disp = None - self.max_disp = None - self.maps_computed = False - - self.stereo = cv2.StereoSGBM_create( - minDisparity=0, - numDisparities=NUM_DISPARITIES, - blockSize=5, - P1=8 * 3 * 5 * 5, - P2=32 * 3 * 5 * 5, - disp12MaxDiff=1, - uniquenessRatio=5, - speckleWindowSize=25, - speckleRange=2, - preFilterCap=63, - mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY, - ) - self.clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8)) - - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - - self.create_subscription(CameraInfo, LEFT_CAMERA_INFO_TOPIC, self._cam1_intrinsics_cb, qos_profile_sensor_data) - self.create_subscription(CameraInfo, RIGHT_CAMERA_INFO_TOPIC, self._cam2_intrinsics_cb, qos_profile_sensor_data) - - left_sub = Subscriber(self, Image, LEFT_IMAGE_TOPIC, qos_profile=qos_profile_sensor_data) - right_sub = Subscriber(self, Image, RIGHT_IMAGE_TOPIC, qos_profile=qos_profile_sensor_data) - self.sync = ApproximateTimeSynchronizer([left_sub, right_sub], queue_size=10, slop=0.05) - self.sync.registerCallback(self._sync_cb) - - self.depth_pub = self.create_publisher(Image, DEPTH_TOPIC, qos_profile_sensor_data) - self.disp_vis_pub = self.create_publisher(Image, DISPARITY_VIS_TOPIC, 10) - self.depth_info_pub = self.create_publisher(CameraInfo, DEPTH_INFO_TOPIC, 10) - - self.setup_timer = self.create_timer(0.5, self._try_compute_maps) - self.frame_count = 0 - self.process_every_n = SKIP_FRAMES - - self.get_logger().info('Stereo depth node waiting for camera info and TF...') - - def _cam1_intrinsics_cb(self, msg: CameraInfo): - """Cache the left fisheye intrinsics the first time they arrive.""" - - if self.K1 is not None: - return - self.K1 = np.array(msg.k, dtype=np.float64).reshape(3, 3) - self.D1 = np.array(msg.d[:4], dtype=np.float64) - self.image_size = (msg.width, msg.height) - self.get_logger().info('Got left fisheye camera info') - - def _cam2_intrinsics_cb(self, msg: CameraInfo): - """Cache the right fisheye intrinsics the first time they arrive.""" - - if self.K2 is not None: - return - self.K2 = np.array(msg.k, dtype=np.float64).reshape(3, 3) - self.D2 = np.array(msg.d[:4], dtype=np.float64) - self.get_logger().info('Got right fisheye camera info') - - def _try_compute_maps(self): - """Build rectification maps and the cropped output camera model once TF is ready.""" - - if self.maps_computed or self.K1 is None or self.K2 is None or self.image_size is None: - return - - try: - transform = self.tf_buffer.lookup_transform( - STEREO_FRAME_ID, - 'camera_fisheye2_optical_frame', - rclpy.time.Time(), - ) - except Exception as exc: - self.get_logger().warn(f'TF not ready yet: {exc}') - return - - translation = transform.transform.translation - rotation = transform.transform.rotation - T = np.array([translation.x, translation.y, translation.z], dtype=np.float64) - R = Rotation.from_quat([rotation.x, rotation.y, rotation.z, rotation.w]).as_matrix() - - self.baseline = float(abs(T[0])) - if self.baseline < 1e-6: - self.get_logger().error('Baseline is zero. Check stereo TF before flying.') - return - - half_rotvec = Rotation.from_matrix(R).as_rotvec() / 2.0 - R1 = Rotation.from_rotvec(half_rotvec).as_matrix() - R2 = Rotation.from_rotvec(-half_rotvec).as_matrix() - - K1_scaled = self.K1.copy() - K2_scaled = self.K2.copy() - for K in (K1_scaled, K2_scaled): - K[0, 0] *= self.scale - K[1, 1] *= self.scale - K[0, 2] *= self.scale - K[1, 2] *= self.scale - - scaled_size = (int(self.image_size[0] * self.scale), int(self.image_size[1] * self.scale)) - self.left_xmap, self.left_ymap = cv2.fisheye.initUndistortRectifyMap( - self.K1, self.D1, R1, K1_scaled, scaled_size, cv2.CV_32FC1 - ) - self.right_xmap, self.right_ymap = cv2.fisheye.initUndistortRectifyMap( - self.K2, self.D2, R2, K2_scaled, scaled_size, cv2.CV_32FC1 - ) - - self.crop_start_col = self._estimate_left_crop(scaled_size) - self.output_width = scaled_size[0] - self.crop_start_col - self.output_height = scaled_size[1] - if self.output_width < MIN_PUBLISHED_WIDTH_PIXELS: - self.get_logger().error('Stereo crop removed too much width. Reduce the crop override.') - return - - self.fx_scaled = float(K1_scaled[0, 0]) - self.fy_scaled = float(K1_scaled[1, 1]) - self.cx_scaled = float(K1_scaled[0, 2]) - float(self.crop_start_col) - self.cy_scaled = float(K1_scaled[1, 2]) - self.min_disp = (self.fx_scaled * self.baseline) / MAX_DEPTH_METERS - self.max_disp = (self.fx_scaled * self.baseline) / MIN_DEPTH_METERS - self.warp_M = np.float32([[1, 0, 0], [0, 1, self.vertical_offset]]) - - self.depth_camera_info = CameraInfo() - self.depth_camera_info.header.frame_id = STEREO_FRAME_ID - self.depth_camera_info.width = self.output_width - self.depth_camera_info.height = self.output_height - self.depth_camera_info.distortion_model = 'plumb_bob' - self.depth_camera_info.d = [0.0] * 5 - self.depth_camera_info.k = [ - self.fx_scaled, 0.0, self.cx_scaled, - 0.0, self.fy_scaled, self.cy_scaled, - 0.0, 0.0, 1.0, - ] - self.depth_camera_info.r = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - ] - self.depth_camera_info.p = [ - self.fx_scaled, 0.0, self.cx_scaled, 0.0, - 0.0, self.fy_scaled, self.cy_scaled, 0.0, - 0.0, 0.0, 1.0, 0.0, - ] - self.depth_camera_info.roi.x_offset = self.crop_start_col - self.depth_camera_info.roi.y_offset = 0 - self.depth_camera_info.roi.width = self.output_width - self.depth_camera_info.roi.height = self.output_height - self.depth_camera_info.roi.do_rectify = True - - self.maps_computed = True - self.setup_timer.cancel() - self.get_logger().info( - f'Stereo depth ready. Baseline={self.baseline:.4f} m, ' - f'crop_left={self.crop_start_col} px, output={self.output_width}x{self.output_height}' - ) - - def _estimate_left_crop(self, scaled_size: tuple[int, int]) -> int: - """Estimate the invalid left strip created by fisheye rectification.""" - - width, _ = scaled_size - if LEFT_CROP_OVERRIDE_PIXELS is not None: - return max(0, min(int(LEFT_CROP_OVERRIDE_PIXELS), width - MIN_PUBLISHED_WIDTH_PIXELS)) - - valid_left = ( - (self.left_xmap >= 0.0) & (self.left_xmap < self.image_size[0] - 1) & - (self.left_ymap >= 0.0) & (self.left_ymap < self.image_size[1] - 1) - ) - valid_right = ( - (self.right_xmap >= 0.0) & (self.right_xmap < self.image_size[0] - 1) & - (self.right_ymap >= 0.0) & (self.right_ymap < self.image_size[1] - 1) - ) - overlap_ratio = np.mean(valid_left & valid_right, axis=0) - good_cols = np.where(overlap_ratio >= LEFT_CROP_MIN_OVERLAP_RATIO)[0] - if good_cols.size == 0: - return 0 - - crop = int(good_cols[0]) - if crop > 0: - crop += LEFT_CROP_EXTRA_PADDING_PIXELS - return max(0, min(crop, width - MIN_PUBLISHED_WIDTH_PIXELS)) - - def _sync_cb(self, left_msg: Image, right_msg: Image): - """Rectify, crop, compute disparity/depth, and publish the cropped outputs.""" - - if not self.maps_computed: - return - - self.frame_count += 1 - if self.frame_count % self.process_every_n != 0: - return - - left = self.bridge.imgmsg_to_cv2(left_msg, desired_encoding='mono8') - right = self.bridge.imgmsg_to_cv2(right_msg, desired_encoding='mono8') - - left_rect = cv2.remap(left, self.left_xmap, self.left_ymap, cv2.INTER_LINEAR) - right_rect = cv2.remap(right, self.right_xmap, self.right_ymap, cv2.INTER_LINEAR) - right_rect = cv2.warpAffine(right_rect, self.warp_M, (right_rect.shape[1], right_rect.shape[0])) - - left_rect = self.clahe.apply(left_rect) - right_rect = self.clahe.apply(right_rect) - - if self.crop_start_col > 0: - left_rect = left_rect[:, self.crop_start_col:] - right_rect = right_rect[:, self.crop_start_col:] - - disparity = self.stereo.compute(left_rect, right_rect).astype(np.float32) / 16.0 - valid_disp = (disparity > self.min_disp) & (disparity < self.max_disp) - disparity = np.where(valid_disp, disparity, 0.0) - - depth = np.where(disparity > 0.0, (self.fx_scaled * self.baseline) / disparity, 0.0).astype(np.float32) - depth = np.clip(depth, 0.0, MAX_DEPTH_METERS) - - depth_msg = self.bridge.cv2_to_imgmsg(depth, encoding='32FC1') - depth_msg.header = left_msg.header - depth_msg.header.frame_id = STEREO_FRAME_ID - self.depth_pub.publish(depth_msg) - - info_msg = CameraInfo() - info_msg.header = depth_msg.header - info_msg.width = self.depth_camera_info.width - info_msg.height = self.depth_camera_info.height - info_msg.distortion_model = self.depth_camera_info.distortion_model - info_msg.d = list(self.depth_camera_info.d) - info_msg.k = list(self.depth_camera_info.k) - info_msg.r = list(self.depth_camera_info.r) - info_msg.p = list(self.depth_camera_info.p) - info_msg.roi = self.depth_camera_info.roi - self.depth_info_pub.publish(info_msg) - - if self.disp_vis_pub.get_subscription_count() > 0: - disp_vis = np.clip(disparity, 0.0, NUM_DISPARITIES) - disp_vis = (disp_vis / NUM_DISPARITIES * 255.0).astype(np.uint8) - disp_vis = cv2.applyColorMap(disp_vis, cv2.COLORMAP_JET) - disp_msg = self.bridge.cv2_to_imgmsg(disp_vis, encoding='bgr8') - disp_msg.header = depth_msg.header - self.disp_vis_pub.publish(disp_msg) - - -def main(args=None): - """Spin the stereo depth node until shutdown.""" - - rclpy.init(args=args) - node = StereoDepthNode() - try: - rclpy.spin(node) - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/resource/parking_enforcement b/ros2_ws/src/parking_enforcement_package/parking_enforcement/resource/parking_enforcement deleted file mode 100644 index e69de29..0000000 diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/setup.cfg b/ros2_ws/src/parking_enforcement_package/parking_enforcement/setup.cfg deleted file mode 100644 index 2e732f7..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/parking_enforcement -[install] -install_scripts=$base/lib/parking_enforcement diff --git a/ros2_ws/src/parking_enforcement_package/parking_enforcement/setup.py b/ros2_ws/src/parking_enforcement_package/parking_enforcement/setup.py deleted file mode 100644 index 250e2f7..0000000 --- a/ros2_ws/src/parking_enforcement_package/parking_enforcement/setup.py +++ /dev/null @@ -1,38 +0,0 @@ -from setuptools import setup - -package_name = 'parking_enforcement' - -setup( - name=package_name, - version='0.1.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), - (f'share/{package_name}', ['package.xml']), - ( - f'share/{package_name}/launch', - [ - 'launch/parking_demo.launch.py', - 'launch/parking_first_pass.launch.py', - 'launch/parking_revisit.launch.py', - ], - ), - (f'share/{package_name}', ['README.md']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='OpenAI', - maintainer_email='support@example.com', - description='ROS2 nodes for ROB498 parking-enforcement demo', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'parking_mission_node = parking_enforcement.parking_mission_node:main', - 'parking_revisit_node = parking_enforcement.parking_revisit_node:main', - 'stereo_depth_node = parking_enforcement.stereo_depth_node:main', - 'local_costmap_node = parking_enforcement.local_costmap_node:main', - 'plate_reader_node = parking_enforcement.plate_reader_node:main', - ], - }, -) diff --git a/ros2_ws/src/perception/perception/depth_node.py b/ros2_ws/src/perception/perception/depth_node.py index 6979d74..6cb6479 100644 --- a/ros2_ws/src/perception/perception/depth_node.py +++ b/ros2_ws/src/perception/perception/depth_node.py @@ -3,6 +3,7 @@ from rclpy.qos import qos_profile_sensor_data from sensor_msgs.msg import Image, CameraInfo from stereo_msgs.msg import DisparityImage +from std_msgs.msg import Bool from cv_bridge import CvBridge from message_filters import Subscriber, ApproximateTimeSynchronizer from tf2_ros import Buffer, TransformListener @@ -16,6 +17,7 @@ NUM_DISPARITIES = 64 # Manual crop for the invalid left disparity strip. Tune this in pixels. LEFT_CROP_PIXELS = 62 +BLOCKED_LOG_PERIOD_SEC = 1.0 class DepthNode(Node): def __init__(self): @@ -99,9 +101,17 @@ def __init__(self): self.frame_count = 0 self.process_every_n = SKIP_FRAMES self.crop_start_col = 0 + self.enabled = False + self._blocked_reason = None + self._last_blocked_log_time = None + + self.create_subscription(Bool, '/perception/depth_enabled', self._enable_cb, 10) self.get_logger().info('Waiting for camera info and TF...') + def _enable_cb(self, msg: Bool): + self.enabled = msg.data + def _cam1_intrinsics_cb(self, msg): if self.K1 is not None: return @@ -191,9 +201,20 @@ def _try_compute_maps(self): f'Output: {scaled_size[0] - self.crop_start_col}x{scaled_size[1]}') def _sync_cb(self, left_msg, right_msg): + if not self.enabled: + self._log_blocked_state( + 'disabled', + 'Depth idle: /perception/depth_enabled = false; early-returning from _sync_cb().', + ) + return + if not self.maps_computed: return + self._clear_blocked_state( + 'Depth resumed: /perception/depth_enabled = true; _sync_cb() active again.' + ) + self.frame_count += 1 if self.frame_count % self.process_every_n != 0: return @@ -259,6 +280,31 @@ def _sync_cb(self, left_msg, right_msg): disp_vis_msg.header = left_msg.header self.disp_vis_pub.publish(disp_vis_msg) + 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 main(args=None): rclpy.init(args=args) diff --git a/ros2_ws/src/waypoint_publisher/package.xml b/ros2_ws/src/waypoint_publisher/package.xml index 528e785..4218bf2 100644 --- a/ros2_ws/src/waypoint_publisher/package.xml +++ b/ros2_ws/src/waypoint_publisher/package.xml @@ -7,6 +7,10 @@ jetson TODO: License declaration + geometry_msgs + rclpy + std_msgs + ament_copyright ament_flake8 ament_pep257 diff --git a/ros2_ws/src/waypoint_publisher/waypoint_publisher/waypoint_publisher.py b/ros2_ws/src/waypoint_publisher/waypoint_publisher/waypoint_publisher.py index 40badc1..d209e34 100644 --- a/ros2_ws/src/waypoint_publisher/waypoint_publisher/waypoint_publisher.py +++ b/ros2_ws/src/waypoint_publisher/waypoint_publisher/waypoint_publisher.py @@ -1,43 +1,47 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import PoseArray, Pose +from std_msgs.msg import Int32MultiArray +WAYPOINT_TOPIC = 'rob498_drone_8/comm/waypoints' +WAYPOINT_SCAN_FLAGS_TOPIC = 'rob498_drone_8/comm/waypoint_scan_flags' # Edit these waypoints for your space # WAYPOINTS = [ -# (0.0, 0.0, 1.0), -# (1.0, 0.0, 1.0), -# (1.0, -1.0, 1.0), +# {'x': 0.0, 'y': 0.0, 'z': 1.0, 'scan_plate': False}, +# {'x': 1.0, 'y': 0.0, 'z': 1.0, 'scan_plate': True}, +# {'x': 1.0, 'y': -1.0, 'z': 1.0, 'scan_plate': True}, # ] # Bigger square pattern for testing # WAYPOINTS = [ -# (0.0, 0.0, 1.0), -# (-1.0, 1.0, 1.0), -# (-1.0, -1.0, 1.0), -# (1.0, -1.0, 1.0), -# (1.0, 1.0, 1.0), -# (1.0, 1.0, 1.0), +# {'x': 0.0, 'y': 0.0, 'z': 1.0, 'scan_plate': False}, +# {'x': -1.0, 'y': 1.0, 'z': 1.0, 'scan_plate': False}, +# {'x': -1.0, 'y': -1.0, 'z': 1.0, 'scan_plate': True}, +# {'x': 1.0, 'y': -1.0, 'z': 1.0, 'scan_plate': True}, +# {'x': 1.0, 'y': 1.0, 'z': 1.0, 'scan_plate': True}, +# {'x': 1.0, 'y': 1.0, 'z': 1.0, 'scan_plate': False}, # ] WAYPOINTS = [ - (0.2, 0.0, 2.0), - (3.0, 0.0, 2.0), - (3.0, -0.5, 2.0), - (3.0, -1.0, 2.0), + {'x': 0.2, 'y': 0.0, 'z': 2.0, 'scan_plate': False}, + {'x': 3.0, 'y': 0.0, 'z': 2.0, 'scan_plate': False}, + {'x': 3.0, 'y': -0.5, 'z': 2.0, 'scan_plate': False}, + {'x': 3.0, 'y': -1.0, 'z': 2.0, 'scan_plate': False}, + {'x': 0.0, 'y': 0.0, 'z': 2.0, 'scan_plate': False} ] # WAYPOINTS = [ -# (0.2, 0.0, 0.5), -# (1.5, 0.0, 0.5) +# {'x': 0.2, 'y': 0.0, 'z': 0.5, 'scan_plate': False}, +# {'x': 1.5, 'y': 0.0, 'z': 0.5, 'scan_plate': True}, # ] class WaypointPublisher(Node): def __init__(self): super().__init__('waypoint_publisher') - self.pub = self.create_publisher( - PoseArray, 'rob498_drone_8/comm/waypoints', 10) + self.waypoint_pub = self.create_publisher(PoseArray, WAYPOINT_TOPIC, 10) + self.scan_flags_pub = self.create_publisher(Int32MultiArray, WAYPOINT_SCAN_FLAGS_TOPIC, 10) # Publish once after 2 seconds to give comm node time to start self.timer = self.create_timer(2.0, self._publish) @@ -49,20 +53,32 @@ def _publish(self): if self.published: return - msg = PoseArray() - for x, y, z in WAYPOINTS: + waypoint_msg = PoseArray() + scan_flags_msg = Int32MultiArray() + scan_flags = [] + + for waypoint in WAYPOINTS: p = Pose() - p.position.x = x - p.position.y = y - p.position.z = z - msg.poses.append(p) + p.position.x = waypoint['x'] + p.position.y = waypoint['y'] + p.position.z = waypoint['z'] + waypoint_msg.poses.append(p) + scan_flags.append(1 if waypoint['scan_plate'] else 0) + + scan_flags_msg.data = scan_flags - self.pub.publish(msg) + self.waypoint_pub.publish(waypoint_msg) + self.scan_flags_pub.publish(scan_flags_msg) self.published = True - self.get_logger().info(f'Published {len(WAYPOINTS)} waypoints:') - for i, (x, y, z) in enumerate(WAYPOINTS): - self.get_logger().info(f' WP {i + 1}: ({x}, {y}, {z})') + self.get_logger().info( + f'Published {len(WAYPOINTS)} waypoints and {len(scan_flags)} scan flags:' + ) + for i, waypoint in enumerate(WAYPOINTS): + waypoint_type = 'scan' if waypoint['scan_plate'] else 'nav-only' + self.get_logger().info( + f" WP {i + 1}: ({waypoint['x']}, {waypoint['y']}, {waypoint['z']}) [{waypoint_type}]" + ) self.timer.cancel() @@ -76,4 +92,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/setup.sh b/setup.sh index 7a1cf31..13d60e2 100755 --- a/setup.sh +++ b/setup.sh @@ -46,6 +46,9 @@ tmux split-window -h -t "$SESSION" tmux select-pane -t 8 tmux split-window -h -t "$SESSION" +# 6b. Split once more for plate reader pane. +tmux split-window -h -t 11 + # 7. Populate panes with text (but don't press Enter) tmux send-keys -t 0 "ros2 launch px4_autonomy_modules mavros.launch.py gcs_url:=udp://@$GCS_IP" tmux send-keys -t 1 "ros2 launch realsense2_camera rs_launch.py" @@ -58,9 +61,9 @@ tmux send-keys -t 7 "ros2 run waypoint_publisher waypoint_publisher" tmux send-keys -t 8 "ros2 service call /rob498_drone_8/comm/launch std_srvs/srv/Trigger {}" tmux send-keys -t 9 "ros2 service call /rob498_drone_8/comm/test std_srvs/srv/Trigger {}" tmux send-keys -t 10 "ros2 service call /rob498_drone_8/comm/abort std_srvs/srv/Trigger {}" -tmux send-keys -t 11 "ros2 bag record /stereo/disp_vis occupancy_node/viz -o wed_" +tmux send-keys -t 11 "ros2 run comm plate_reader_node" +tmux send-keys -t 12 "ros2 bag record -a -o fri_.bag" #tmux send-keys -t 10 "./video.sh \"$GCS_IP\"" -# pane 11 intentionally left blank # 8. Attach tmux attach-session -t "$SESSION"