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