Task1/task1/scripts/competition_task1_manager.py

#!/usr/bin/env python3
"""Run competition task 1 after a valid voice command."""

import json
import math
import subprocess
import sys
import threading
import time
from pathlib import Path

import actionlib
import rospy
import rospkg
from actionlib_msgs.msg import GoalStatus
from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import String
from std_srvs.srv import SetBool, Trigger


class TaskFailure(Exception):
    """A controlled task failure that must not use stale TTS output."""


class CompetitionTask1Manager:
    WALLS = ("wall1", "wall2", "wall3")

    def __init__(self):
        self._lock = threading.Lock()
        self._status_pub = rospy.Publisher(
            "/competition_task1/status", String, queue_size=10, latch=True
        )
        self._orders_pub = rospy.Publisher(
            "/competition_task1/orders", String, queue_size=1, latch=True
        )
        self._initial_pose_pub = rospy.Publisher(
            "/initialpose", PoseWithCovarianceStamped, queue_size=1
        )
        self._client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        self._cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
        self._latest_amcl_yaw = None
        self._latest_amcl_x = None
        self._latest_amcl_y = None
        self._latest_amcl_time = 0.0
        self._amcl_sub = rospy.Subscriber(
            "/amcl_pose", PoseWithCovarianceStamped, self._amcl_callback, queue_size=10
        )
        self._qr_sub = rospy.Subscriber(
            "/qr_scan_result", String, self._qr_callback, queue_size=1
        )
        self._active_wall = None
        self._products = {wall: None for wall in self.WALLS}
        self._seen_products = set()
        self._product_order = []
        self._scan_deadline = None
        self._config = self._load_config()
        self._frame_id = self._config["frame_id"]
        self._start_pose = self._config["start_pose"]
        self._initial_pose_settle = float(self._config["initial_pose_settle_seconds"])
        self._initial_pose_position_tolerance = float(
            self._config["initial_pose_position_tolerance_m"]
        )
        self._initial_pose_yaw_tolerance = float(
            self._config["initial_pose_yaw_tolerance_rad"]
        )
        self._points = self._config["points"]
        self._scan_window = float(self._config["scan_window_seconds"])
        self._settle = float(self._config["scan_settle_seconds"])
        self._center_rotation_speed = abs(float(
            self._config["center_rotation_speed_rad_s"]
        ))
        self._center_rotation_tolerance = float(
            self._config["center_rotation_yaw_tolerance_rad"]
        )
        self._center_rotation_timeout = float(
            self._config["center_rotation_timeout_seconds"]
        )
        self._scan_timeout = float(self._config["scan_total_timeout_seconds"])
        self._initial_navigation_timeout = float(
            self._config["initial_navigation_timeout_seconds"]
        )
        self._server_timeout = float(
            self._config["move_base_server_timeout_seconds"]
        )
        rospy.on_shutdown(self._shutdown)

    @staticmethod
    def _load_config():
        package_path = Path(rospkg.RosPack().get_path("ucar_nav"))
        config_path = package_path / "config" / "competition" / "task1_scan_points.json"
        with config_path.open("r", encoding="utf-8") as stream:
            config = json.load(stream)
        if config.get("frame_id") != "map":
            raise TaskFailure("任务1扫码点必须使用 map 坐标系")
        return config

    def _publish(self, state, detail):
        message = "%s: %s" % (state, detail)
        rospy.loginfo("competition task1 %s", message)
        self._status_pub.publish(String(data=message))

    def _amcl_callback(self, message):
        orientation = message.pose.pose.orientation
        yaw = math.atan2(
            2.0 * (orientation.w * orientation.z + orientation.x * orientation.y),
            1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z),
        )
        with self._lock:
            self._latest_amcl_x = message.pose.pose.position.x
            self._latest_amcl_y = message.pose.pose.position.y
            self._latest_amcl_yaw = yaw
            self._latest_amcl_time = time.monotonic()

    def _set_initial_pose(self):
        try:
            x = float(self._start_pose["x"])
            y = float(self._start_pose["y"])
            yaw = float(self._start_pose["yaw"])
        except (KeyError, TypeError, ValueError) as error:
            raise TaskFailure("初始位姿配置无效：%s" % error)
        if not all(math.isfinite(value) for value in (x, y, yaw)):
            raise TaskFailure("初始位姿包含非有限数值")

        message = PoseWithCovarianceStamped()
        message.header.frame_id = self._frame_id
        message.pose.pose.position.x = x
        message.pose.pose.position.y = y
        message.pose.pose.orientation.z = math.sin(yaw / 2.0)
        message.pose.pose.orientation.w = math.cos(yaw / 2.0)
        message.pose.covariance[0] = 0.25
        message.pose.covariance[7] = 0.25
        message.pose.covariance[35] = 0.0685
        self._publish("INITIALIZING", "设置AMCL初始位姿 x=%.3f y=%.3f yaw=%.3f" % (x, y, yaw))
        for _ in range(3):
            message.header.stamp = rospy.Time.now()
            self._initial_pose_pub.publish(message)
            rospy.sleep(0.1)

        deadline = time.monotonic() + self._initial_pose_settle
        while not rospy.is_shutdown() and time.monotonic() < deadline:
            with self._lock:
                current_x = self._latest_amcl_x
                current_y = self._latest_amcl_y
                current_yaw = self._latest_amcl_yaw
            if current_x is not None and current_y is not None and current_yaw is not None:
                position_error = math.hypot(current_x - x, current_y - y)
                yaw_error = abs(self._wrap_to_pi(current_yaw - yaw))
                if (position_error <= self._initial_pose_position_tolerance
                        and yaw_error <= self._initial_pose_yaw_tolerance):
                    self._publish("INITIALIZED", "AMCL已确认初始位姿")
                    return
            rospy.sleep(0.05)
        raise TaskFailure("AMCL未确认自动设置的初始位姿")

    def _shutdown(self):
        self._client.cancel_all_goals()
        self._stop_robot()
        self._set_scanner_mode(False, False, required=False)

    def _qr_callback(self, message):
        product = message.data.strip()
        if not product:
            return
        with self._lock:
            if product in self._seen_products:
                rospy.logwarn("task1 ignored duplicate product: %s", product)
                return
            self._seen_products.add(product)
            self._product_order.append(product)
            wall = self._active_wall
            if wall is not None and self._products[wall] is None:
                self._products[wall] = product
                self._active_wall = None
        location = wall if wall is not None else "scan_area"
        self._publish("QR_FOUND", "%s识别到%s（第%d个商品）" % (
            location, product, len(self._product_order)
        ))

    def _reset_qr_scanner(self):
        try:
            rospy.wait_for_service("/qr_scan/reset", timeout=3.0)
            response = rospy.ServiceProxy("/qr_scan/reset", Trigger)()
        except (rospy.ROSException, rospy.ServiceException) as error:
            raise TaskFailure("二维码节点未就绪或不支持重置：%s" % error)
        if not response.success:
            raise TaskFailure("二维码节点重置失败：%s" % response.message)

    @staticmethod
    def _set_bool_service(name, value, required):
        try:
            rospy.wait_for_service(name, timeout=3.0)
            response = rospy.ServiceProxy(name, SetBool)(value)
        except (rospy.ROSException, rospy.ServiceException) as error:
            if required:
                raise TaskFailure("二维码服务%s不可用：%s" % (name, error))
            rospy.logwarn("二维码服务%s关闭失败：%s", name, error)
            return
        if not response.success:
            if required:
                raise TaskFailure("二维码服务%s切换失败：%s" % (name, response.message))
            rospy.logwarn("二维码服务%s关闭失败：%s", name, response.message)

    def _set_scanner_mode(self, npu_enabled, decode_enabled, required=True):
        # Decode is toggled first so idle mode never fetches QR URLs.
        self._set_bool_service("/qr_scan/set_decode_enabled", decode_enabled, required)
        self._set_bool_service("/qr_scan/set_npu_enabled", npu_enabled, required)

    def _remaining_scan_time(self):
        if self._scan_deadline is None:
            return None
        return self._scan_deadline - time.monotonic()

    def _require_scan_time(self):
        remaining = self._remaining_scan_time()
        if remaining is not None and remaining <= 0.0:
            raise TaskFailure("扫码总超时（%.0f 秒）" % self._scan_timeout)
        return remaining

    def _goal_from_point(self, name):
        try:
            point = self._points[name]
            x = float(point["x"])
            y = float(point["y"])
            yaw = float(point["yaw"])
        except (KeyError, TypeError, ValueError) as error:
            raise TaskFailure("扫码点%s配置无效：%s" % (name, error))
        if not all(math.isfinite(value) for value in (x, y, yaw)):
            raise TaskFailure("扫码点%s包含非有限数值" % name)
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = self._frame_id
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = x
        goal.target_pose.pose.position.y = y
        goal.target_pose.pose.orientation = Quaternion(
            z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0)
        )
        return goal

    def _navigate(self, point_name, initial=False):
        remaining = None if initial else self._require_scan_time()
        timeout = self._initial_navigation_timeout if initial else remaining
        goal = self._goal_from_point(point_name)
        self._publish(
            "NAVIGATING",
            "%s x=%.3f y=%.3f yaw=%.3f" % (
                point_name,
                goal.target_pose.pose.position.x,
                goal.target_pose.pose.position.y,
                2.0 * math.atan2(
                    goal.target_pose.pose.orientation.z,
                    goal.target_pose.pose.orientation.w,
                ),
            ),
        )
        self._client.send_goal(goal)
        deadline = time.monotonic() + timeout
        terminal_states = {
            GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED,
            GoalStatus.RECALLED, GoalStatus.LOST,
        }
        while not rospy.is_shutdown() and time.monotonic() < deadline:
            state = self._client.get_state()
            if state == GoalStatus.SUCCEEDED:
                return
            if state in terminal_states:
                raise TaskFailure("未到达%s，move_base状态码%d" % (point_name, state))
            rospy.sleep(0.05)
        self._client.cancel_goal()
        if initial:
            raise TaskFailure("前往领取区初始点超时")
        raise TaskFailure("前往%s时扫码总超时" % point_name)

    def _wrap_to_pi(self, angle):
        return math.atan2(math.sin(angle), math.cos(angle))

    def _stop_robot(self):
        command = Twist()
        for _ in range(3):
            self._cmd_vel_pub.publish(command)
            rospy.sleep(0.03)

    def _turn_in_place(self, target_point):
        target = float(self._points[target_point]["yaw"])
        remaining = self._require_scan_time()
        timeout = min(self._center_rotation_timeout, remaining)
        deadline = time.monotonic() + timeout
        self._client.cancel_all_goals()
        self._stop_robot()
        self._publish(
            "ROTATING",
            "%s slow in-place turn to %.3f rad at %.2f rad/s" % (
                target_point, target, self._center_rotation_speed
            ),
        )
        try:
            while not rospy.is_shutdown() and time.monotonic() < deadline:
                with self._lock:
                    yaw = self._latest_amcl_yaw
                if yaw is None:
                    rospy.sleep(0.05)
                    continue
                error = self._wrap_to_pi(target - yaw)
                if abs(error) <= self._center_rotation_tolerance:
                    return
                command = Twist()
                command.angular.z = (
                    self._center_rotation_speed if error > 0.0
                    else -self._center_rotation_speed
                )
                self._cmd_vel_pub.publish(command)
                rospy.sleep(0.05)
        finally:
            self._stop_robot()
        raise TaskFailure("中央原地转向%s超时" % target_point)

    def _scan_wall(self, wall, point_name):
        if self._all_walls_found():
            return True
        if self._products[wall] is not None:
            self._publish("SKIPPED", "%s已经识别，跳过%s" % (wall, point_name))
            return True
        self._require_scan_time()
        self._set_scanner_mode(True, True)
        try:
            rospy.sleep(min(self._settle, self._require_scan_time()))
            with self._lock:
                self._active_wall = wall
            self._publish("SCANNING", "%s在%s等待二维码" % (wall, point_name))
            deadline = time.monotonic() + min(
                self._scan_window, self._require_scan_time()
            )
            while not rospy.is_shutdown() and time.monotonic() < deadline:
                with self._lock:
                    if self._products[wall] is not None or self._all_walls_found():
                        return True
                rospy.sleep(0.05)
            with self._lock:
                return self._products[wall] is not None or self._all_walls_found()
        finally:
            with self._lock:
                if self._active_wall == wall:
                    self._active_wall = None
            self._set_scanner_mode(False, False, required=False)

    def _all_walls_found(self):
        return len(self._product_order) >= len(self.WALLS)

    def _clear_previous_files(self):
        spark_dir = self._spark_dir()
        for filename in ("tts_output.txt", "llm_result.json", "qr_products_input.json"):
            path = spark_dir / "audio" / filename
            if path.exists():
                path.unlink()

    @staticmethod
    def _spark_dir():
        speech_path = Path(rospkg.RosPack().get_path("speech_command"))
        return speech_path.parent / "SparkTalk"

    def _write_products_for_spark(self):
        products = list(self._product_order)
        if len(products) != len(self.WALLS):
            raise TaskFailure("未收集到三个不同商品")
        output = self._spark_dir() / "audio" / "qr_products_input.json"
        payload = {"products": products, "wall_products": self._products}
        with output.open("w", encoding="utf-8") as stream:
            json.dump(payload, stream, ensure_ascii=False, indent=2)
        self._publish("QR_COMPLETE", "已收集%s" % "、".join(products))

    @staticmethod
    def _factory_category(category):
        return {"食品": "食品", "日用品": "日用品", "电子产品": "电子"}.get(str(category).strip())

    @staticmethod
    def _warehouse_for(category):
        return {"食品": "食品加工车间", "日用品": "日用品加工车间",
                "电子产品": "电子产品生产车间"}.get(category)

    def _publish_orders(self, spark_dir):
        path = spark_dir / "audio" / "llm_result.json"
        try:
            with path.open("r", encoding="utf-8") as stream:
                result = json.load(stream)
            raw_tasks = [("real_task", result["real_task"]),
                         ("simulation_task", result["simulation_task"])]
            orders = []
            for order_id, task in raw_tasks:
                if task.get("success") is not True:
                    raise ValueError("%s未成功分类" % order_id)
                product = str(task["matched_product"]).strip()
                category = str(task["category"]).strip()
                factory_category = self._factory_category(category)
                warehouse = self._warehouse_for(category)
                if not product or factory_category is None or warehouse is None:
                    raise ValueError("%s字段或类别无效" % order_id)
                orders.append({"order_id": order_id, "product": product,
                               "category": category, "factory_category": factory_category,
                               "warehouse": warehouse})
            if orders[0]["factory_category"] == orders[1]["factory_category"]:
                raise ValueError("两份订单类别相同")
        except (OSError, ValueError, TypeError, KeyError, json.JSONDecodeError) as error:
            raise TaskFailure("星火订单结果无效：%s" % error)
        self._orders_pub.publish(String(data=json.dumps({"orders": orders}, ensure_ascii=False, sort_keys=True)))
        self._publish("ORDERS_READY", "已发布真实与仿真两份订单")

    def _run_spark(self):
        spark_dir = self._spark_dir()
        result = subprocess.run([sys.executable, "SparkMain.py"], cwd=str(spark_dir))
        if result.returncode != 0:
            raise TaskFailure("星火分类或JSON校验失败，退出码%d" % result.returncode)
        self._publish_orders(spark_dir)
        self._publish("COMPLETE", "任务1完成，等待语音播报")

    def run(self):
        self._clear_previous_files()
        self._publish("WAITING_FOR_NAV", "等待move_base")
        if not self._client.wait_for_server(rospy.Duration(self._server_timeout)):
            raise TaskFailure("%s秒内未连接到move_base" % self._server_timeout)
        self._set_initial_pose()
        self._reset_qr_scanner()

        self._navigate("wall1_p1", initial=True)
        self._scan_deadline = time.monotonic() + self._scan_timeout
        self._scan_wall("wall1", "wall1_p1")

        # 在中央C仅导航一次；其余两个墙面朝向由低速原地旋转完成。
        self._navigate("center_wall1")
        self._turn_in_place("center_wall1")
        self._scan_wall("wall1", "center_wall1")
        if not self._all_walls_found():
            self._turn_in_place("center_wall2")
            self._scan_wall("wall2", "center_wall2")
        if not self._all_walls_found():
            self._turn_in_place("center_wall3")
            self._scan_wall("wall3", "center_wall3")

        for wall, point in (
            ("wall3", "wall3_p1"),
            ("wall2", "wall2_p3"),
            ("wall2", "wall2_p1"),
            ("wall1", "wall1_p3"),
        ):
            if self._all_walls_found():
                break
            if self._products[wall] is None:
                self._navigate(point)
                self._scan_wall(wall, point)

        if not self._all_walls_found():
            missing = [wall for wall in self.WALLS if self._products[wall] is None]
            raise TaskFailure("扫码结束，未识别%s" % "、".join(missing))
        self._write_products_for_spark()
        self._run_spark()


def main():
    rospy.init_node("competition_task1_manager")
    manager = CompetitionTask1Manager()
    try:
        manager.run()
        return 0
    except TaskFailure as error:
        manager._publish("FAILED", str(error))
        return 1
    except Exception as error:
        rospy.logerr("competition task1 crashed: %s", error)
        manager._publish("FAILED", "任务执行器异常：%s" % error)
        return 1


if __name__ == "__main__":
    sys.exit(main())