From ba65fee28a9be46cd5ec8273f7a43f18538a1c01 Mon Sep 17 00:00:00 2001 From: joyHuang Date: Wed, 10 Dec 2025 23:55:58 +0900 Subject: [PATCH] . --- myrobot_pkg/launch/xacro.launch | 5 +- myrobot_pkg/scripts/controller.py | 104 +++++++++--------------------- 2 files changed, 32 insertions(+), 77 deletions(-) diff --git a/myrobot_pkg/launch/xacro.launch b/myrobot_pkg/launch/xacro.launch index 008f342..b57e081 100644 --- a/myrobot_pkg/launch/xacro.launch +++ b/myrobot_pkg/launch/xacro.launch @@ -34,8 +34,9 @@ - - + + + diff --git a/myrobot_pkg/scripts/controller.py b/myrobot_pkg/scripts/controller.py index d2246ea..2ec59ff 100644 --- a/myrobot_pkg/scripts/controller.py +++ b/myrobot_pkg/scripts/controller.py @@ -1,6 +1,3 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - import rospy import actionlib import threading @@ -12,41 +9,31 @@ class MissionController: def __init__(self): rospy.init_node('nav_controller') - # --- 設定 --- self.waypoint_queue = [] self.current_goal_idx = 0 self.mission_complete = False - # --- ROS Publisher --- - # 1. 速度控制 (強制停車用) - 提高 queue_size 確保指令送達 - self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) - # 2. 視覺化標記 + # 提高發布頻率與佇列大小，確保煞車指令優先權 + self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=100) self.marker_pub = rospy.Publisher('/waypoint_markers', MarkerArray, queue_size=10) - # --- ROS Subscriber --- rospy.Subscriber('/clicked_point', PointStamped, self.click_callback) - # --- 導航客戶端 --- self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - rospy.loginfo("正在連接 Move Base Server...") + rospy.loginfo("Waiting for move_base...") self.client.wait_for_server() - rospy.loginfo("導航系統已連線！") + rospy.loginfo("Connected!") - # --- 讀取參數 --- self.load_waypoints_from_param() - # 啟動監控執行緒 self.monitor_thread = threading.Thread(target=self.mission_loop) self.monitor_thread.start() def load_waypoints_from_param(self): point_list = rospy.get_param('~waypoints', []) if point_list: - rospy.loginfo(f"讀取到 {len(point_list)} 個路徑點。") for p in point_list: self.add_waypoint(p[0], p[1]) - else: - rospy.logwarn("無預設路徑點。") def add_waypoint(self, x, y): target_pose = PoseStamped() @@ -57,59 +44,29 @@ class MissionController: target_pose.pose.orientation.w = 1.0 self.waypoint_queue.append(target_pose) - rospy.loginfo(f"加入點: ({x}, {y})") + rospy.loginfo(f"Add waypoint: ({x}, {y})") self.publish_markers() def click_callback(self, msg): - # 如果任務已結束，允許手動點擊來重置並開始新任務 if self.mission_complete: - rospy.loginfo("檢測到新輸入，重置任務狀態...") self.mission_complete = False self.current_goal_idx = 0 - self.waypoint_queue = [] # 清空舊的 + self.waypoint_queue = [] self.add_waypoint(msg.point.x, msg.point.y) def publish_markers(self): - marker_array = MarkerArray() - if self.current_goal_idx >= len(self.waypoint_queue): - delete_all = Marker() - delete_all.action = Marker.DELETEALL - marker_array.markers.append(delete_all) - self.marker_pub.publish(marker_array) - return - - line_marker = Marker() - line_marker.header.frame_id = "map" - line_marker.type = Marker.LINE_STRIP - line_marker.action = Marker.ADD - line_marker.id = 999 - line_marker.scale.x = 0.05 - line_marker.color.r, line_marker.color.g, line_marker.color.b, line_marker.color.a = 0.0, 0.0, 1.0, 0.8 - - for i in range(self.current_goal_idx, len(self.waypoint_queue)): - p = self.waypoint_queue[i].pose.position - line_marker.points.append(p) - - text_marker = Marker() - text_marker.header.frame_id = "map" - text_marker.type = Marker.TEXT_VIEW_FACING - text_marker.action = Marker.ADD - text_marker.id = i - text_marker.pose.position.x, text_marker.pose.position.y, text_marker.pose.position.z = p.x, p.y, 0.5 - text_marker.text = f"P{i+1}" - text_marker.scale.z = 0.3 - text_marker.color.r, text_marker.color.g, text_marker.color.b, text_marker.color.a = 1.0, 0.0, 0.0, 1.0 - marker_array.markers.append(text_marker) - - marker_array.markers.append(line_marker) - self.marker_pub.publish(marker_array) + # (視覺化程式碼保持不變，為節省篇幅省略，請保留您原本的視覺化代碼) + pass def stop_robot(self): - """持續發送速度 0 的指令""" + """暴力煞車：發送全 0 速度""" stop_msg = Twist() stop_msg.linear.x = 0.0 stop_msg.linear.y = 0.0 + stop_msg.linear.z = 0.0 + stop_msg.angular.x = 0.0 + stop_msg.angular.y = 0.0 stop_msg.angular.z = 0.0 self.cmd_vel_pub.publish(stop_msg) @@ -117,44 +74,41 @@ class MissionController: 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 + + # 只要不是 ABORTED (失敗)，我們都視為成功，讓它繼續下一點 + # 這樣可以避免因為一點點誤差卡在原地 + state = self.client.get_state() + return state in [actionlib.GoalStatus.SUCCEEDED, actionlib.GoalStatus.PREEMPTED] def mission_loop(self): - # 提高頻率到 10Hz，讓煞車指令更密集 - rate = rospy.Rate(10) + rate = rospy.Rate(10) # 10Hz rospy.sleep(1.0) while not rospy.is_shutdown(): + # 1. 執行導航 if self.current_goal_idx < len(self.waypoint_queue): - # --- 導航中 --- - self.publish_markers() target = self.waypoint_queue[self.current_goal_idx] - rospy.loginfo(f"--- 前往 P{self.current_goal_idx + 1} ---") + rospy.loginfo(f"Go to Waypoint {self.current_goal_idx + 1}") - success = self.execute_navigation(target) - - if success: - rospy.loginfo(f"P{self.current_goal_idx + 1} 到達！") - else: - rospy.logwarn("導航受阻，跳過此點。") + self.execute_navigation(target) + # 不管結果如何，強制切換到下一點 (避免卡死) + # 如果你想確保一定到達，可以加判斷，但容易卡住 self.current_goal_idx += 1 + # 2. 任務結束 else: - # --- 任務結束 --- if len(self.waypoint_queue) > 0: if not self.mission_complete: - rospy.loginfo(">>> 任務全部完成，執行強制停車。") - self.publish_markers() # 清除標記 - - # 1. 告訴 Move Base 取消所有目標 (不要再嘗試對齊方向) + rospy.loginfo("Mission Complete! Stopping robot.") + # 【關鍵】取消所有 MoveBase 任務，讓它停止規劃 self.client.cancel_all_goals() self.mission_complete = True - # 2. 【關鍵修改】在迴圈內持續呼叫 stop_robot - # 只要任務結束，每一輪迴圈都會發送速度0 - # 這會強制覆蓋掉任何滑行或旋轉的殘留指令 + # 【關鍵】持續發送停止指令，覆蓋掉任何滑行或殘留命令 self.stop_robot() rate.sleep()