feat: force stop

myrobot_pkg/scripts/controller.py

@@ -5,95 +5,120 @@ import rospy
 import actionlib
 import threading
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
-from geometry_msgs.msg import PoseStamped, PointStamped
+from geometry_msgs.msg import PoseStamped, PointStamped, Twist
 
 class MissionController:
     def __init__(self):
         rospy.init_node('nav_controller')
 
-        # --- 資料結構 ---
+        # --- 設定 ---
         self.waypoint_queue = [] 
         self.current_goal_idx = 0
+        self.mission_complete = False # 任務完成旗標
 
-        # --- 連接導航系統 ---
+        # --- ROS 通訊 ---
+        # 1. 速度控制 (用來強制煞車)
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
+        
+        # 2. 訂閱 RViz 點擊 (保留手動加點功能)
+        rospy.Subscriber('/clicked_point', PointStamped, self.click_callback)
+
+        # 3. 連接導航系統
         self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
         rospy.loginfo("正在連接 Move Base Server...")
         self.client.wait_for_server()
         rospy.loginfo("導航系統已連線！")
 
-        # --- [新增] 從 Parameter Server 讀取路徑點 ---
+        # --- 從 Launch 檔讀取參數 ---
         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)} 個預設路徑點，準備執行...")
+            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)
+                self.add_waypoint(p[0], p[1])
         else:
-            rospy.logwarn("未檢測到預設路徑點，請在 RViz 中點擊或檢查 Launch 檔。")
+            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 # 預設朝向前方
+        # 設定 orientation w=1.0 (朝向地圖前方)，避免機器人到點後瘋狂旋轉找角度
+        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)
+        # 如果任務已經結束，允許手動點擊後重新開始
+        if self.mission_complete:
+            self.mission_complete = False
+
+    def stop_robot(self):
+        """發送 0 速度指令強制停下"""
+        stop_msg = Twist()
+        stop_msg.linear.x = 0
+        stop_msg.angular.z = 0
+        self.cmd_vel_pub.publish(stop_msg)
 
     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)
+        rate = rospy.Rate(5) # 5Hz
+        rospy.sleep(1.0) # 等待系統穩定
 
         while not rospy.is_shutdown():
+            # 情況 A: 還有點沒跑完
             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}/{len(self.waypoint_queue)} 個點 ---")
                 
-                rospy.loginfo(f"--- 開始前往第 {self.current_goal_idx + 1} 個點 ---")
                 success = self.execute_navigation(target)
                 
                 if success:
                     rospy.loginfo("到達目標！")
                 else:
-                    rospy.logwarn("導航失敗或受阻，跳至下一點。")
+                    rospy.logwarn("導航受阻，跳至下一個點。")
                 
                 self.current_goal_idx += 1
+
+            # 情況 B: 全部跑完
             else:
-                # 跑完所有點後，在這裡空轉等待新指令
-                rate.sleep()
+                if not self.mission_complete and len(self.waypoint_queue) > 0:
+                    rospy.loginfo(">>> 所有路徑點執行完畢，停止機器人。")
+                    
+                    # 1. 取消任何殘留的導航目標
+                    self.client.cancel_all_goals()
+                    
+                    # 2. 強制發送停止指令幾次，確保它不動
+                    for _ in range(5):
+                        self.stop_robot()
+                        rospy.sleep(0.1)
+                        
+                    self.mission_complete = True
+                
+                # 持續發送停止訊號，防止滑動或旋轉 (Optional)
+                if self.mission_complete:
+                    # 如果想要它完全不動，可以在這裡持續發 0 速度
+                    # self.stop_robot() 
+                    pass
+
+            rate.sleep()
 
 if __name__ == '__main__':
     try: