diff --git a/myrobot_pkg/scripts/controller.py b/myrobot_pkg/scripts/controller.py index d8626a1..d2246ea 100644 --- a/myrobot_pkg/scripts/controller.py +++ b/myrobot_pkg/scripts/controller.py @@ -5,7 +5,7 @@ import rospy import actionlib import threading from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped, PointStamped, Twist, Point +from geometry_msgs.msg import PoseStamped, PointStamped, Twist from visualization_msgs.msg import Marker, MarkerArray class MissionController: @@ -18,9 +18,9 @@ class MissionController: self.mission_complete = False # --- ROS Publisher --- - # 1. 速度控制 (強制停車用) + # 1. 速度控制 (強制停車用) - 提高 queue_size 確保指令送達 self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) - # 2. [新增] 視覺化標記 (用來畫點跟線) + # 2. 視覺化標記 self.marker_pub = rospy.Publisher('/waypoint_markers', MarkerArray, queue_size=10) # --- ROS Subscriber --- @@ -32,7 +32,7 @@ class MissionController: self.client.wait_for_server() rospy.loginfo("導航系統已連線！") - # --- 讀取參數並啟動 --- + # --- 讀取參數 --- self.load_waypoints_from_param() # 啟動監控執行緒 @@ -58,23 +58,20 @@ class MissionController: self.waypoint_queue.append(target_pose) rospy.loginfo(f"加入點: ({x}, {y})") - - # 每次加點都更新 RViz 畫面 self.publish_markers() def click_callback(self, msg): + # 如果任務已結束，允許手動點擊來重置並開始新任務 if self.mission_complete: - self.mission_complete = False # 重置狀態允許新任務 + 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): - """在 RViz 畫出剩下的路徑點與連線""" marker_array = MarkerArray() - - # 如果沒有點，清空畫面並返回 if self.current_goal_idx >= len(self.waypoint_queue): delete_all = Marker() delete_all.action = Marker.DELETEALL @@ -82,45 +79,38 @@ class MissionController: self.marker_pub.publish(marker_array) return - # 1. 設定連線 (LINE_STRIP) - 顯示點到點的路徑 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 = 0.0 - line_marker.color.g = 0.0 - line_marker.color.b = 1.0 # 藍色線 - line_marker.color.a = 0.8 + 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) - - # 2. 設定文字編號 (TEXT_VIEW_FACING) + 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 = p.x - text_marker.pose.position.y = p.y - text_marker.pose.position.z = 0.5 # 字浮在半空中 + 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 = 1.0 - text_marker.color.g = 0.0 - text_marker.color.b = 0.0 # 紅色字 - text_marker.color.a = 1.0 + 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) def stop_robot(self): + """持續發送速度 0 的指令""" stop_msg = Twist() + stop_msg.linear.x = 0.0 + stop_msg.linear.y = 0.0 + stop_msg.angular.z = 0.0 self.cmd_vel_pub.publish(stop_msg) def execute_navigation(self, target_pose): @@ -131,14 +121,14 @@ class MissionController: return self.client.get_state() == actionlib.GoalStatus.SUCCEEDED def mission_loop(self): - rate = rospy.Rate(5) + # 提高頻率到 10Hz，讓煞車指令更密集 + rate = rospy.Rate(10) rospy.sleep(1.0) while not rospy.is_shutdown(): if self.current_goal_idx < len(self.waypoint_queue): - # 更新 RViz 顯示 (讓已到達的點消失) + # --- 導航中 --- self.publish_markers() - target = self.waypoint_queue[self.current_goal_idx] rospy.loginfo(f"--- 前往 P{self.current_goal_idx + 1} ---") @@ -146,18 +136,26 @@ class MissionController: if success: rospy.loginfo(f"P{self.current_goal_idx + 1} 到達！") + else: + rospy.logwarn("導航受阻，跳過此點。") self.current_goal_idx += 1 else: - if not self.mission_complete and len(self.waypoint_queue) > 0: - rospy.loginfo(">>> 任務全部完成，停止。") - self.publish_markers() # 清除所有標記 - self.client.cancel_all_goals() - for _ in range(5): - self.stop_robot() - rospy.sleep(0.1) - self.mission_complete = True + # --- 任務結束 --- + if len(self.waypoint_queue) > 0: + if not self.mission_complete: + rospy.loginfo(">>> 任務全部完成，執行強制停車。") + self.publish_markers() # 清除標記 + + # 1. 告訴 Move Base 取消所有目標 (不要再嘗試對齊方向) + self.client.cancel_all_goals() + self.mission_complete = True + + # 2. 【關鍵修改】在迴圈內持續呼叫 stop_robot + # 只要任務結束，每一輪迴圈都會發送速度0 + # 這會強制覆蓋掉任何滑行或旋轉的殘留指令 + self.stop_robot() rate.sleep()