Task2/task2/scripts/competition_task2_executor.py

#!/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()