#!/usr/bin/env python3 """Execute one task-2 order through the configured factory observation route.""" from __future__ import annotations import json import math import threading from pathlib import Path import rospy import rospkg from geometry_msgs.msg import PoseStamped, Quaternion from std_msgs.msg import String from std_srvs.srv import Trigger class Task2Executor: _NEXT_POINT_PREFIXES = ('ALL_SEARCH_CANDIDATES_EXHAUSTED', 'NO_SAFE_SEARCH_CANDIDATES', 'MOVE_BASE_UNAVAILABLE', 'OBSERVATION_MISMATCH') def __init__(self): rospy.init_node('competition_task2_executor') self._lock = threading.RLock() self._points = self._load_points() self._current_order = None self._active = False self._pending_start_order_id = None self._index = 0 self._last_alignment_status = '' self._status_pub = rospy.Publisher('/competition_task2/status', String, queue_size=10, latch=True) self._result_pub = rospy.Publisher('/competition_task2/order_result', String, queue_size=10, latch=True) self._nominal_pub = rospy.Publisher('/factory_search_adapter/nominal_goal', PoseStamped, queue_size=1) self._reset = rospy.ServiceProxy('/factory_search_adapter/reset', Trigger) rospy.Subscriber('/competition_task2/current_order', String, self._order_cb, queue_size=1) rospy.Subscriber('/competition_task2/command', String, self._command_cb, queue_size=10) rospy.Subscriber('/factory_search_adapter/status', String, self._adapter_cb, queue_size=10) rospy.Subscriber('/factory_alignment_demo/status', String, self._alignment_cb, queue_size=10) self._publish('IDLE') @staticmethod def _load_points(): path = Path(rospkg.RosPack().get_path('ucar_nav')) / 'config' / 'competition' / 'task2_observation_points.yaml' try: import yaml with path.open(encoding='utf-8') as stream: config = yaml.safe_load(stream) except (OSError, ImportError) as error: raise rospy.ROSException('任务二观察点配置不可读：%s' % error) if not isinstance(config, dict) or config.get('frame_id') != 'map': raise rospy.ROSException('任务二观察点必须使用map坐标系') points = config.get('observation_points') if not isinstance(points, list) or not points: raise rospy.ROSException('任务二观察点不能为空') out = [] for item in points: values = tuple(float(item[key]) for key in ('x', 'y', 'yaw')) if not all(math.isfinite(value) for value in values): raise rospy.ROSException('任务二观察点包含非有限数值') out.append(values) return out def _publish(self, detail): rospy.loginfo('task2 executor: %s', detail) self._status_pub.publish(String(data=detail)) def _order_cb(self, message): try: order = json.loads(message.data) if not all(str(order[key]).strip() for key in ('order_id', 'product', 'category', 'warehouse')): raise ValueError('订单字段不完整') except (ValueError, TypeError, KeyError, json.JSONDecodeError) as error: self._publish('ORDER_INVALID %s' % error) return with self._lock: self._current_order = order self._active = False self._publish('ORDER_READY order_id=%s' % order['order_id']) if self._pending_start_order_id == order['order_id']: self._pending_start_order_id = None self._start_order_locked(order['order_id']) def _start_order_locked(self, order_id): if self._active and self._current_order.get('order_id') == order_id: self._publish('START_DUPLICATE_IGNORED order_id=%s' % order_id) return self._active = True self._index = 0 self._last_alignment_status = '' self._send_current_point('START_ORDER') def _command_cb(self, message): command = message.data.strip() with self._lock: if command == 'STOP': was_active = self._active self._pending_start_order_id = None self._active = False if was_active: self._reset_adapter(required=False) self._publish('STOPPED') return if not command.startswith('START_ORDER order_id='): return order_id = command.split('=', 1)[1].strip() if not order_id: return if self._current_order is None or self._current_order.get('order_id') != order_id: self._pending_start_order_id = order_id self._publish('START_WAITING_FOR_ORDER order_id=%s' % order_id) return self._pending_start_order_id = None self._start_order_locked(order_id) def _reset_adapter(self, required=True): try: rospy.wait_for_service('/factory_search_adapter/reset', timeout=1.5) response = self._reset() if not response.success: raise rospy.ServiceException(response.message) return True except (rospy.ROSException, rospy.ServiceException) as error: if required: self._fail('ADAPTER_RESET_FAILED %s' % error) else: rospy.logwarn('task2 executor could not reset adapter while stopping: %s', error) return False def _send_current_point(self, reason): if not self._active: return if self._index >= len(self._points): self._fail('SEARCH_ROUTE_EXHAUSTED') return if not self._reset_adapter(): return x, y, yaw = self._points[self._index] pose = PoseStamped() pose.header.frame_id = 'map' pose.header.stamp = rospy.Time.now() pose.pose.position.x, pose.pose.position.y = x, y pose.pose.orientation = Quaternion(z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0)) self._nominal_pub.publish(pose) self._publish('OBSERVATION_SENT %d/%d reason=%s x=%.3f y=%.3f yaw=%.3f' % ( self._index + 1, len(self._points), reason, x, y, yaw)) def _next_point(self, reason): if not self._active: return self._index += 1 self._send_current_point(reason) def _adapter_cb(self, message): status = message.data.strip() with self._lock: if not self._active: return if status.startswith(self._NEXT_POINT_PREFIXES): self._next_point(status.split()[0]) def _alignment_cb(self, message): status = message.data.strip() with self._lock: if not self._active or status == self._last_alignment_status: return self._last_alignment_status = status if status.startswith('FACTORY_ENTRY_COMPLETE'): self._complete_order() return # These are recoverable search-position failures. The adapter # retries another candidate and only route exhaustion is terminal. entry_goal_failed = ( status.startswith('ENTRY_GOAL_') and not status.startswith('ENTRY_GOAL_SENT') ) if status.startswith('FACTORY_ENTRY_FAILED') or entry_goal_failed: self._publish('ENTRY_RETRY_PENDING %s' % status) def _complete_order(self): if not self._active or self._current_order is None: return self._active = False result = dict(self._current_order) result['status'] = 'ORDER_PARKED' self._result_pub.publish(String( data=json.dumps(result, ensure_ascii=False, sort_keys=True))) self._publish('ORDER_PARKED order_id=%s' % result['order_id']) def _fail(self, reason): if not self._active: return self._active = False result = dict(self._current_order or {}) result.update({'status': 'FAILED', 'reason': reason}) self._result_pub.publish(String( data=json.dumps(result, ensure_ascii=False, sort_keys=True))) self._publish('FAILED %s' % reason) def main(): Task2Executor() rospy.spin() if __name__ == '__main__': main()