fix: gemini complete

myrobot_pkg/scripts/controller.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import rospy
+import actionlib
+import threading
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from geometry_msgs.msg import PoseStamped, PointStamped
+
+class MissionController:
+    def __init__(self):
+        rospy.init_node('nav_controller')
+
+        # --- 資料結構 ---
+        self.waypoint_queue = [] 
+        self.current_goal_idx = 0
+
+        # --- 連接導航系統 ---
+        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
+        rospy.loginfo("正在連接 Move Base Server...")
+        self.client.wait_for_server()
+        rospy.loginfo("導航系統已連線！")
+
+        # --- [新增] 從 Parameter Server 讀取路徑點 ---
+        self.load_waypoints_from_param()
+
+        # 訂閱 RViz 點擊 (保留功能，讓你可以手動加點)
+        rospy.Subscriber('/clicked_point', PointStamped, self.click_callback)
+
+        # 啟動主執行緒
+        self.monitor_thread = threading.Thread(target=self.mission_loop)
+        self.monitor_thread.start()
+
+    def load_waypoints_from_param(self):
+        """從 launch 檔讀取路徑點陣列"""
+        # 讀取私有參數 '~waypoints'，如果沒設定則回傳空串列
+        point_list = rospy.get_param('~waypoints', [])
+
+        if point_list:
+            rospy.loginfo(f"讀取到 {len(point_list)} 個預設路徑點，準備執行...")
+            for p in point_list:
+                # 假設格式是 [[x1, y1], [x2, y2], ...]
+                x, y = p[0], p[1]
+                self.add_waypoint(x, y)
+        else:
+            rospy.logwarn("未檢測到預設路徑點，請在 RViz 中點擊或檢查 Launch 檔。")
+
+    def add_waypoint(self, x, y):
+        """將 x, y 轉換為 PoseStamped 並加入隊列"""
+        target_pose = PoseStamped()
+        target_pose.header.frame_id = "map"
+        target_pose.header.stamp = rospy.Time.now()
+        target_pose.pose.position.x = x
+        target_pose.pose.position.y = y
+        target_pose.pose.orientation.w = 1.0 # 預設朝向前方
+        
+        self.waypoint_queue.append(target_pose)
+        rospy.loginfo(f"已加入路徑點: ({x}, {y})")
+
+    def click_callback(self, msg):
+        """RViz 點擊回調"""
+        self.add_waypoint(msg.point.x, msg.point.y)
+
+    def execute_navigation(self, target_pose):
+        """發送導航指令"""
+        goal = MoveBaseGoal()
+        goal.target_pose = target_pose
+        self.client.send_goal(goal)
+        
+        # 等待到達結果
+        self.client.wait_for_result()
+        return self.client.get_state() == actionlib.GoalStatus.SUCCEEDED
+
+    def mission_loop(self):
+        """主迴圈：依序執行隊列中的點"""
+        rate = rospy.Rate(2) # 2Hz
+        
+        # 等待一小段時間確保系統穩定
+        rospy.sleep(1.0)
+
+        while not rospy.is_shutdown():
+            if self.current_goal_idx < len(self.waypoint_queue):
+                target = self.waypoint_queue[self.current_goal_idx]
+                
+                rospy.loginfo(f"--- 開始前往第 {self.current_goal_idx + 1} 個點 ---")
+                success = self.execute_navigation(target)
+                
+                if success:
+                    rospy.loginfo("到達目標！")
+                else:
+                    rospy.logwarn("導航失敗或受阻，跳至下一點。")
+                
+                self.current_goal_idx += 1
+            else:
+                # 跑完所有點後，在這裡空轉等待新指令
+                rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        MissionController()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass