|
|
@@ -0,0 +1,922 @@
|
|
|
+#!/usr/bin/env python3
|
|
|
+"""Align to a factory sign, then optionally navigate to a stand-off point on its wall."""
|
|
|
+from __future__ import annotations
|
|
|
+
|
|
|
+import json
|
|
|
+import math
|
|
|
+import time
|
|
|
+
|
|
|
+import actionlib
|
|
|
+import rospy
|
|
|
+import tf2_ros
|
|
|
+from actionlib_msgs.msg import GoalStatus, GoalStatusArray
|
|
|
+from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist
|
|
|
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
|
|
|
+from nav_msgs.msg import OccupancyGrid, Odometry
|
|
|
+from std_msgs.msg import Bool, Float32, String
|
|
|
+from std_srvs.srv import SetBool
|
|
|
+
|
|
|
+
|
|
|
+class FactoryAlignmentDemo:
|
|
|
+ _RUNNING_STATES = {
|
|
|
+ GoalStatus.PENDING, GoalStatus.ACTIVE, GoalStatus.PREEMPTING, GoalStatus.RECALLING,
|
|
|
+ }
|
|
|
+ _FAILED_STATES = {
|
|
|
+ GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED,
|
|
|
+ GoalStatus.RECALLED, GoalStatus.LOST,
|
|
|
+ }
|
|
|
+
|
|
|
+ def __init__(self):
|
|
|
+ rospy.init_node("factory_alignment_demo")
|
|
|
+ self._enabled = bool(rospy.get_param("~enabled", False))
|
|
|
+ # In competition mode only an explicit task-2 order may start recognition.
|
|
|
+ self._require_order = bool(rospy.get_param("~require_order", False))
|
|
|
+ self._current_order = None
|
|
|
+ self._completed_order_id = None
|
|
|
+ self._pending_start_order_id = None
|
|
|
+ self._armed_order_id = None
|
|
|
+ self._angular_sign = float(rospy.get_param("~angular_sign", -1.0))
|
|
|
+ self._kp = float(rospy.get_param("~kp", 0.25))
|
|
|
+ self._min_speed = float(rospy.get_param("~min_angular_speed", 0.10))
|
|
|
+ self._max_speed = float(rospy.get_param("~max_angular_speed", 0.12))
|
|
|
+ self._tolerance = float(rospy.get_param("~center_tolerance", 0.18))
|
|
|
+ self._center_frames_required = int(rospy.get_param("~center_confirm_frames", 3))
|
|
|
+ self._max_angular_accel = float(rospy.get_param("~max_angular_acceleration", 0.15))
|
|
|
+ self._detection_timeout = float(rospy.get_param("~detection_timeout_seconds", 0.35))
|
|
|
+ self._alignment_timeout = float(rospy.get_param("~alignment_timeout_seconds", 30.0))
|
|
|
+ self._scan_steps = int(rospy.get_param("~scan_steps", 6))
|
|
|
+ self._scan_direction = 1.0 if float(rospy.get_param("~scan_direction", 1.0)) >= 0.0 else -1.0
|
|
|
+ self._scan_speed = abs(float(rospy.get_param("~scan_angular_speed", 0.20)))
|
|
|
+ self._scan_yaw_tolerance = float(rospy.get_param("~scan_yaw_tolerance", 0.03))
|
|
|
+ self._scan_detection_window = float(rospy.get_param("~scan_detection_window_seconds", 1.0))
|
|
|
+ self._odom_timeout = float(rospy.get_param("~odom_timeout_seconds", 0.50))
|
|
|
+ rate = float(rospy.get_param("~control_rate", 20.0))
|
|
|
+
|
|
|
+ # This is explicitly opt-in because it sends a real move_base goal after alignment.
|
|
|
+ self._wall_approach_enabled = bool(rospy.get_param("~wall_approach_enabled", False))
|
|
|
+ self._wall_standoff = float(rospy.get_param("~wall_standoff_distance", 0.30))
|
|
|
+ self._wall_ray_max_distance = float(rospy.get_param("~wall_ray_max_distance", 5.0))
|
|
|
+ self._wall_occupied_threshold = int(rospy.get_param("~wall_occupied_threshold", 65))
|
|
|
+ self._wall_fit_radius = float(rospy.get_param("~wall_fit_radius", 0.60))
|
|
|
+ self._wall_line_inlier_distance = float(rospy.get_param("~wall_line_inlier_distance", 0.05))
|
|
|
+ self._wall_line_min_length = float(rospy.get_param("~wall_line_min_length", 0.25))
|
|
|
+ self._wall_line_min_support = int(rospy.get_param("~wall_line_min_support", 8))
|
|
|
+ self._wall_line_min_facing_alignment = float(
|
|
|
+ rospy.get_param("~wall_line_min_facing_alignment", 0.50)
|
|
|
+ )
|
|
|
+ self._wall_line_max_points = int(rospy.get_param("~wall_line_max_points", 180))
|
|
|
+ self._tf_map_frame = rospy.get_param("~tf_map_frame", "map")
|
|
|
+ self._tf_base_frame = rospy.get_param("~tf_base_frame", "base_link")
|
|
|
+ self._tf_lookup_timeout = float(rospy.get_param("~tf_lookup_timeout_seconds", 0.20))
|
|
|
+ self._amcl_fallback_timeout = float(
|
|
|
+ rospy.get_param("~amcl_pose_fallback_timeout_seconds", 5.0)
|
|
|
+ )
|
|
|
+ self._wall_goal_server_timeout = float(rospy.get_param("~wall_goal_server_timeout_seconds", 2.0))
|
|
|
+ self._ocr_confirmation_enabled = bool(
|
|
|
+ rospy.get_param("~ocr_confirmation_enabled", True)
|
|
|
+ )
|
|
|
+ self._ocr_timeout = float(rospy.get_param("~ocr_timeout_seconds", 15.0))
|
|
|
+ self._ocr_service_name = rospy.get_param(
|
|
|
+ "~ocr_enable_service", "/sign_recognition/set_enabled"
|
|
|
+ )
|
|
|
+ # Terminal white-box entry after OCR confirmation. The box touches the
|
|
|
+ # wall and extends 0.50 m outward; base_link is at the vehicle centre.
|
|
|
+ self._entry_goal_enabled = bool(rospy.get_param("~entry_goal_enabled", True))
|
|
|
+ self._entry_standoff = float(rospy.get_param("~entry_standoff_distance", 0.28))
|
|
|
+ self._entry_white_box_depth = float(rospy.get_param("~entry_white_box_depth", 0.50))
|
|
|
+ self._entry_vehicle_length = float(rospy.get_param("~entry_vehicle_length", 0.335))
|
|
|
+ if self._scan_steps <= 0:
|
|
|
+ raise ValueError("scan_steps must be positive")
|
|
|
+ if self._wall_standoff <= 0.0 or self._wall_ray_max_distance <= 0.0:
|
|
|
+ raise ValueError("wall approach distances must be positive")
|
|
|
+ half_vehicle_length = self._entry_vehicle_length / 2.0
|
|
|
+ if (self._entry_standoff <= half_vehicle_length
|
|
|
+ or self._entry_standoff + half_vehicle_length > self._entry_white_box_depth):
|
|
|
+ raise ValueError("entry goal does not keep the vehicle inside the white box")
|
|
|
+
|
|
|
+ self._error = 0.0
|
|
|
+ self._visible = False
|
|
|
+ self._latest_odom_yaw = None
|
|
|
+ self._latest_odom_monotonic = 0.0
|
|
|
+ self._latest_amcl_pose = None
|
|
|
+ self._latest_amcl_monotonic = 0.0
|
|
|
+ self._static_map = None
|
|
|
+ self._scan_state = "IDLE"
|
|
|
+ self._scan_completed_steps = 0
|
|
|
+ self._scan_target_yaw = None
|
|
|
+ self._scan_detection_deadline = None
|
|
|
+ self._last_detection_monotonic = 0.0
|
|
|
+ self._detection_sequence = 0
|
|
|
+ self._processed_sequence = 0
|
|
|
+ self._center_frame_count = 0
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._last_control_monotonic = time.monotonic()
|
|
|
+ self._navigation_seen_active = False
|
|
|
+ self._navigation_succeeded = False
|
|
|
+ self._alignment_started_monotonic = None
|
|
|
+ self._has_control = False
|
|
|
+ self._last_status = None
|
|
|
+ self._aligned = False
|
|
|
+ self._wall_goal_active = False
|
|
|
+ self._wall_goal_finished = False
|
|
|
+ self._entry_goal_active = False
|
|
|
+ self._entry_goal_finished = False
|
|
|
+ self._final_alignment_active = False
|
|
|
+ self._ocr_active = False
|
|
|
+ self._ocr_started_monotonic = None
|
|
|
+ self._factory_type = None
|
|
|
+ self._ocr_enabled = False
|
|
|
+ self._recognition_enabled = None
|
|
|
+ self._recognition_service_name = rospy.get_param(
|
|
|
+ "~recognition_enable_service", "/factory_sign_recognition/set_enabled"
|
|
|
+ )
|
|
|
+ self._recognition_enable = rospy.ServiceProxy(self._recognition_service_name, SetBool)
|
|
|
+ self._ocr_enable = rospy.ServiceProxy(self._ocr_service_name, SetBool)
|
|
|
+ self._wall_goal_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
|
|
|
+ self._tf_buffer = tf2_ros.Buffer()
|
|
|
+ self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
|
|
|
+
|
|
|
+ cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/factory_alignment_demo/cmd_vel")
|
|
|
+ self._cmd_pub = rospy.Publisher(cmd_vel_topic, Twist, queue_size=1)
|
|
|
+ self._status_pub = rospy.Publisher("/factory_alignment_demo/status", String, queue_size=1, latch=True)
|
|
|
+ self._wall_goal_pub = rospy.Publisher(
|
|
|
+ "/factory_alignment_demo/wall_goal", PoseStamped, queue_size=1, latch=True
|
|
|
+ )
|
|
|
+ self._entry_goal_pub = rospy.Publisher(
|
|
|
+ "/factory_alignment_demo/entry_goal", PoseStamped, queue_size=1, latch=True
|
|
|
+ )
|
|
|
+ self._factory_type_pub = rospy.Publisher(
|
|
|
+ "/factory_alignment_demo/factory_type", String, queue_size=1, latch=True
|
|
|
+ )
|
|
|
+ rospy.Subscriber("/competition_task2/current_order", String, self._order_callback, queue_size=1)
|
|
|
+ rospy.Subscriber("/competition_task2/command", String, self._task_command_callback, queue_size=10)
|
|
|
+ rospy.Subscriber("/factory_sign/target_center_error", Float32, self._error_callback, queue_size=1)
|
|
|
+ rospy.Subscriber("/factory_sign/target_visible", Bool, self._visible_callback, queue_size=1)
|
|
|
+ rospy.Subscriber("/sign_recognition", String, self._ocr_result_callback, queue_size=1)
|
|
|
+ rospy.Subscriber("/move_base/status", GoalStatusArray, self._navigation_callback, queue_size=5)
|
|
|
+ rospy.Subscriber("/odom", Odometry, self._odom_callback, queue_size=10)
|
|
|
+ rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self._amcl_callback, queue_size=10)
|
|
|
+ rospy.Subscriber("/map", OccupancyGrid, self._map_callback, queue_size=1)
|
|
|
+ rospy.Timer(rospy.Duration(1.0 / rate), self._control_callback)
|
|
|
+ rospy.on_shutdown(self._shutdown)
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._publish_status("DISABLED" if not self._enabled else ("WAITING_FOR_ORDER" if self._require_order else "WAITING_FOR_NAV_GOAL"))
|
|
|
+ rospy.loginfo(
|
|
|
+ "factory alignment demo ready: enabled=%s wall_approach=%s output=%s",
|
|
|
+ self._enabled, self._wall_approach_enabled, cmd_vel_topic,
|
|
|
+ )
|
|
|
+
|
|
|
+ def _publish_status(self, status):
|
|
|
+ if status != self._last_status:
|
|
|
+ self._status_pub.publish(String(data=status))
|
|
|
+ self._last_status = status
|
|
|
+
|
|
|
+ @staticmethod
|
|
|
+ def _normalise_factory_category(value):
|
|
|
+ return {"食品": "食品", "日用品": "日用品", "电子": "电子", "电子产品": "电子"}.get(str(value).strip())
|
|
|
+
|
|
|
+ def _order_callback(self, message):
|
|
|
+ try:
|
|
|
+ order = json.loads(message.data)
|
|
|
+ order_id = str(order["order_id"]).strip()
|
|
|
+ expected = self._normalise_factory_category(order.get("category", order.get("factory_category", "")))
|
|
|
+ product = str(order["product"]).strip()
|
|
|
+ warehouse = str(order["warehouse"]).strip()
|
|
|
+ if not order_id or expected is None or not product or not warehouse:
|
|
|
+ raise ValueError("missing required order field")
|
|
|
+ except (ValueError, TypeError, KeyError, json.JSONDecodeError) as error:
|
|
|
+ self._current_order = None
|
|
|
+ self._publish_status("ORDER_INVALID %s" % error)
|
|
|
+ return
|
|
|
+ # A fresh order is the only permitted way to clear the terminal parking lock.
|
|
|
+ self._current_order = {"order_id": order_id, "category": expected,
|
|
|
+ "product": product, "warehouse": warehouse}
|
|
|
+ self._armed_order_id = order_id if self._pending_start_order_id == order_id else None
|
|
|
+ if self._armed_order_id is not None:
|
|
|
+ self._pending_start_order_id = None
|
|
|
+ self._completed_order_id = None
|
|
|
+ self._navigation_seen_active = False
|
|
|
+ self._navigation_succeeded = False
|
|
|
+ self._wall_goal_finished = False
|
|
|
+ self._entry_goal_finished = False
|
|
|
+ self._publish_status("ORDER_READY order_id=%s expected=%s" % (order_id, expected))
|
|
|
+
|
|
|
+ def _task_command_callback(self, message):
|
|
|
+ command = message.data.strip()
|
|
|
+ if command == "STOP":
|
|
|
+ self._pending_start_order_id = None
|
|
|
+ self._armed_order_id = None
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._ocr_active = False
|
|
|
+ self._ocr_started_monotonic = None
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ if self._wall_goal_active or self._entry_goal_active:
|
|
|
+ self._wall_goal_client.cancel_goal()
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._set_ocr_enabled(False, required=False)
|
|
|
+ self._publish_status("TASK2_STOPPED")
|
|
|
+ return
|
|
|
+ if not command.startswith("START_ORDER order_id="):
|
|
|
+ return
|
|
|
+ order_id = command.split("=", 1)[1].strip()
|
|
|
+ if not order_id:
|
|
|
+ return
|
|
|
+ self._pending_start_order_id = order_id
|
|
|
+ if self._current_order is not None and self._current_order.get("order_id") == order_id:
|
|
|
+ self._armed_order_id = order_id
|
|
|
+ self._pending_start_order_id = None
|
|
|
+ self._publish_status("ORDER_ARMED order_id=%s" % order_id)
|
|
|
+
|
|
|
+ def _error_callback(self, message):
|
|
|
+ self._error = max(-1.0, min(1.0, message.data))
|
|
|
+ self._last_detection_monotonic = time.monotonic()
|
|
|
+ self._detection_sequence += 1
|
|
|
+
|
|
|
+ def _visible_callback(self, message):
|
|
|
+ self._visible = message.data
|
|
|
+
|
|
|
+ def _odom_callback(self, message):
|
|
|
+ orientation = message.pose.pose.orientation
|
|
|
+ self._latest_odom_yaw = self._yaw_from_quaternion(orientation)
|
|
|
+ self._latest_odom_monotonic = time.monotonic()
|
|
|
+
|
|
|
+ def _amcl_callback(self, message):
|
|
|
+ orientation = message.pose.pose.orientation
|
|
|
+ self._latest_amcl_pose = (
|
|
|
+ message.pose.pose.position.x,
|
|
|
+ message.pose.pose.position.y,
|
|
|
+ self._yaw_from_quaternion(orientation),
|
|
|
+ )
|
|
|
+ self._latest_amcl_monotonic = time.monotonic()
|
|
|
+
|
|
|
+ def _map_callback(self, message):
|
|
|
+ if message.header.frame_id.lstrip("/") != "map":
|
|
|
+ rospy.logwarn_throttle(5.0, "factory alignment ignored map frame %s", message.header.frame_id)
|
|
|
+ return
|
|
|
+ self._static_map = message
|
|
|
+
|
|
|
+ @staticmethod
|
|
|
+ def _yaw_from_quaternion(orientation):
|
|
|
+ return math.atan2(
|
|
|
+ 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y),
|
|
|
+ 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z),
|
|
|
+ )
|
|
|
+
|
|
|
+ @staticmethod
|
|
|
+ def _wrap_to_pi(angle):
|
|
|
+ return math.atan2(math.sin(angle), math.cos(angle))
|
|
|
+
|
|
|
+ def _set_recognition_enabled(self, enabled, required):
|
|
|
+ if self._recognition_enabled is enabled:
|
|
|
+ return True
|
|
|
+ try:
|
|
|
+ rospy.wait_for_service(self._recognition_service_name, timeout=2.0)
|
|
|
+ response = self._recognition_enable(enabled)
|
|
|
+ except (rospy.ROSException, rospy.ServiceException) as error:
|
|
|
+ if required:
|
|
|
+ self._publish_status("RECOGNITION_SERVICE_UNAVAILABLE")
|
|
|
+ else:
|
|
|
+ rospy.logwarn("factory recognition service unavailable: %s", error)
|
|
|
+ return False
|
|
|
+ if not response.success:
|
|
|
+ rospy.logwarn("factory recognition switch failed: %s", response.message)
|
|
|
+ return False
|
|
|
+ self._recognition_enabled = enabled
|
|
|
+ return True
|
|
|
+
|
|
|
+ def _set_ocr_enabled(self, enabled, required):
|
|
|
+ if not self._ocr_confirmation_enabled:
|
|
|
+ return not enabled
|
|
|
+ if self._ocr_enabled is enabled:
|
|
|
+ return True
|
|
|
+ try:
|
|
|
+ rospy.wait_for_service(self._ocr_service_name, timeout=2.0)
|
|
|
+ response = self._ocr_enable(enabled)
|
|
|
+ except (rospy.ROSException, rospy.ServiceException) as error:
|
|
|
+ if required:
|
|
|
+ self._publish_status("OCR_SERVICE_UNAVAILABLE")
|
|
|
+ else:
|
|
|
+ rospy.logwarn("sign OCR service unavailable: %s", error)
|
|
|
+ return False
|
|
|
+ if not response.success:
|
|
|
+ rospy.logwarn("sign OCR switch failed: %s", response.message)
|
|
|
+ if required:
|
|
|
+ self._publish_status("OCR_SERVICE_UNAVAILABLE")
|
|
|
+ return False
|
|
|
+ self._ocr_enabled = enabled
|
|
|
+ return True
|
|
|
+
|
|
|
+ def _start_ocr_confirmation(self):
|
|
|
+ self._ocr_active = True
|
|
|
+ self._ocr_started_monotonic = time.monotonic()
|
|
|
+ self._factory_type = None
|
|
|
+ if not self._set_ocr_enabled(True, required=True):
|
|
|
+ self._ocr_active = False
|
|
|
+ self._ocr_started_monotonic = None
|
|
|
+ return False
|
|
|
+ self._publish_status("OCR_READING")
|
|
|
+ return True
|
|
|
+
|
|
|
+ def _ocr_result_callback(self, message):
|
|
|
+ if not self._ocr_active:
|
|
|
+ return
|
|
|
+ result = message.data.strip()
|
|
|
+ self._ocr_active = False
|
|
|
+ self._ocr_started_monotonic = None
|
|
|
+ self._set_ocr_enabled(False, required=False)
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ factory_type = self._normalise_factory_category(result)
|
|
|
+ if factory_type is not None:
|
|
|
+ self._factory_type = factory_type
|
|
|
+ self._factory_type_pub.publish(String(data=factory_type))
|
|
|
+ if self._require_order:
|
|
|
+ expected = self._current_order["category"] if self._current_order else None
|
|
|
+ if factory_type != expected:
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ self._publish_status("FACTORY_MISMATCH order_id=%s detected=%s expected=%s" % (
|
|
|
+ self._current_order["order_id"] if self._current_order else "NONE",
|
|
|
+ factory_type, expected or "NONE"))
|
|
|
+ return
|
|
|
+ self._publish_status("FACTORY_MATCHED order_id=%s type=%s" % (
|
|
|
+ self._current_order["order_id"], factory_type))
|
|
|
+ else:
|
|
|
+ self._publish_status("FACTORY_CONFIRMED type=%s" % factory_type)
|
|
|
+ if self._entry_goal_enabled:
|
|
|
+ self._wall_goal_finished = False
|
|
|
+ self._start_entry_approach()
|
|
|
+ return
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ else:
|
|
|
+ self._factory_type = None
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ self._publish_status("FACTORY_OCR_FAILED result=%s" % (result or "EMPTY"))
|
|
|
+
|
|
|
+ def _navigation_callback(self, message):
|
|
|
+ if self._require_order and self._current_order is None:
|
|
|
+ return
|
|
|
+ if (self._require_order
|
|
|
+ and self._armed_order_id != self._current_order.get("order_id")):
|
|
|
+ return
|
|
|
+ if self._completed_order_id is not None:
|
|
|
+ return
|
|
|
+ if not message.status_list:
|
|
|
+ return
|
|
|
+ latest = max(
|
|
|
+ message.status_list,
|
|
|
+ key=lambda status: (status.goal_id.stamp.to_nsec(), status.goal_id.id),
|
|
|
+ )
|
|
|
+ state = latest.status
|
|
|
+ # move_base statuses generated by our own wall/entry goals must not reset this state machine.
|
|
|
+ if self._wall_goal_active or self._entry_goal_active:
|
|
|
+ return
|
|
|
+ if (self._wall_goal_finished or self._entry_goal_finished) and state not in self._RUNNING_STATES:
|
|
|
+ return
|
|
|
+ if state in self._RUNNING_STATES:
|
|
|
+ self._wall_goal_finished = False
|
|
|
+ self._entry_goal_finished = False
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._set_ocr_enabled(False, required=False)
|
|
|
+ self._ocr_active = False
|
|
|
+ self._ocr_started_monotonic = None
|
|
|
+ self._entry_goal_active = False
|
|
|
+ self._entry_goal_finished = False
|
|
|
+ self._factory_type = None
|
|
|
+ self._factory_type_pub.publish(String(data=""))
|
|
|
+ self._navigation_seen_active = True
|
|
|
+ self._navigation_succeeded = False
|
|
|
+ self._aligned = False
|
|
|
+ self._final_alignment_active = False
|
|
|
+ self._alignment_started_monotonic = None
|
|
|
+ if self._has_control:
|
|
|
+ self._publish_stop()
|
|
|
+ self._center_frame_count = 0
|
|
|
+ self._scan_state = "IDLE"
|
|
|
+ self._scan_completed_steps = 0
|
|
|
+ self._scan_target_yaw = None
|
|
|
+ self._scan_detection_deadline = None
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._last_control_monotonic = time.monotonic()
|
|
|
+ self._processed_sequence = self._detection_sequence
|
|
|
+ self._has_control = False
|
|
|
+ return
|
|
|
+ if self._navigation_seen_active and state == GoalStatus.SUCCEEDED:
|
|
|
+ self._navigation_succeeded = True
|
|
|
+ if self._alignment_started_monotonic is None:
|
|
|
+ self._alignment_started_monotonic = time.monotonic()
|
|
|
+ self._scan_state = "INITIAL_DETECT"
|
|
|
+ self._scan_completed_steps = 0
|
|
|
+ self._scan_target_yaw = None
|
|
|
+ self._scan_detection_deadline = self._alignment_started_monotonic + self._scan_detection_window
|
|
|
+ self._set_recognition_enabled(True, required=True)
|
|
|
+ self._publish_status("INITIAL_DETECTING")
|
|
|
+ return
|
|
|
+ if self._navigation_seen_active and state in self._FAILED_STATES:
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._set_ocr_enabled(False, required=False)
|
|
|
+ self._ocr_active = False
|
|
|
+ self._navigation_succeeded = False
|
|
|
+ self._aligned = False
|
|
|
+ self._alignment_started_monotonic = None
|
|
|
+ self._publish_status("NAVIGATION_NOT_SUCCEEDED")
|
|
|
+
|
|
|
+ def _publish_stop(self):
|
|
|
+ self._cmd_pub.publish(Twist())
|
|
|
+
|
|
|
+ def _shutdown(self):
|
|
|
+ self._wall_goal_client.cancel_goal()
|
|
|
+ if self._has_control:
|
|
|
+ self._publish_stop()
|
|
|
+
|
|
|
+ def _publish_smooth_command(self, desired_angular_z, now):
|
|
|
+ elapsed = max(0.0, min(0.2, now - self._last_control_monotonic))
|
|
|
+ max_delta = self._max_angular_accel * elapsed
|
|
|
+ delta = desired_angular_z - self._current_angular_z
|
|
|
+ if abs(delta) <= max_delta:
|
|
|
+ self._current_angular_z = desired_angular_z
|
|
|
+ else:
|
|
|
+ self._current_angular_z += math.copysign(max_delta, delta)
|
|
|
+ self._last_control_monotonic = now
|
|
|
+ command = Twist()
|
|
|
+ command.angular.z = self._current_angular_z
|
|
|
+ self._has_control = True
|
|
|
+ self._cmd_pub.publish(command)
|
|
|
+ return self._current_angular_z
|
|
|
+
|
|
|
+ def _run_search_scan(self, now):
|
|
|
+ if self._scan_state == "COMPLETE":
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ self._publish_status("FACTORY_NOT_FOUND_AFTER_360_DEG_SCAN")
|
|
|
+ return
|
|
|
+
|
|
|
+ odom_fresh = (
|
|
|
+ self._latest_odom_yaw is not None
|
|
|
+ and now - self._latest_odom_monotonic <= self._odom_timeout
|
|
|
+ )
|
|
|
+ if self._scan_state == "TURN":
|
|
|
+ if not odom_fresh:
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._publish_smooth_command(0.0, now)
|
|
|
+ self._publish_status("WAITING_FOR_ODOM")
|
|
|
+ return
|
|
|
+ if self._scan_target_yaw is None:
|
|
|
+ self._scan_target_yaw = self._wrap_to_pi(
|
|
|
+ self._latest_odom_yaw + self._scan_direction * 2.0 * math.pi / self._scan_steps
|
|
|
+ )
|
|
|
+ yaw_error = self._wrap_to_pi(self._scan_target_yaw - self._latest_odom_yaw)
|
|
|
+ if abs(yaw_error) <= self._scan_yaw_tolerance:
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._scan_state = "SETTLE"
|
|
|
+ command_z = self._publish_smooth_command(0.0, now)
|
|
|
+ self._publish_status("SCAN_STEP_%d_SETTLING command_z=%+.3f" % (
|
|
|
+ self._scan_completed_steps + 1, command_z
|
|
|
+ ))
|
|
|
+ return
|
|
|
+ self._target_angular_z = math.copysign(self._scan_speed, yaw_error)
|
|
|
+ command_z = self._publish_smooth_command(self._target_angular_z, now)
|
|
|
+ self._publish_status("SCANNING_STEP_%d/%d yaw_error=%+.3f command_z=%+.3f" % (
|
|
|
+ self._scan_completed_steps + 1, self._scan_steps, yaw_error, command_z
|
|
|
+ ))
|
|
|
+ return
|
|
|
+
|
|
|
+ if self._scan_state == "SETTLE":
|
|
|
+ command_z = self._publish_smooth_command(0.0, now)
|
|
|
+ if abs(command_z) <= 0.005:
|
|
|
+ self._scan_state = "DETECT"
|
|
|
+ self._scan_detection_deadline = now + self._scan_detection_window
|
|
|
+ self._set_recognition_enabled(True, required=True)
|
|
|
+ self._publish_status("SCAN_STEP_%d_DETECTING" % (self._scan_completed_steps + 1))
|
|
|
+ else:
|
|
|
+ self._publish_status("SCAN_STEP_%d_SETTLING command_z=%+.3f" % (
|
|
|
+ self._scan_completed_steps + 1, command_z
|
|
|
+ ))
|
|
|
+ return
|
|
|
+
|
|
|
+ if self._scan_state in ("INITIAL_DETECT", "DETECT"):
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ detected = self._visible and now - self._last_detection_monotonic <= self._detection_timeout
|
|
|
+ if detected:
|
|
|
+ self._scan_state = "ALIGN"
|
|
|
+ self._alignment_started_monotonic = now
|
|
|
+ self._center_frame_count = 0
|
|
|
+ self._processed_sequence = self._detection_sequence
|
|
|
+ self._publish_status("FACTORY_FOUND_STARTING_ALIGNMENT")
|
|
|
+ return
|
|
|
+ if now < self._scan_detection_deadline:
|
|
|
+ state = "INITIAL_DETECTING" if self._scan_state == "INITIAL_DETECT" else "SCAN_STEP_%d_DETECTING" % (self._scan_completed_steps + 1)
|
|
|
+ self._publish_status(state)
|
|
|
+ return
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ if self._scan_state == "INITIAL_DETECT":
|
|
|
+ self._scan_state = "TURN"
|
|
|
+ self._scan_target_yaw = None
|
|
|
+ else:
|
|
|
+ self._scan_completed_steps += 1
|
|
|
+ if self._scan_completed_steps >= self._scan_steps:
|
|
|
+ self._scan_state = "COMPLETE"
|
|
|
+ else:
|
|
|
+ self._scan_state = "TURN"
|
|
|
+ self._scan_target_yaw = None
|
|
|
+
|
|
|
+ @staticmethod
|
|
|
+ def _map_origin_yaw(grid):
|
|
|
+ return FactoryAlignmentDemo._yaw_from_quaternion(grid.info.origin.orientation)
|
|
|
+
|
|
|
+ @staticmethod
|
|
|
+ def _occupied(grid, row, column, threshold):
|
|
|
+ if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
|
|
|
+ return False
|
|
|
+ return grid.data[row * grid.info.width + column] >= threshold
|
|
|
+
|
|
|
+ @classmethod
|
|
|
+ def _map_to_grid(cls, grid, x, y):
|
|
|
+ resolution = grid.info.resolution
|
|
|
+ if resolution <= 0.0:
|
|
|
+ return None
|
|
|
+ yaw = cls._map_origin_yaw(grid)
|
|
|
+ dx = x - grid.info.origin.position.x
|
|
|
+ dy = y - grid.info.origin.position.y
|
|
|
+ column = int(math.floor((math.cos(yaw) * dx + math.sin(yaw) * dy) / resolution))
|
|
|
+ row = int(math.floor((-math.sin(yaw) * dx + math.cos(yaw) * dy) / resolution))
|
|
|
+ if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
|
|
|
+ return None
|
|
|
+ return row, column
|
|
|
+
|
|
|
+ @classmethod
|
|
|
+ def _grid_to_map(cls, grid, row, column):
|
|
|
+ resolution = grid.info.resolution
|
|
|
+ yaw = cls._map_origin_yaw(grid)
|
|
|
+ local_x = (column + 0.5) * resolution
|
|
|
+ local_y = (row + 0.5) * resolution
|
|
|
+ origin = grid.info.origin.position
|
|
|
+ return (
|
|
|
+ origin.x + math.cos(yaw) * local_x - math.sin(yaw) * local_y,
|
|
|
+ origin.y + math.sin(yaw) * local_x + math.cos(yaw) * local_y,
|
|
|
+ )
|
|
|
+
|
|
|
+ def _raycast_wall(self, grid, pose):
|
|
|
+ x, y, heading = pose
|
|
|
+ step = max(grid.info.resolution * 0.5, 0.01)
|
|
|
+ previous_cell = None
|
|
|
+ samples = int(math.ceil(self._wall_ray_max_distance / step))
|
|
|
+ for sample in range(1, samples + 1):
|
|
|
+ distance = sample * step
|
|
|
+ cell = self._map_to_grid(grid, x + distance * math.cos(heading), y + distance * math.sin(heading))
|
|
|
+ if cell is None:
|
|
|
+ break
|
|
|
+ if cell == previous_cell:
|
|
|
+ continue
|
|
|
+ previous_cell = cell
|
|
|
+ row, column = cell
|
|
|
+ if self._occupied(grid, row, column, self._wall_occupied_threshold):
|
|
|
+ hit_x, hit_y = self._grid_to_map(grid, row, column)
|
|
|
+ return hit_x, hit_y, row, column
|
|
|
+ return None
|
|
|
+
|
|
|
+ def _wall_normal_toward_robot(self, grid, hit_row, hit_column, robot_x, robot_y, heading):
|
|
|
+ radius_cells = max(1, int(math.ceil(self._wall_fit_radius / grid.info.resolution)))
|
|
|
+ queue = [(hit_row, hit_column)]
|
|
|
+ visited = set()
|
|
|
+ points = []
|
|
|
+ while queue:
|
|
|
+ row, column = queue.pop()
|
|
|
+ if (row, column) in visited:
|
|
|
+ continue
|
|
|
+ visited.add((row, column))
|
|
|
+ if not self._occupied(grid, row, column, self._wall_occupied_threshold):
|
|
|
+ continue
|
|
|
+ if math.hypot(row - hit_row, column - hit_column) > radius_cells:
|
|
|
+ continue
|
|
|
+ points.append(self._grid_to_map(grid, row, column))
|
|
|
+ for delta_row in (-1, 0, 1):
|
|
|
+ for delta_column in (-1, 0, 1):
|
|
|
+ if delta_row or delta_column:
|
|
|
+ queue.append((row + delta_row, column + delta_column))
|
|
|
+ if len(points) < self._wall_line_min_support:
|
|
|
+ return None
|
|
|
+
|
|
|
+ # A corner joins two wall segments. Fit several local lines, then retain
|
|
|
+ # the one whose outward normal faces the visually aligned vehicle.
|
|
|
+ if len(points) > self._wall_line_max_points:
|
|
|
+ stride = float(len(points)) / self._wall_line_max_points
|
|
|
+ points = [points[int(index * stride)] for index in range(self._wall_line_max_points)]
|
|
|
+ hit_x, hit_y = self._grid_to_map(grid, hit_row, hit_column)
|
|
|
+ desired_normal_x = -math.cos(heading)
|
|
|
+ desired_normal_y = -math.sin(heading)
|
|
|
+ best = None
|
|
|
+ for first_index, first in enumerate(points):
|
|
|
+ for second in points[first_index + 1:]:
|
|
|
+ dx = second[0] - first[0]
|
|
|
+ dy = second[1] - first[1]
|
|
|
+ length = math.hypot(dx, dy)
|
|
|
+ if length < self._wall_line_min_length:
|
|
|
+ continue
|
|
|
+ # The selected line must describe the actually struck wall cell.
|
|
|
+ hit_distance = abs(dy * (hit_x - first[0]) - dx * (hit_y - first[1])) / length
|
|
|
+ if hit_distance > self._wall_line_inlier_distance:
|
|
|
+ continue
|
|
|
+ normal = math.atan2(dy, dx) + math.pi / 2.0
|
|
|
+ if math.cos(normal) * (robot_x - hit_x) + math.sin(normal) * (robot_y - hit_y) < 0.0:
|
|
|
+ normal += math.pi
|
|
|
+ facing = math.cos(normal) * desired_normal_x + math.sin(normal) * desired_normal_y
|
|
|
+ if facing < self._wall_line_min_facing_alignment:
|
|
|
+ continue
|
|
|
+ support = 0
|
|
|
+ for point in points:
|
|
|
+ distance = abs(dy * (point[0] - first[0]) - dx * (point[1] - first[1])) / length
|
|
|
+ if distance <= self._wall_line_inlier_distance:
|
|
|
+ support += 1
|
|
|
+ if support < self._wall_line_min_support:
|
|
|
+ continue
|
|
|
+ score = support * (0.5 + 0.5 * facing)
|
|
|
+ if best is None or score > best[0]:
|
|
|
+ best = (score, normal)
|
|
|
+ if best is None:
|
|
|
+ return None
|
|
|
+ return self._wrap_to_pi(best[1])
|
|
|
+
|
|
|
+ def _map_pose(self):
|
|
|
+ try:
|
|
|
+ transform = self._tf_buffer.lookup_transform(
|
|
|
+ self._tf_map_frame, self._tf_base_frame, rospy.Time(0),
|
|
|
+ rospy.Duration(self._tf_lookup_timeout),
|
|
|
+ )
|
|
|
+ translation = transform.transform.translation
|
|
|
+ return (
|
|
|
+ translation.x, translation.y,
|
|
|
+ self._yaw_from_quaternion(transform.transform.rotation),
|
|
|
+ ), None
|
|
|
+ except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
|
|
|
+ tf2_ros.ExtrapolationException, tf2_ros.TimeoutException) as error:
|
|
|
+ rospy.logwarn_throttle(5.0, "factory alignment TF pose unavailable: %s", error)
|
|
|
+ if (self._latest_amcl_pose is not None
|
|
|
+ and time.monotonic() - self._latest_amcl_monotonic <= self._amcl_fallback_timeout):
|
|
|
+ return self._latest_amcl_pose, None
|
|
|
+ return None, "MAP_POSE_UNAVAILABLE"
|
|
|
+
|
|
|
+ def _compute_wall_goal(self, standoff_distance=None):
|
|
|
+ standoff = self._wall_standoff if standoff_distance is None else standoff_distance
|
|
|
+ if standoff <= 0.0:
|
|
|
+ return None, "WALL_GOAL_INVALID_STANDOFF"
|
|
|
+ if self._static_map is None:
|
|
|
+ return None, "STATIC_MAP_UNAVAILABLE"
|
|
|
+ pose, failure = self._map_pose()
|
|
|
+ if failure is not None:
|
|
|
+ return None, failure
|
|
|
+ grid = self._static_map
|
|
|
+ ray_hit = self._raycast_wall(grid, pose)
|
|
|
+ if ray_hit is None:
|
|
|
+ return None, "WALL_RAY_NO_HIT"
|
|
|
+ hit_x, hit_y, hit_row, hit_column = ray_hit
|
|
|
+ robot_x, robot_y, heading = pose
|
|
|
+ normal = self._wall_normal_toward_robot(
|
|
|
+ grid, hit_row, hit_column, robot_x, robot_y, heading
|
|
|
+ )
|
|
|
+ if normal is None:
|
|
|
+ return None, "WALL_NORMAL_UNAVAILABLE"
|
|
|
+ goal_x = hit_x + standoff * math.cos(normal)
|
|
|
+ goal_y = hit_y + standoff * math.sin(normal)
|
|
|
+ goal_cell = self._map_to_grid(grid, goal_x, goal_y)
|
|
|
+ if goal_cell is None:
|
|
|
+ return None, "WALL_GOAL_OUTSIDE_MAP"
|
|
|
+ if self._occupied(grid, goal_cell[0], goal_cell[1], self._wall_occupied_threshold):
|
|
|
+ return None, "WALL_GOAL_OCCUPIED"
|
|
|
+ return (goal_x, goal_y, self._wrap_to_pi(normal + math.pi), hit_x, hit_y), None
|
|
|
+
|
|
|
+ def _start_wall_approach(self):
|
|
|
+ computed, failure = self._compute_wall_goal()
|
|
|
+ if failure is not None:
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ self._publish_status(failure)
|
|
|
+ return False
|
|
|
+ if not self._wall_goal_client.wait_for_server(rospy.Duration(self._wall_goal_server_timeout)):
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ self._publish_status("WALL_GOAL_MOVE_BASE_UNAVAILABLE")
|
|
|
+ return False
|
|
|
+ goal_x, goal_y, goal_yaw, hit_x, hit_y = computed
|
|
|
+ goal = MoveBaseGoal()
|
|
|
+ goal.target_pose.header.frame_id = "map"
|
|
|
+ goal.target_pose.header.stamp = rospy.Time.now()
|
|
|
+ goal.target_pose.pose.position.x = goal_x
|
|
|
+ goal.target_pose.pose.position.y = goal_y
|
|
|
+ goal.target_pose.pose.orientation = Quaternion(
|
|
|
+ z=math.sin(goal_yaw / 2.0), w=math.cos(goal_yaw / 2.0)
|
|
|
+ )
|
|
|
+ self._wall_goal_pub.publish(goal.target_pose)
|
|
|
+ self._wall_goal_active = True
|
|
|
+ self._has_control = False
|
|
|
+ self._wall_goal_client.send_goal(goal, done_cb=self._wall_goal_done)
|
|
|
+ self._publish_status(
|
|
|
+ "WALL_GOAL_SENT x=%.3f y=%.3f yaw=%.3f wall_x=%.3f wall_y=%.3f" % (
|
|
|
+ goal_x, goal_y, goal_yaw, hit_x, hit_y
|
|
|
+ )
|
|
|
+ )
|
|
|
+ return True
|
|
|
+
|
|
|
+ def _start_entry_approach(self):
|
|
|
+ # Re-read map -> base_link and cast a fresh ray after OCR confirmation.
|
|
|
+ computed, failure = self._compute_wall_goal(self._entry_standoff)
|
|
|
+ if failure is not None:
|
|
|
+ self._entry_goal_finished = True
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ self._publish_status("ENTRY_GOAL_%s" % failure)
|
|
|
+ return False
|
|
|
+ if not self._wall_goal_client.wait_for_server(rospy.Duration(self._wall_goal_server_timeout)):
|
|
|
+ self._entry_goal_finished = True
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ self._publish_status("ENTRY_GOAL_MOVE_BASE_UNAVAILABLE")
|
|
|
+ return False
|
|
|
+ goal_x, goal_y, goal_yaw, hit_x, hit_y = computed
|
|
|
+ goal = MoveBaseGoal()
|
|
|
+ goal.target_pose.header.frame_id = "map"
|
|
|
+ goal.target_pose.header.stamp = rospy.Time.now()
|
|
|
+ goal.target_pose.pose.position.x = goal_x
|
|
|
+ goal.target_pose.pose.position.y = goal_y
|
|
|
+ goal.target_pose.pose.orientation = Quaternion(
|
|
|
+ z=math.sin(goal_yaw / 2.0), w=math.cos(goal_yaw / 2.0)
|
|
|
+ )
|
|
|
+ self._entry_goal_pub.publish(goal.target_pose)
|
|
|
+ self._entry_goal_active = True
|
|
|
+ self._has_control = False
|
|
|
+ self._wall_goal_client.send_goal(goal, done_cb=self._entry_goal_done)
|
|
|
+ self._publish_status(
|
|
|
+ "ENTRY_GOAL_SENT x=%.3f y=%.3f yaw=%.3f wall_x=%.3f wall_y=%.3f" % (
|
|
|
+ goal_x, goal_y, goal_yaw, hit_x, hit_y
|
|
|
+ )
|
|
|
+ )
|
|
|
+ return True
|
|
|
+
|
|
|
+ def _entry_goal_done(self, state, _result):
|
|
|
+ self._entry_goal_active = False
|
|
|
+ self._entry_goal_finished = True
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ if state == GoalStatus.SUCCEEDED:
|
|
|
+ self._publish_status("FACTORY_ENTRY_COMPLETE type=%s" % (self._factory_type or "UNKNOWN"))
|
|
|
+ if self._require_order:
|
|
|
+ self._completed_order_id = self._current_order["order_id"] if self._current_order else None
|
|
|
+ else:
|
|
|
+ reason = "move_base_state=%d" % state
|
|
|
+ self._publish_status("FACTORY_ENTRY_FAILED %s" % reason)
|
|
|
+
|
|
|
+ def _wall_goal_done(self, state, _result):
|
|
|
+ self._wall_goal_active = False
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ if state != GoalStatus.SUCCEEDED:
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ self._final_alignment_active = False
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._aligned = False
|
|
|
+ self._publish_status("WALL_APPROACH_FAILED move_base_state=%d" % state)
|
|
|
+ return
|
|
|
+
|
|
|
+ # The map-derived goal brings the vehicle to the wall stand-off point.
|
|
|
+ # Re-enable vision there for one final heading correction only.
|
|
|
+ self._wall_goal_finished = False
|
|
|
+ self._final_alignment_active = True
|
|
|
+ self._aligned = False
|
|
|
+ self._scan_state = "ALIGN"
|
|
|
+ self._alignment_started_monotonic = time.monotonic()
|
|
|
+ self._center_frame_count = 0
|
|
|
+ self._processed_sequence = self._detection_sequence
|
|
|
+ self._set_recognition_enabled(True, required=False)
|
|
|
+ self._publish_status("WALL_APPROACH_REFINING_ALIGNMENT")
|
|
|
+
|
|
|
+ def _control_callback(self, _event):
|
|
|
+ if not self._enabled:
|
|
|
+ self._publish_status("DISABLED")
|
|
|
+ return
|
|
|
+ if self._require_order and self._current_order is None:
|
|
|
+ self._publish_status("WAITING_FOR_ORDER")
|
|
|
+ return
|
|
|
+ if (self._require_order
|
|
|
+ and self._armed_order_id != self._current_order.get("order_id")):
|
|
|
+ self._publish_status("WAITING_FOR_START_ORDER")
|
|
|
+ return
|
|
|
+ if self._completed_order_id is not None:
|
|
|
+ return
|
|
|
+ if not self._navigation_seen_active:
|
|
|
+ self._publish_status("WAITING_FOR_NAV_GOAL")
|
|
|
+ return
|
|
|
+ if not self._navigation_succeeded:
|
|
|
+ self._publish_status("NAVIGATING")
|
|
|
+ return
|
|
|
+
|
|
|
+ now = time.monotonic()
|
|
|
+ if self._ocr_active:
|
|
|
+ if now - self._ocr_started_monotonic > self._ocr_timeout:
|
|
|
+ self._ocr_active = False
|
|
|
+ self._ocr_started_monotonic = None
|
|
|
+ self._set_ocr_enabled(False, required=False)
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ self._publish_status("FACTORY_OCR_FAILED timeout")
|
|
|
+ else:
|
|
|
+ self._publish_status("OCR_READING")
|
|
|
+ return
|
|
|
+ if self._wall_goal_active or self._entry_goal_active:
|
|
|
+ return
|
|
|
+ if self._wall_goal_finished or self._entry_goal_finished:
|
|
|
+ return
|
|
|
+ if self._scan_state not in ("ALIGN", "IDLE"):
|
|
|
+ self._run_search_scan(now)
|
|
|
+ return
|
|
|
+ if self._aligned:
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ self._publish_status("ALIGNED")
|
|
|
+ return
|
|
|
+ if now - self._alignment_started_monotonic > self._alignment_timeout:
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ self._publish_status("ALIGNMENT_TIMEOUT")
|
|
|
+ return
|
|
|
+
|
|
|
+ target_fresh = self._visible and now - self._last_detection_monotonic <= self._detection_timeout
|
|
|
+ if not target_fresh:
|
|
|
+ self._center_frame_count = 0
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ command_z = self._publish_smooth_command(0.0, now)
|
|
|
+ self._publish_status("SEARCHING_FACTORY command_z=%+.3f" % command_z)
|
|
|
+ return
|
|
|
+
|
|
|
+ if self._processed_sequence != self._detection_sequence:
|
|
|
+ self._processed_sequence = self._detection_sequence
|
|
|
+ if abs(self._error) <= self._tolerance:
|
|
|
+ self._center_frame_count += 1
|
|
|
+ self._target_angular_z = 0.0
|
|
|
+ if self._center_frame_count >= self._center_frames_required:
|
|
|
+ self._current_angular_z = 0.0
|
|
|
+ self._publish_stop()
|
|
|
+ self._has_control = True
|
|
|
+ if self._wall_approach_enabled and not self._final_alignment_active:
|
|
|
+ self._start_wall_approach()
|
|
|
+ return
|
|
|
+ self._aligned = True
|
|
|
+ self._final_alignment_active = False
|
|
|
+ if self._wall_approach_enabled and self._ocr_confirmation_enabled:
|
|
|
+ self._publish_status("WALL_APPROACH_ALIGNED")
|
|
|
+ # Full-frame OCR subscribes directly to the camera, so the
|
|
|
+ # RKNN locator can be stopped before the OCR attempt.
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ if self._start_ocr_confirmation():
|
|
|
+ return
|
|
|
+ self._wall_goal_finished = True
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ return
|
|
|
+ self._wall_goal_finished = self._wall_approach_enabled
|
|
|
+ self._set_recognition_enabled(False, required=False)
|
|
|
+ self._publish_status("WALL_APPROACH_ALIGNED" if self._wall_approach_enabled else "ALIGNED")
|
|
|
+ return
|
|
|
+ else:
|
|
|
+ self._center_frame_count = 0
|
|
|
+ target = self._angular_sign * self._kp * self._error
|
|
|
+ target = max(-self._max_speed, min(self._max_speed, target))
|
|
|
+ if abs(target) < self._min_speed:
|
|
|
+ target = math.copysign(self._min_speed, target)
|
|
|
+ self._target_angular_z = target
|
|
|
+
|
|
|
+ command_z = self._publish_smooth_command(self._target_angular_z, now)
|
|
|
+ if self._center_frame_count:
|
|
|
+ label = "FINAL_CENTER_FRAME" if self._final_alignment_active else "CENTER_FRAME"
|
|
|
+ self._publish_status("%s %d/%d command_z=%+.3f" % (
|
|
|
+ label, self._center_frame_count, self._center_frames_required, command_z
|
|
|
+ ))
|
|
|
+ else:
|
|
|
+ label = "FINAL_SMOOTH_ALIGN" if self._final_alignment_active else "SMOOTH_ALIGN"
|
|
|
+ self._publish_status("%s error=%+.3f target_z=%+.3f command_z=%+.3f" % (
|
|
|
+ label, self._error, self._target_angular_z, command_z
|
|
|
+ ))
|
|
|
+
|
|
|
+
|
|
|
+if __name__ == "__main__":
|
|
|
+ FactoryAlignmentDemo()
|
|
|
+ rospy.spin()
|
ucar
1 місяць тому