#!/usr/bin/env python3 """Adapt a nominal factory-search pose into obstacle-aware move_base candidates.""" from __future__ import annotations import math import queue import threading import time import actionlib import rospy import tf2_ros from actionlib_msgs.msg import GoalStatus from geometry_msgs.msg import Pose, PoseArray, PoseStamped, Quaternion from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from nav_msgs.msg import OccupancyGrid from nav_msgs.srv import GetPlan, GetPlanRequest from std_msgs.msg import String from std_srvs.srv import Trigger, TriggerResponse class _PlanTask: """One make_plan request handled by the sole background RPC worker.""" def __init__(self, request): self.request = request self.response = None self.error = None self.finished = threading.Event() class FactorySearchPointAdapter: """Send only safe observation candidates; never publish cmd_vel.""" _NAV_FAILURE_STATES = { GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.LOST, } _SEARCH_RETRY_PREFIXES = ( "FACTORY_NOT_FOUND_AFTER_360_DEG_SCAN", "ALIGNMENT_TIMEOUT", "WALL_NORMAL_UNAVAILABLE", "WALL_RAY_NO_HIT", "WALL_GOAL_INVALID_STANDOFF", "WALL_GOAL_OUTSIDE_MAP", "WALL_GOAL_OCCUPIED", "WALL_GOAL_MOVE_BASE_UNAVAILABLE", "WALL_APPROACH_FAILED", "FACTORY_OCR_FAILED", "FACTORY_ENTRY_FAILED", ) def __init__(self): rospy.init_node("factory_search_point_adapter") self._frame_id = rospy.get_param("~frame_id", "map") self._base_frame = rospy.get_param("~base_frame", "base_link") self._nominal_topic = rospy.get_param( "~nominal_goal_topic", "/factory_search_adapter/nominal_goal" ) self._static_map_topic = rospy.get_param("~static_map_topic", "/map") self._local_costmap_topic = rospy.get_param( "~local_costmap_topic", "/move_base/local_costmap/costmap" ) self._alignment_status_topic = rospy.get_param( "~alignment_status_topic", "/factory_alignment_demo/status" ) self._plan_service_name = rospy.get_param("~make_plan_service", "/move_base/make_plan") self._ring_radii = [float(value) for value in rospy.get_param("~ring_radii", [0.4, 0.6])] self._ring_directions = int(rospy.get_param("~ring_directions", 8)) self._footprint_length = float(rospy.get_param("~checked_footprint_length", 0.395)) self._footprint_width = float(rospy.get_param("~checked_footprint_width", 0.316)) self._static_occupied_threshold = int(rospy.get_param("~static_occupied_threshold", 65)) self._local_cost_threshold = int(rospy.get_param("~local_cost_threshold", 80)) self._local_costmap_timeout = float(rospy.get_param("~local_costmap_timeout_seconds", 2.0)) self._goal_timeout = float(rospy.get_param("~candidate_goal_timeout_seconds", 45.0)) self._search_timeout = float(rospy.get_param("~candidate_search_timeout_seconds", 80.0)) self._server_timeout = float(rospy.get_param("~move_base_server_timeout_seconds", 2.0)) self._plan_response_timeout = float(rospy.get_param("~make_plan_response_timeout_seconds", 0.4)) self._timer_period = float(rospy.get_param("~timer_period_seconds", 0.2)) self._validate_config() self._static_map = None self._local_costmap = None self._local_costmap_monotonic = 0.0 self._nominal_pose = None self._candidates = [] self._candidate_index = 0 self._state = "IDLE" self._active_sequence = 0 self._active_deadline = None self._search_deadline = None self._plan_timeout_this_round = False self._plan_fallback_announced = False self._last_status = None self._completion_locked = False self._client = actionlib.SimpleActionClient("move_base", MoveBaseAction) self._plan_client = rospy.ServiceProxy(self._plan_service_name, GetPlan) self._plan_request_queue = queue.Queue(maxsize=1) self._plan_worker_thread = threading.Thread( target=self._plan_worker, name="factory_make_plan", daemon=True ) self._plan_worker_thread.start() self._tf_buffer = tf2_ros.Buffer() self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) self._status_pub = rospy.Publisher( "/factory_search_adapter/status", String, queue_size=10, latch=True ) self._candidate_pub = rospy.Publisher( "/factory_search_adapter/candidates", PoseArray, queue_size=1, latch=True ) self._selected_goal_pub = rospy.Publisher( "/factory_search_adapter/selected_goal", PoseStamped, queue_size=1, latch=True ) self._reset_service = rospy.Service("/factory_search_adapter/reset", Trigger, self._reset_callback) rospy.Subscriber(self._nominal_topic, PoseStamped, self._nominal_callback, queue_size=1) rospy.Subscriber(self._static_map_topic, OccupancyGrid, self._static_map_callback, queue_size=1) rospy.Subscriber(self._local_costmap_topic, OccupancyGrid, self._local_costmap_callback, queue_size=1) rospy.Subscriber(self._alignment_status_topic, String, self._alignment_status_callback, queue_size=10) rospy.Timer(rospy.Duration(self._timer_period), self._timer_callback) rospy.on_shutdown(self._cancel_own_goal) self._publish_status("WAITING_FOR_NOMINAL_GOAL") def _validate_config(self): if self._frame_id != "map": raise ValueError("factory search candidates must use the map frame") if self._ring_directions < 4: raise ValueError("ring_directions must be at least 4") if any(radius <= 0.0 for radius in self._ring_radii): raise ValueError("ring radii must be positive") if self._footprint_length <= 0.0 or self._footprint_width <= 0.0: raise ValueError("checked footprint dimensions must be positive") if not 0 <= self._static_occupied_threshold <= 100: raise ValueError("static_occupied_threshold must be in [0, 100]") if not 1 <= self._local_cost_threshold <= 100: raise ValueError("local_cost_threshold must be in [1, 100]") if min(self._local_costmap_timeout, self._goal_timeout, self._search_timeout, self._server_timeout, self._plan_response_timeout, self._timer_period) <= 0.0: raise ValueError("search adapter timeouts must be positive") @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 _quaternion_from_yaw(yaw): return Quaternion(z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0)) def _publish_status(self, status): if status == self._last_status: return self._last_status = status rospy.loginfo("factory search adapter: %s", status) self._status_pub.publish(String(data=status)) def _static_map_callback(self, message): if message.header.frame_id.lstrip("/") != self._frame_id: rospy.logwarn_throttle(5.0, "factory search ignored static map frame %s", message.header.frame_id) return self._static_map = message def _local_costmap_callback(self, message): if message.header.frame_id.lstrip("/") != self._frame_id: rospy.logwarn_throttle( 5.0, "factory search ignored local costmap frame %s (expected %s)", message.header.frame_id, self._frame_id, ) return self._local_costmap = message self._local_costmap_monotonic = time.monotonic() def _reset_callback(self, _request): self._cancel_own_goal() self._active_sequence += 1 self._nominal_pose = None self._candidates = [] self._candidate_index = 0 self._active_deadline = None self._search_deadline = None self._plan_timeout_this_round = False self._plan_fallback_announced = False self._completion_locked = False self._state = "IDLE" self._publish_status("RESET_READY_FOR_NEXT_OBSERVATION") return TriggerResponse(success=True, message="factory search adapter reset") def _nominal_callback(self, message): if self._completion_locked: self._publish_status("NOMINAL_GOAL_IGNORED_COMPLETE_LOCKED") return if message.header.frame_id.lstrip("/") != self._frame_id: self._publish_status("NOMINAL_GOAL_WRONG_FRAME") return yaw = self._yaw_from_quaternion(message.pose.orientation) values = (message.pose.position.x, message.pose.position.y, yaw) if not all(math.isfinite(value) for value in values): self._publish_status("NOMINAL_GOAL_INVALID") return self._cancel_own_goal() self._active_sequence += 1 self._nominal_pose = values self._candidates = [] self._candidate_index = 0 self._active_deadline = None self._search_deadline = None self._plan_timeout_this_round = False self._plan_fallback_announced = False self._state = "PREPARING" self._publish_status("PREPARING_CANDIDATES x=%.3f y=%.3f yaw=%.3f" % values) def _cancel_own_goal(self): if hasattr(self, "_client") and self._state == "NAVIGATING": self._client.cancel_goal() @staticmethod def _grid_origin_yaw(grid): return FactorySearchPointAdapter._yaw_from_quaternion(grid.info.origin.orientation) def _map_to_grid(self, grid, x, y): resolution = grid.info.resolution if resolution <= 0.0: return None origin = grid.info.origin.position yaw = self._grid_origin_yaw(grid) dx, dy = x - origin.x, y - origin.y local_x = math.cos(yaw) * dx + math.sin(yaw) * dy local_y = -math.sin(yaw) * dx + math.cos(yaw) * dy column, row = int(math.floor(local_x / resolution)), int(math.floor(local_y / resolution)) if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width: return None return row, column def _grid_to_map(self, grid, row, column): resolution = grid.info.resolution origin = grid.info.origin.position yaw = self._grid_origin_yaw(grid) local_x, local_y = (column + 0.5) * resolution, (row + 0.5) * resolution 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, ) @staticmethod def _cost_at(grid, row, column): return grid.data[row * grid.info.width + column] def _footprint_cost(self, grid, x, y, yaw, threshold): """Return max cell cost, or None if footprint reaches unknown/outside/lethal cells.""" resolution = grid.info.resolution if resolution <= 0.0: return None radius = math.hypot(self._footprint_length / 2.0, self._footprint_width / 2.0) centre = self._map_to_grid(grid, x, y) if centre is None: return None radius_cells = int(math.ceil(radius / resolution)) + 1 max_cost = 0 found = False for row in range(centre[0] - radius_cells, centre[0] + radius_cells + 1): for column in range(centre[1] - radius_cells, centre[1] + radius_cells + 1): if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width: return None cell_x, cell_y = self._grid_to_map(grid, row, column) dx, dy = cell_x - x, cell_y - y longitudinal = math.cos(yaw) * dx + math.sin(yaw) * dy lateral = -math.sin(yaw) * dx + math.cos(yaw) * dy if (abs(longitudinal) > self._footprint_length / 2.0 or abs(lateral) > self._footprint_width / 2.0): continue found = True cost = self._cost_at(grid, row, column) if cost < 0 or cost >= threshold: return None max_cost = max(max_cost, cost) return max_cost if found else None def _footprint_is_inside_grid(self, grid, x, y, yaw): """Whether the complete checked rectangle is covered by this grid. The local costmap is a rolling window. A candidate farther than that window is not evidence of an obstacle; it simply cannot yet be checked against live cone observations. Static-map and global-plan checks still apply in that case, and TEB will receive the current local map while driving there. """ half_length = self._footprint_length / 2.0 half_width = self._footprint_width / 2.0 for longitudinal in (-half_length, half_length): for lateral in (-half_width, half_width): corner_x = x + math.cos(yaw) * longitudinal - math.sin(yaw) * lateral corner_y = y + math.sin(yaw) * longitudinal + math.cos(yaw) * lateral if self._map_to_grid(grid, corner_x, corner_y) is None: return False return True def _fresh_local_costmap(self): return (self._local_costmap is not None and time.monotonic() - self._local_costmap_monotonic <= self._local_costmap_timeout) def _current_pose(self): try: transform = self._tf_buffer.lookup_transform( self._frame_id, self._base_frame, rospy.Time(0), rospy.Duration(0.2) ) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException, tf2_ros.TimeoutException): return None translation = transform.transform.translation return translation.x, translation.y, self._yaw_from_quaternion(transform.transform.rotation) def _pose_stamped(self, x, y, yaw): pose = PoseStamped() pose.header.frame_id = self._frame_id pose.header.stamp = rospy.Time.now() pose.pose.position.x = x pose.pose.position.y = y pose.pose.orientation = self._quaternion_from_yaw(yaw) return pose def _plan_worker(self): """Serialize potentially stuck service calls in one daemon worker.""" while True: task = self._plan_request_queue.get() try: task.response = self._plan_client(task.request) except Exception as error: # rospy may expose several service exceptions. task.error = error finally: task.finished.set() def _announce_plan_fallback(self): if self._plan_fallback_announced: return self._plan_fallback_announced = True self._publish_status("MAKE_PLAN_TIMEOUT_FALLBACK_TO_MOVE_BASE") def _plan_length(self, x, y, yaw): """Return reachable, length, and whether a live plan was obtained. The timer thread never calls the service directly. A stalled RPC can leave one daemon worker blocked, but this search round immediately falls back and no additional make_plan calls are queued for its other candidates. """ if self._plan_timeout_this_round: return True, float("inf"), False # Do not probe the service from the timer callback. In the field a # registered service can still stall during a transport handshake. # The only RPC is therefore made by _plan_worker below. start = self._current_pose() if start is None: self._plan_timeout_this_round = True self._announce_plan_fallback() return True, float("inf"), False request = GetPlanRequest() request.start = self._pose_stamped(*start) request.goal = self._pose_stamped(x, y, yaw) request.tolerance = 0.0 task = _PlanTask(request) try: self._plan_request_queue.put_nowait(task) except queue.Full: self._plan_timeout_this_round = True self._announce_plan_fallback() return True, float("inf"), False if not task.finished.wait(self._plan_response_timeout): self._plan_timeout_this_round = True self._announce_plan_fallback() return True, float("inf"), False if task.error is not None or task.response is None: rospy.logwarn_throttle(2.0, "factory search make_plan failed: %s", task.error) self._plan_timeout_this_round = True self._announce_plan_fallback() return True, float("inf"), False poses = task.response.plan.poses if len(poses) < 2: return False, None, True length = 0.0 for first, second in zip(poses, poses[1:]): dx = second.pose.position.x - first.pose.position.x dy = second.pose.position.y - first.pose.position.y length += math.hypot(dx, dy) return True, length, True def _candidate_valid(self, candidate, with_plan): if self._static_map is None: return False, "STATIC_MAP_UNAVAILABLE", None static_cost = self._footprint_cost( self._static_map, candidate["x"], candidate["y"], candidate["yaw"], self._static_occupied_threshold, ) if static_cost is None: return False, "STATIC_FOOTPRINT_BLOCKED", None local_costmap_checked = ( self._fresh_local_costmap() and self._footprint_is_inside_grid( self._local_costmap, candidate["x"], candidate["y"], candidate["yaw"]) ) if local_costmap_checked: local_cost = self._footprint_cost( self._local_costmap, candidate["x"], candidate["y"], candidate["yaw"], self._local_cost_threshold, ) if local_cost is None: return False, "LOCAL_FOOTPRINT_BLOCKED", None else: # A rolling local costmap may be absent or not cover a distant # candidate. That is not a cone collision: retain the static-map # and global-plan checks, then let TEB use live obstacles in motion. local_cost = self._local_cost_threshold - 1 plan_length = candidate.get("plan_length", float("inf")) make_plan_checked = candidate.get("make_plan_checked", False) if with_plan: reachable, plan_length, make_plan_checked = self._plan_length( candidate["x"], candidate["y"], candidate["yaw"] ) if reachable is None: return False, "MAP_BASE_TF_UNAVAILABLE", None if not reachable: return False, "GLOBAL_PLAN_UNREACHABLE", None candidate["local_cost"] = local_cost candidate["local_costmap_checked"] = local_costmap_checked candidate["make_plan_checked"] = make_plan_checked candidate["plan_length"] = plan_length return True, None, candidate def _generate_raw_candidates(self): x, y, yaw = self._nominal_pose generated = [{"x": x, "y": y, "yaw": yaw, "radius": 0.0}] seen = {(round(x, 4), round(y, 4))} for radius in self._ring_radii: for index in range(self._ring_directions): angle = 2.0 * math.pi * index / self._ring_directions cx, cy = x + radius * math.cos(angle), y + radius * math.sin(angle) key = round(cx, 4), round(cy, 4) if key not in seen: seen.add(key) generated.append({"x": cx, "y": cy, "yaw": yaw, "radius": radius}) return generated def _publish_candidates(self): message = PoseArray() message.header.frame_id = self._frame_id message.header.stamp = rospy.Time.now() for candidate in self._candidates: pose = Pose() pose.position.x, pose.position.y = candidate["x"], candidate["y"] pose.orientation = self._quaternion_from_yaw(candidate["yaw"]) message.poses.append(pose) self._candidate_pub.publish(message) def _prepare_candidates(self): if self._static_map is None: self._publish_status("WAITING_FOR_STATIC_MAP") return if not self._fresh_local_costmap(): self._publish_status("LOCAL_COSTMAP_UNAVAILABLE_STATIC_PLAN_FALLBACK") accepted = [] for candidate in self._generate_raw_candidates(): valid, _reason, value = self._candidate_valid(candidate, with_plan=True) if valid: accepted.append(value) if not accepted: self._state = "FAILED" self._publish_status("NO_SAFE_SEARCH_CANDIDATES") return accepted.sort(key=lambda item: (item["radius"], item["local_cost"], item["plan_length"])) self._candidates = accepted self._candidate_index = 0 self._publish_candidates() self._publish_status("CANDIDATES_READY count=%d" % len(self._candidates)) self._send_next_candidate("INITIAL") def _send_next_candidate(self, reason): while self._candidate_index < len(self._candidates): candidate = self._candidates[self._candidate_index] self._candidate_index += 1 valid, invalid_reason, candidate = self._candidate_valid(candidate, with_plan=True) if not valid: self._publish_status("CANDIDATE_SKIPPED reason=%s" % invalid_reason) continue if not self._client.wait_for_server(rospy.Duration(self._server_timeout)): self._state = "FAILED" self._publish_status("MOVE_BASE_UNAVAILABLE") return goal = MoveBaseGoal() goal.target_pose = self._pose_stamped(candidate["x"], candidate["y"], candidate["yaw"]) self._active_sequence += 1 sequence = self._active_sequence self._state = "NAVIGATING" self._active_deadline = time.monotonic() + self._goal_timeout self._selected_goal_pub.publish(goal.target_pose) self._client.send_goal( goal, done_cb=lambda state, result, seq=sequence, item=candidate: self._goal_done(seq, item, state, result), ) self._publish_status( "CANDIDATE_SENT %d/%d reason=%s local_costmap=%s make_plan=%s x=%.3f y=%.3f yaw=%.3f" % ( self._candidate_index, len(self._candidates), reason, "CHECKED" if candidate["local_costmap_checked"] else "FALLBACK", "CHECKED" if candidate["make_plan_checked"] else "FALLBACK", candidate["x"], candidate["y"], candidate["yaw"], ) ) return self._state = "FAILED" self._active_deadline = None self._publish_status("ALL_SEARCH_CANDIDATES_EXHAUSTED") def _goal_done(self, sequence, candidate, state, _result): if sequence != self._active_sequence or self._state != "NAVIGATING": return self._active_deadline = None if state == GoalStatus.SUCCEEDED: self._state = "WAITING_FOR_SEARCH" self._search_deadline = time.monotonic() + self._search_timeout self._publish_status( "CANDIDATE_REACHED %d/%d; WAITING_FOR_FACTORY_SEARCH" % ( self._candidate_index, len(self._candidates) ) ) return if state in self._NAV_FAILURE_STATES: self._publish_status("CANDIDATE_NAV_FAILED state=%d; TRYING_NEXT" % state) else: self._publish_status("CANDIDATE_NAV_UNEXPECTED state=%d; TRYING_NEXT" % state) self._send_next_candidate("NAVIGATION_FAILURE") def _alignment_status_callback(self, message): if self._state != "WAITING_FOR_SEARCH": return status = message.data if status.startswith("FACTORY_ENTRY_COMPLETE"): self._state = "COMPLETE" self._completion_locked = True self._search_deadline = None self._publish_status("FACTORY_ENTRY_COMPLETE; REMAINING_CANDIDATES_CANCELLED_LOCKED") return if status.startswith("FACTORY_MISMATCH"): self._state = "FAILED" self._search_deadline = None self._publish_status("OBSERVATION_MISMATCH") return entry_goal_failed = ( status.startswith("ENTRY_GOAL_") and not status.startswith("ENTRY_GOAL_SENT") ) if status.startswith(self._SEARCH_RETRY_PREFIXES) or entry_goal_failed: self._search_deadline = None self._publish_status("FACTORY_SEARCH_FAILED status=%s; TRYING_NEXT" % status) self._send_next_candidate("SEARCH_FAILURE") def _timer_callback(self, _event): try: self._timer_tick() except Exception as error: rospy.logerr("factory search timer error: %s", error) self._publish_status("ADAPTER_TIMER_ERROR") def _timer_tick(self): if self._state == "PREPARING": self._prepare_candidates() return active_deadline = self._active_deadline if (self._state == "NAVIGATING" and active_deadline is not None and time.monotonic() > active_deadline): self._client.cancel_goal() self._active_sequence += 1 self._active_deadline = None self._publish_status("CANDIDATE_NAV_TIMEOUT; TRYING_NEXT") self._send_next_candidate("NAVIGATION_TIMEOUT") return search_deadline = self._search_deadline if (self._state == "WAITING_FOR_SEARCH" and search_deadline is not None and time.monotonic() > search_deadline): self._search_deadline = None self._publish_status("FACTORY_SEARCH_TIMEOUT; TRYING_NEXT") self._send_next_candidate("SEARCH_TIMEOUT") def main(): try: FactorySearchPointAdapter() rospy.spin() except (ValueError, rospy.ROSException) as error: rospy.logfatal("factory search adapter did not start: %s", error) raise if __name__ == "__main__": main()