收尾專案：打造一台自主導航機器人

第 4 章 URDF/xacro/TF2 ──→ 機器人模型與座標系統（所有模組的共同地基）
      │
第 5 章 Gazebo ──→ 模擬環境、感測器資料來源
      │
      ├──→ 第 8 章 YOLO 物體偵測 ──→ 發布 /detections
      │         │
      │         ▼
      │    偵測到目標物體？
      │         │ 是
      │         ▼
第 6 章 Nav2 ──→ 中斷巡邏、規劃前往物體位置的路徑 ──→ 執行導航
      │
第 9 章 Lifecycle Node ──→ 統一管理巡邏控制器的啟用/停用狀態

import rclpy
from rclpy.lifecycle import Node as LifecycleNode
from rclpy.lifecycle import State, TransitionCallbackReturn
from rclpy.action import ActionClient
from vision_msgs.msg import Detection2DArray
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped


PATROL_WAYPOINTS = [
    (1.0, 0.0),
    (1.0, 2.0),
    (-1.0, 2.0),
    (-1.0, 0.0),
]

TARGET_CLASS = "red_box"


class PatrolController(LifecycleNode):
    def __init__(self):
        super().__init__("patrol_controller")
        self.nav_client = None
        self.detection_sub = None
        self.mode = "idle"
        self.waypoint_index = 0

    def on_configure(self, state: State) -> TransitionCallbackReturn:
        self.nav_client = ActionClient(self, NavigateToPose, "navigate_to_pose")
        self.detection_sub = self.create_subscription(
            Detection2DArray, "/detections", self.on_detections, 10
        )
        self.get_logger().info("配置完成，等待啟用")
        return TransitionCallbackReturn.SUCCESS

    def on_activate(self, state: State) -> TransitionCallbackReturn:
        self.mode = "patrolling"
        self.send_next_waypoint()
        return super().on_activate(state)

    def on_deactivate(self, state: State) -> TransitionCallbackReturn:
        self.mode = "idle"
        return super().on_deactivate(state)

    def on_detections(self, msg: Detection2DArray):
        if self.mode != "patrolling":
            return

        for detection in msg.detections:
            for result in detection.results:
                if result.hypothesis.class_id == TARGET_CLASS and \
                   result.hypothesis.score > 0.7:
                    self.get_logger().info(
                        f"偵測到目標物體，信心分數 {result.hypothesis.score:.2f}，中斷巡邏"
                    )
                    self.mode = "approaching_target"
                    self.send_goal_to_detected_object(detection)
                    return

    def send_next_waypoint(self):
        if self.mode != "patrolling":
            return
        x, y = PATROL_WAYPOINTS[self.waypoint_index]
        self.waypoint_index = (self.waypoint_index + 1) % len(PATROL_WAYPOINTS)
        self.send_nav_goal(x, y, on_result=self.send_next_waypoint)

    def send_goal_to_detected_object(self, detection):
        estimated_x, estimated_y = self.estimate_object_position(detection)
        self.send_nav_goal(estimated_x, estimated_y, on_result=self.on_reached_target)

    def send_nav_goal(self, x, y, on_result):
        self.nav_client.wait_for_server()
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = PoseStamped()
        goal_msg.pose.header.frame_id = "map"
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.orientation.w = 1.0

        future = self.nav_client.send_goal_async(goal_msg)
        future.add_done_callback(
            lambda f: self._on_goal_response(f, on_result)
        )

    def _on_goal_response(self, future, on_result):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().warn("導航目標被拒絕")
            return
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(lambda f: on_result())

    def on_reached_target(self):
        self.get_logger().info("已抵達目標物體位置，發布通知")
        self.mode = "patrolling"
        self.send_next_waypoint()

    def estimate_object_position(self, detection):
        return (0.5, 1.0)


def main():
    rclpy.init()
    node = PatrolController()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

[INFO] [patrol_controller]: 偵測到目標物體，信心分數 0.84，中斷巡邏
[INFO] [patrol_controller]: 已抵達目標物體位置，發布通知