|
|
@@ -0,0 +1,473 @@
|
|
|
+#!/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())
|
ucar
1 месяц назад