#!/usr/bin/env python3 """Align to a factory sign, then optionally navigate to a stand-off point on its wall.""" from __future__ import annotations import json import math import time import actionlib import rospy import tf2_ros from actionlib_msgs.msg import GoalStatus, GoalStatusArray from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from nav_msgs.msg import OccupancyGrid, Odometry from std_msgs.msg import Bool, Float32, String from std_srvs.srv import SetBool class FactoryAlignmentDemo: _RUNNING_STATES = { GoalStatus.PENDING, GoalStatus.ACTIVE, GoalStatus.PREEMPTING, GoalStatus.RECALLING, } _FAILED_STATES = { GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.LOST, } def __init__(self): rospy.init_node("factory_alignment_demo") self._enabled = bool(rospy.get_param("~enabled", False)) # In competition mode only an explicit task-2 order may start recognition. self._require_order = bool(rospy.get_param("~require_order", False)) self._current_order = None self._completed_order_id = None self._pending_start_order_id = None self._armed_order_id = None self._angular_sign = float(rospy.get_param("~angular_sign", -1.0)) self._kp = float(rospy.get_param("~kp", 0.25)) self._min_speed = float(rospy.get_param("~min_angular_speed", 0.10)) self._max_speed = float(rospy.get_param("~max_angular_speed", 0.12)) self._tolerance = float(rospy.get_param("~center_tolerance", 0.18)) self._center_frames_required = int(rospy.get_param("~center_confirm_frames", 3)) self._max_angular_accel = float(rospy.get_param("~max_angular_acceleration", 0.15)) self._detection_timeout = float(rospy.get_param("~detection_timeout_seconds", 0.35)) self._alignment_timeout = float(rospy.get_param("~alignment_timeout_seconds", 30.0)) self._scan_steps = int(rospy.get_param("~scan_steps", 6)) self._scan_direction = 1.0 if float(rospy.get_param("~scan_direction", 1.0)) >= 0.0 else -1.0 self._scan_speed = abs(float(rospy.get_param("~scan_angular_speed", 0.20))) self._scan_yaw_tolerance = float(rospy.get_param("~scan_yaw_tolerance", 0.03)) self._scan_detection_window = float(rospy.get_param("~scan_detection_window_seconds", 1.0)) self._odom_timeout = float(rospy.get_param("~odom_timeout_seconds", 0.50)) rate = float(rospy.get_param("~control_rate", 20.0)) # This is explicitly opt-in because it sends a real move_base goal after alignment. self._wall_approach_enabled = bool(rospy.get_param("~wall_approach_enabled", False)) self._wall_standoff = float(rospy.get_param("~wall_standoff_distance", 0.30)) self._wall_ray_max_distance = float(rospy.get_param("~wall_ray_max_distance", 5.0)) self._wall_occupied_threshold = int(rospy.get_param("~wall_occupied_threshold", 65)) self._wall_fit_radius = float(rospy.get_param("~wall_fit_radius", 0.60)) self._wall_line_inlier_distance = float(rospy.get_param("~wall_line_inlier_distance", 0.05)) self._wall_line_min_length = float(rospy.get_param("~wall_line_min_length", 0.25)) self._wall_line_min_support = int(rospy.get_param("~wall_line_min_support", 8)) self._wall_line_min_facing_alignment = float( rospy.get_param("~wall_line_min_facing_alignment", 0.50) ) self._wall_line_max_points = int(rospy.get_param("~wall_line_max_points", 180)) self._tf_map_frame = rospy.get_param("~tf_map_frame", "map") self._tf_base_frame = rospy.get_param("~tf_base_frame", "base_link") self._tf_lookup_timeout = float(rospy.get_param("~tf_lookup_timeout_seconds", 0.20)) self._amcl_fallback_timeout = float( rospy.get_param("~amcl_pose_fallback_timeout_seconds", 5.0) ) self._wall_goal_server_timeout = float(rospy.get_param("~wall_goal_server_timeout_seconds", 2.0)) self._ocr_confirmation_enabled = bool( rospy.get_param("~ocr_confirmation_enabled", True) ) self._ocr_timeout = float(rospy.get_param("~ocr_timeout_seconds", 15.0)) self._ocr_service_name = rospy.get_param( "~ocr_enable_service", "/sign_recognition/set_enabled" ) # Terminal white-box entry after OCR confirmation. The box touches the # wall and extends 0.50 m outward; base_link is at the vehicle centre. self._entry_goal_enabled = bool(rospy.get_param("~entry_goal_enabled", True)) self._entry_standoff = float(rospy.get_param("~entry_standoff_distance", 0.28)) self._entry_white_box_depth = float(rospy.get_param("~entry_white_box_depth", 0.50)) self._entry_vehicle_length = float(rospy.get_param("~entry_vehicle_length", 0.335)) if self._scan_steps <= 0: raise ValueError("scan_steps must be positive") if self._wall_standoff <= 0.0 or self._wall_ray_max_distance <= 0.0: raise ValueError("wall approach distances must be positive") half_vehicle_length = self._entry_vehicle_length / 2.0 if (self._entry_standoff <= half_vehicle_length or self._entry_standoff + half_vehicle_length > self._entry_white_box_depth): raise ValueError("entry goal does not keep the vehicle inside the white box") self._error = 0.0 self._visible = False self._latest_odom_yaw = None self._latest_odom_monotonic = 0.0 self._latest_amcl_pose = None self._latest_amcl_monotonic = 0.0 self._static_map = None self._scan_state = "IDLE" self._scan_completed_steps = 0 self._scan_target_yaw = None self._scan_detection_deadline = None self._last_detection_monotonic = 0.0 self._detection_sequence = 0 self._processed_sequence = 0 self._center_frame_count = 0 self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._last_control_monotonic = time.monotonic() self._navigation_seen_active = False self._navigation_succeeded = False self._alignment_started_monotonic = None self._has_control = False self._last_status = None self._aligned = False self._wall_goal_active = False self._wall_goal_finished = False self._entry_goal_active = False self._entry_goal_finished = False self._final_alignment_active = False self._ocr_active = False self._ocr_started_monotonic = None self._factory_type = None self._ocr_enabled = False self._recognition_enabled = None self._recognition_service_name = rospy.get_param( "~recognition_enable_service", "/factory_sign_recognition/set_enabled" ) self._recognition_enable = rospy.ServiceProxy(self._recognition_service_name, SetBool) self._ocr_enable = rospy.ServiceProxy(self._ocr_service_name, SetBool) self._wall_goal_client = actionlib.SimpleActionClient("move_base", MoveBaseAction) self._tf_buffer = tf2_ros.Buffer() self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/factory_alignment_demo/cmd_vel") self._cmd_pub = rospy.Publisher(cmd_vel_topic, Twist, queue_size=1) self._status_pub = rospy.Publisher("/factory_alignment_demo/status", String, queue_size=1, latch=True) self._wall_goal_pub = rospy.Publisher( "/factory_alignment_demo/wall_goal", PoseStamped, queue_size=1, latch=True ) self._entry_goal_pub = rospy.Publisher( "/factory_alignment_demo/entry_goal", PoseStamped, queue_size=1, latch=True ) self._factory_type_pub = rospy.Publisher( "/factory_alignment_demo/factory_type", String, queue_size=1, latch=True ) rospy.Subscriber("/competition_task2/current_order", String, self._order_callback, queue_size=1) rospy.Subscriber("/competition_task2/command", String, self._task_command_callback, queue_size=10) rospy.Subscriber("/factory_sign/target_center_error", Float32, self._error_callback, queue_size=1) rospy.Subscriber("/factory_sign/target_visible", Bool, self._visible_callback, queue_size=1) rospy.Subscriber("/sign_recognition", String, self._ocr_result_callback, queue_size=1) rospy.Subscriber("/move_base/status", GoalStatusArray, self._navigation_callback, queue_size=5) rospy.Subscriber("/odom", Odometry, self._odom_callback, queue_size=10) rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self._amcl_callback, queue_size=10) rospy.Subscriber("/map", OccupancyGrid, self._map_callback, queue_size=1) rospy.Timer(rospy.Duration(1.0 / rate), self._control_callback) rospy.on_shutdown(self._shutdown) self._set_recognition_enabled(False, required=False) self._publish_status("DISABLED" if not self._enabled else ("WAITING_FOR_ORDER" if self._require_order else "WAITING_FOR_NAV_GOAL")) rospy.loginfo( "factory alignment demo ready: enabled=%s wall_approach=%s output=%s", self._enabled, self._wall_approach_enabled, cmd_vel_topic, ) def _publish_status(self, status): if status != self._last_status: self._status_pub.publish(String(data=status)) self._last_status = status @staticmethod def _normalise_factory_category(value): return {"食品": "食品", "日用品": "日用品", "电子": "电子", "电子产品": "电子"}.get(str(value).strip()) def _order_callback(self, message): try: order = json.loads(message.data) order_id = str(order["order_id"]).strip() expected = self._normalise_factory_category(order.get("category", order.get("factory_category", ""))) product = str(order["product"]).strip() warehouse = str(order["warehouse"]).strip() if not order_id or expected is None or not product or not warehouse: raise ValueError("missing required order field") except (ValueError, TypeError, KeyError, json.JSONDecodeError) as error: self._current_order = None self._publish_status("ORDER_INVALID %s" % error) return # A fresh order is the only permitted way to clear the terminal parking lock. self._current_order = {"order_id": order_id, "category": expected, "product": product, "warehouse": warehouse} self._armed_order_id = order_id if self._pending_start_order_id == order_id else None if self._armed_order_id is not None: self._pending_start_order_id = None self._completed_order_id = None self._navigation_seen_active = False self._navigation_succeeded = False self._wall_goal_finished = False self._entry_goal_finished = False self._publish_status("ORDER_READY order_id=%s expected=%s" % (order_id, expected)) def _task_command_callback(self, message): command = message.data.strip() if command == "STOP": self._pending_start_order_id = None self._armed_order_id = None self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._ocr_active = False self._ocr_started_monotonic = None self._publish_stop() self._has_control = True if self._wall_goal_active or self._entry_goal_active: self._wall_goal_client.cancel_goal() self._set_recognition_enabled(False, required=False) self._set_ocr_enabled(False, required=False) self._publish_status("TASK2_STOPPED") return if not command.startswith("START_ORDER order_id="): return order_id = command.split("=", 1)[1].strip() if not order_id: return self._pending_start_order_id = order_id if self._current_order is not None and self._current_order.get("order_id") == order_id: self._armed_order_id = order_id self._pending_start_order_id = None self._publish_status("ORDER_ARMED order_id=%s" % order_id) def _error_callback(self, message): self._error = max(-1.0, min(1.0, message.data)) self._last_detection_monotonic = time.monotonic() self._detection_sequence += 1 def _visible_callback(self, message): self._visible = message.data def _odom_callback(self, message): orientation = message.pose.pose.orientation self._latest_odom_yaw = self._yaw_from_quaternion(orientation) self._latest_odom_monotonic = time.monotonic() def _amcl_callback(self, message): orientation = message.pose.pose.orientation self._latest_amcl_pose = ( message.pose.pose.position.x, message.pose.pose.position.y, self._yaw_from_quaternion(orientation), ) self._latest_amcl_monotonic = time.monotonic() def _map_callback(self, message): if message.header.frame_id.lstrip("/") != "map": rospy.logwarn_throttle(5.0, "factory alignment ignored map frame %s", message.header.frame_id) return self._static_map = message @staticmethod def _yaw_from_quaternion(orientation): return math.atan2( 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y), 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z), ) @staticmethod def _wrap_to_pi(angle): return math.atan2(math.sin(angle), math.cos(angle)) def _set_recognition_enabled(self, enabled, required): if self._recognition_enabled is enabled: return True try: rospy.wait_for_service(self._recognition_service_name, timeout=2.0) response = self._recognition_enable(enabled) except (rospy.ROSException, rospy.ServiceException) as error: if required: self._publish_status("RECOGNITION_SERVICE_UNAVAILABLE") else: rospy.logwarn("factory recognition service unavailable: %s", error) return False if not response.success: rospy.logwarn("factory recognition switch failed: %s", response.message) return False self._recognition_enabled = enabled return True def _set_ocr_enabled(self, enabled, required): if not self._ocr_confirmation_enabled: return not enabled if self._ocr_enabled is enabled: return True try: rospy.wait_for_service(self._ocr_service_name, timeout=2.0) response = self._ocr_enable(enabled) except (rospy.ROSException, rospy.ServiceException) as error: if required: self._publish_status("OCR_SERVICE_UNAVAILABLE") else: rospy.logwarn("sign OCR service unavailable: %s", error) return False if not response.success: rospy.logwarn("sign OCR switch failed: %s", response.message) if required: self._publish_status("OCR_SERVICE_UNAVAILABLE") return False self._ocr_enabled = enabled return True def _start_ocr_confirmation(self): self._ocr_active = True self._ocr_started_monotonic = time.monotonic() self._factory_type = None if not self._set_ocr_enabled(True, required=True): self._ocr_active = False self._ocr_started_monotonic = None return False self._publish_status("OCR_READING") return True def _ocr_result_callback(self, message): if not self._ocr_active: return result = message.data.strip() self._ocr_active = False self._ocr_started_monotonic = None self._set_ocr_enabled(False, required=False) self._set_recognition_enabled(False, required=False) factory_type = self._normalise_factory_category(result) if factory_type is not None: self._factory_type = factory_type self._factory_type_pub.publish(String(data=factory_type)) if self._require_order: expected = self._current_order["category"] if self._current_order else None if factory_type != expected: self._wall_goal_finished = True self._publish_status("FACTORY_MISMATCH order_id=%s detected=%s expected=%s" % ( self._current_order["order_id"] if self._current_order else "NONE", factory_type, expected or "NONE")) return self._publish_status("FACTORY_MATCHED order_id=%s type=%s" % ( self._current_order["order_id"], factory_type)) else: self._publish_status("FACTORY_CONFIRMED type=%s" % factory_type) if self._entry_goal_enabled: self._wall_goal_finished = False self._start_entry_approach() return self._wall_goal_finished = True else: self._factory_type = None self._wall_goal_finished = True self._publish_status("FACTORY_OCR_FAILED result=%s" % (result or "EMPTY")) def _navigation_callback(self, message): if self._require_order and self._current_order is None: return if (self._require_order and self._armed_order_id != self._current_order.get("order_id")): return if self._completed_order_id is not None: return if not message.status_list: return latest = max( message.status_list, key=lambda status: (status.goal_id.stamp.to_nsec(), status.goal_id.id), ) state = latest.status # move_base statuses generated by our own wall/entry goals must not reset this state machine. if self._wall_goal_active or self._entry_goal_active: return if (self._wall_goal_finished or self._entry_goal_finished) and state not in self._RUNNING_STATES: return if state in self._RUNNING_STATES: self._wall_goal_finished = False self._entry_goal_finished = False self._set_recognition_enabled(False, required=False) self._set_ocr_enabled(False, required=False) self._ocr_active = False self._ocr_started_monotonic = None self._entry_goal_active = False self._entry_goal_finished = False self._factory_type = None self._factory_type_pub.publish(String(data="")) self._navigation_seen_active = True self._navigation_succeeded = False self._aligned = False self._final_alignment_active = False self._alignment_started_monotonic = None if self._has_control: self._publish_stop() self._center_frame_count = 0 self._scan_state = "IDLE" self._scan_completed_steps = 0 self._scan_target_yaw = None self._scan_detection_deadline = None self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._last_control_monotonic = time.monotonic() self._processed_sequence = self._detection_sequence self._has_control = False return if self._navigation_seen_active and state == GoalStatus.SUCCEEDED: self._navigation_succeeded = True if self._alignment_started_monotonic is None: self._alignment_started_monotonic = time.monotonic() self._scan_state = "INITIAL_DETECT" self._scan_completed_steps = 0 self._scan_target_yaw = None self._scan_detection_deadline = self._alignment_started_monotonic + self._scan_detection_window self._set_recognition_enabled(True, required=True) self._publish_status("INITIAL_DETECTING") return if self._navigation_seen_active and state in self._FAILED_STATES: self._set_recognition_enabled(False, required=False) self._set_ocr_enabled(False, required=False) self._ocr_active = False self._navigation_succeeded = False self._aligned = False self._alignment_started_monotonic = None self._publish_status("NAVIGATION_NOT_SUCCEEDED") def _publish_stop(self): self._cmd_pub.publish(Twist()) def _shutdown(self): self._wall_goal_client.cancel_goal() if self._has_control: self._publish_stop() def _publish_smooth_command(self, desired_angular_z, now): elapsed = max(0.0, min(0.2, now - self._last_control_monotonic)) max_delta = self._max_angular_accel * elapsed delta = desired_angular_z - self._current_angular_z if abs(delta) <= max_delta: self._current_angular_z = desired_angular_z else: self._current_angular_z += math.copysign(max_delta, delta) self._last_control_monotonic = now command = Twist() command.angular.z = self._current_angular_z self._has_control = True self._cmd_pub.publish(command) return self._current_angular_z def _run_search_scan(self, now): if self._scan_state == "COMPLETE": self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._publish_stop() self._has_control = True self._publish_status("FACTORY_NOT_FOUND_AFTER_360_DEG_SCAN") return odom_fresh = ( self._latest_odom_yaw is not None and now - self._latest_odom_monotonic <= self._odom_timeout ) if self._scan_state == "TURN": if not odom_fresh: self._target_angular_z = 0.0 self._publish_smooth_command(0.0, now) self._publish_status("WAITING_FOR_ODOM") return if self._scan_target_yaw is None: self._scan_target_yaw = self._wrap_to_pi( self._latest_odom_yaw + self._scan_direction * 2.0 * math.pi / self._scan_steps ) yaw_error = self._wrap_to_pi(self._scan_target_yaw - self._latest_odom_yaw) if abs(yaw_error) <= self._scan_yaw_tolerance: self._target_angular_z = 0.0 self._scan_state = "SETTLE" command_z = self._publish_smooth_command(0.0, now) self._publish_status("SCAN_STEP_%d_SETTLING command_z=%+.3f" % ( self._scan_completed_steps + 1, command_z )) return self._target_angular_z = math.copysign(self._scan_speed, yaw_error) command_z = self._publish_smooth_command(self._target_angular_z, now) self._publish_status("SCANNING_STEP_%d/%d yaw_error=%+.3f command_z=%+.3f" % ( self._scan_completed_steps + 1, self._scan_steps, yaw_error, command_z )) return if self._scan_state == "SETTLE": command_z = self._publish_smooth_command(0.0, now) if abs(command_z) <= 0.005: self._scan_state = "DETECT" self._scan_detection_deadline = now + self._scan_detection_window self._set_recognition_enabled(True, required=True) self._publish_status("SCAN_STEP_%d_DETECTING" % (self._scan_completed_steps + 1)) else: self._publish_status("SCAN_STEP_%d_SETTLING command_z=%+.3f" % ( self._scan_completed_steps + 1, command_z )) return if self._scan_state in ("INITIAL_DETECT", "DETECT"): self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._publish_stop() self._has_control = True detected = self._visible and now - self._last_detection_monotonic <= self._detection_timeout if detected: self._scan_state = "ALIGN" self._alignment_started_monotonic = now self._center_frame_count = 0 self._processed_sequence = self._detection_sequence self._publish_status("FACTORY_FOUND_STARTING_ALIGNMENT") return if now < self._scan_detection_deadline: state = "INITIAL_DETECTING" if self._scan_state == "INITIAL_DETECT" else "SCAN_STEP_%d_DETECTING" % (self._scan_completed_steps + 1) self._publish_status(state) return self._set_recognition_enabled(False, required=False) if self._scan_state == "INITIAL_DETECT": self._scan_state = "TURN" self._scan_target_yaw = None else: self._scan_completed_steps += 1 if self._scan_completed_steps >= self._scan_steps: self._scan_state = "COMPLETE" else: self._scan_state = "TURN" self._scan_target_yaw = None @staticmethod def _map_origin_yaw(grid): return FactoryAlignmentDemo._yaw_from_quaternion(grid.info.origin.orientation) @staticmethod def _occupied(grid, row, column, threshold): if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width: return False return grid.data[row * grid.info.width + column] >= threshold @classmethod def _map_to_grid(cls, grid, x, y): resolution = grid.info.resolution if resolution <= 0.0: return None yaw = cls._map_origin_yaw(grid) dx = x - grid.info.origin.position.x dy = y - grid.info.origin.position.y column = int(math.floor((math.cos(yaw) * dx + math.sin(yaw) * dy) / resolution)) row = int(math.floor((-math.sin(yaw) * dx + math.cos(yaw) * dy) / resolution)) if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width: return None return row, column @classmethod def _grid_to_map(cls, grid, row, column): resolution = grid.info.resolution yaw = cls._map_origin_yaw(grid) local_x = (column + 0.5) * resolution local_y = (row + 0.5) * resolution origin = grid.info.origin.position return ( origin.x + math.cos(yaw) * local_x - math.sin(yaw) * local_y, origin.y + math.sin(yaw) * local_x + math.cos(yaw) * local_y, ) def _raycast_wall(self, grid, pose): x, y, heading = pose step = max(grid.info.resolution * 0.5, 0.01) previous_cell = None samples = int(math.ceil(self._wall_ray_max_distance / step)) for sample in range(1, samples + 1): distance = sample * step cell = self._map_to_grid(grid, x + distance * math.cos(heading), y + distance * math.sin(heading)) if cell is None: break if cell == previous_cell: continue previous_cell = cell row, column = cell if self._occupied(grid, row, column, self._wall_occupied_threshold): hit_x, hit_y = self._grid_to_map(grid, row, column) return hit_x, hit_y, row, column return None def _wall_normal_toward_robot(self, grid, hit_row, hit_column, robot_x, robot_y, heading): radius_cells = max(1, int(math.ceil(self._wall_fit_radius / grid.info.resolution))) queue = [(hit_row, hit_column)] visited = set() points = [] while queue: row, column = queue.pop() if (row, column) in visited: continue visited.add((row, column)) if not self._occupied(grid, row, column, self._wall_occupied_threshold): continue if math.hypot(row - hit_row, column - hit_column) > radius_cells: continue points.append(self._grid_to_map(grid, row, column)) for delta_row in (-1, 0, 1): for delta_column in (-1, 0, 1): if delta_row or delta_column: queue.append((row + delta_row, column + delta_column)) if len(points) < self._wall_line_min_support: return None # A corner joins two wall segments. Fit several local lines, then retain # the one whose outward normal faces the visually aligned vehicle. if len(points) > self._wall_line_max_points: stride = float(len(points)) / self._wall_line_max_points points = [points[int(index * stride)] for index in range(self._wall_line_max_points)] hit_x, hit_y = self._grid_to_map(grid, hit_row, hit_column) desired_normal_x = -math.cos(heading) desired_normal_y = -math.sin(heading) best = None for first_index, first in enumerate(points): for second in points[first_index + 1:]: dx = second[0] - first[0] dy = second[1] - first[1] length = math.hypot(dx, dy) if length < self._wall_line_min_length: continue # The selected line must describe the actually struck wall cell. hit_distance = abs(dy * (hit_x - first[0]) - dx * (hit_y - first[1])) / length if hit_distance > self._wall_line_inlier_distance: continue normal = math.atan2(dy, dx) + math.pi / 2.0 if math.cos(normal) * (robot_x - hit_x) + math.sin(normal) * (robot_y - hit_y) < 0.0: normal += math.pi facing = math.cos(normal) * desired_normal_x + math.sin(normal) * desired_normal_y if facing < self._wall_line_min_facing_alignment: continue support = 0 for point in points: distance = abs(dy * (point[0] - first[0]) - dx * (point[1] - first[1])) / length if distance <= self._wall_line_inlier_distance: support += 1 if support < self._wall_line_min_support: continue score = support * (0.5 + 0.5 * facing) if best is None or score > best[0]: best = (score, normal) if best is None: return None return self._wrap_to_pi(best[1]) def _map_pose(self): try: transform = self._tf_buffer.lookup_transform( self._tf_map_frame, self._tf_base_frame, rospy.Time(0), rospy.Duration(self._tf_lookup_timeout), ) translation = transform.transform.translation return ( translation.x, translation.y, self._yaw_from_quaternion(transform.transform.rotation), ), None except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException, tf2_ros.TimeoutException) as error: rospy.logwarn_throttle(5.0, "factory alignment TF pose unavailable: %s", error) if (self._latest_amcl_pose is not None and time.monotonic() - self._latest_amcl_monotonic <= self._amcl_fallback_timeout): return self._latest_amcl_pose, None return None, "MAP_POSE_UNAVAILABLE" def _compute_wall_goal(self, standoff_distance=None): standoff = self._wall_standoff if standoff_distance is None else standoff_distance if standoff <= 0.0: return None, "WALL_GOAL_INVALID_STANDOFF" if self._static_map is None: return None, "STATIC_MAP_UNAVAILABLE" pose, failure = self._map_pose() if failure is not None: return None, failure grid = self._static_map ray_hit = self._raycast_wall(grid, pose) if ray_hit is None: return None, "WALL_RAY_NO_HIT" hit_x, hit_y, hit_row, hit_column = ray_hit robot_x, robot_y, heading = pose normal = self._wall_normal_toward_robot( grid, hit_row, hit_column, robot_x, robot_y, heading ) if normal is None: return None, "WALL_NORMAL_UNAVAILABLE" goal_x = hit_x + standoff * math.cos(normal) goal_y = hit_y + standoff * math.sin(normal) goal_cell = self._map_to_grid(grid, goal_x, goal_y) if goal_cell is None: return None, "WALL_GOAL_OUTSIDE_MAP" if self._occupied(grid, goal_cell[0], goal_cell[1], self._wall_occupied_threshold): return None, "WALL_GOAL_OCCUPIED" return (goal_x, goal_y, self._wrap_to_pi(normal + math.pi), hit_x, hit_y), None def _start_wall_approach(self): computed, failure = self._compute_wall_goal() if failure is not None: self._wall_goal_finished = True self._set_recognition_enabled(False, required=False) self._publish_stop() self._has_control = True self._publish_status(failure) return False if not self._wall_goal_client.wait_for_server(rospy.Duration(self._wall_goal_server_timeout)): self._wall_goal_finished = True self._set_recognition_enabled(False, required=False) self._publish_stop() self._has_control = True self._publish_status("WALL_GOAL_MOVE_BASE_UNAVAILABLE") return False goal_x, goal_y, goal_yaw, hit_x, hit_y = computed goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = goal_x goal.target_pose.pose.position.y = goal_y goal.target_pose.pose.orientation = Quaternion( z=math.sin(goal_yaw / 2.0), w=math.cos(goal_yaw / 2.0) ) self._wall_goal_pub.publish(goal.target_pose) self._wall_goal_active = True self._has_control = False self._wall_goal_client.send_goal(goal, done_cb=self._wall_goal_done) self._publish_status( "WALL_GOAL_SENT x=%.3f y=%.3f yaw=%.3f wall_x=%.3f wall_y=%.3f" % ( goal_x, goal_y, goal_yaw, hit_x, hit_y ) ) return True def _start_entry_approach(self): # Re-read map -> base_link and cast a fresh ray after OCR confirmation. computed, failure = self._compute_wall_goal(self._entry_standoff) if failure is not None: self._entry_goal_finished = True self._publish_stop() self._has_control = True self._publish_status("ENTRY_GOAL_%s" % failure) return False if not self._wall_goal_client.wait_for_server(rospy.Duration(self._wall_goal_server_timeout)): self._entry_goal_finished = True self._publish_stop() self._has_control = True self._publish_status("ENTRY_GOAL_MOVE_BASE_UNAVAILABLE") return False goal_x, goal_y, goal_yaw, hit_x, hit_y = computed goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = goal_x goal.target_pose.pose.position.y = goal_y goal.target_pose.pose.orientation = Quaternion( z=math.sin(goal_yaw / 2.0), w=math.cos(goal_yaw / 2.0) ) self._entry_goal_pub.publish(goal.target_pose) self._entry_goal_active = True self._has_control = False self._wall_goal_client.send_goal(goal, done_cb=self._entry_goal_done) self._publish_status( "ENTRY_GOAL_SENT x=%.3f y=%.3f yaw=%.3f wall_x=%.3f wall_y=%.3f" % ( goal_x, goal_y, goal_yaw, hit_x, hit_y ) ) return True def _entry_goal_done(self, state, _result): self._entry_goal_active = False self._entry_goal_finished = True self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._publish_stop() self._has_control = True if state == GoalStatus.SUCCEEDED: self._publish_status("FACTORY_ENTRY_COMPLETE type=%s" % (self._factory_type or "UNKNOWN")) if self._require_order: self._completed_order_id = self._current_order["order_id"] if self._current_order else None else: reason = "move_base_state=%d" % state self._publish_status("FACTORY_ENTRY_FAILED %s" % reason) def _wall_goal_done(self, state, _result): self._wall_goal_active = False self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._publish_stop() self._has_control = True if state != GoalStatus.SUCCEEDED: self._wall_goal_finished = True self._final_alignment_active = False self._set_recognition_enabled(False, required=False) self._aligned = False self._publish_status("WALL_APPROACH_FAILED move_base_state=%d" % state) return # The map-derived goal brings the vehicle to the wall stand-off point. # Re-enable vision there for one final heading correction only. self._wall_goal_finished = False self._final_alignment_active = True self._aligned = False self._scan_state = "ALIGN" self._alignment_started_monotonic = time.monotonic() self._center_frame_count = 0 self._processed_sequence = self._detection_sequence self._set_recognition_enabled(True, required=False) self._publish_status("WALL_APPROACH_REFINING_ALIGNMENT") def _control_callback(self, _event): if not self._enabled: self._publish_status("DISABLED") return if self._require_order and self._current_order is None: self._publish_status("WAITING_FOR_ORDER") return if (self._require_order and self._armed_order_id != self._current_order.get("order_id")): self._publish_status("WAITING_FOR_START_ORDER") return if self._completed_order_id is not None: return if not self._navigation_seen_active: self._publish_status("WAITING_FOR_NAV_GOAL") return if not self._navigation_succeeded: self._publish_status("NAVIGATING") return now = time.monotonic() if self._ocr_active: if now - self._ocr_started_monotonic > self._ocr_timeout: self._ocr_active = False self._ocr_started_monotonic = None self._set_ocr_enabled(False, required=False) self._set_recognition_enabled(False, required=False) self._wall_goal_finished = True self._publish_status("FACTORY_OCR_FAILED timeout") else: self._publish_status("OCR_READING") return if self._wall_goal_active or self._entry_goal_active: return if self._wall_goal_finished or self._entry_goal_finished: return if self._scan_state not in ("ALIGN", "IDLE"): self._run_search_scan(now) return if self._aligned: self._set_recognition_enabled(False, required=False) self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._publish_stop() self._has_control = True self._publish_status("ALIGNED") return if now - self._alignment_started_monotonic > self._alignment_timeout: self._set_recognition_enabled(False, required=False) self._target_angular_z = 0.0 self._current_angular_z = 0.0 self._publish_stop() self._has_control = True self._publish_status("ALIGNMENT_TIMEOUT") return target_fresh = self._visible and now - self._last_detection_monotonic <= self._detection_timeout if not target_fresh: self._center_frame_count = 0 self._target_angular_z = 0.0 command_z = self._publish_smooth_command(0.0, now) self._publish_status("SEARCHING_FACTORY command_z=%+.3f" % command_z) return if self._processed_sequence != self._detection_sequence: self._processed_sequence = self._detection_sequence if abs(self._error) <= self._tolerance: self._center_frame_count += 1 self._target_angular_z = 0.0 if self._center_frame_count >= self._center_frames_required: self._current_angular_z = 0.0 self._publish_stop() self._has_control = True if self._wall_approach_enabled and not self._final_alignment_active: self._start_wall_approach() return self._aligned = True self._final_alignment_active = False if self._wall_approach_enabled and self._ocr_confirmation_enabled: self._publish_status("WALL_APPROACH_ALIGNED") # Full-frame OCR subscribes directly to the camera, so the # RKNN locator can be stopped before the OCR attempt. self._set_recognition_enabled(False, required=False) if self._start_ocr_confirmation(): return self._wall_goal_finished = True self._set_recognition_enabled(False, required=False) return self._wall_goal_finished = self._wall_approach_enabled self._set_recognition_enabled(False, required=False) self._publish_status("WALL_APPROACH_ALIGNED" if self._wall_approach_enabled else "ALIGNED") return else: self._center_frame_count = 0 target = self._angular_sign * self._kp * self._error target = max(-self._max_speed, min(self._max_speed, target)) if abs(target) < self._min_speed: target = math.copysign(self._min_speed, target) self._target_angular_z = target command_z = self._publish_smooth_command(self._target_angular_z, now) if self._center_frame_count: label = "FINAL_CENTER_FRAME" if self._final_alignment_active else "CENTER_FRAME" self._publish_status("%s %d/%d command_z=%+.3f" % ( label, self._center_frame_count, self._center_frames_required, command_z )) else: label = "FINAL_SMOOTH_ALIGN" if self._final_alignment_active else "SMOOTH_ALIGN" self._publish_status("%s error=%+.3f target_z=%+.3f command_z=%+.3f" % ( label, self._error, self._target_angular_z, command_z )) if __name__ == "__main__": FactoryAlignmentDemo() rospy.spin()