From ddf60c1fa30f5eac9d8b8f6d3664d738f867cefc Mon Sep 17 00:00:00 2001 From: joyHuang Date: Wed, 10 Dec 2025 23:37:06 +0900 Subject: [PATCH] feat: add schedule vision --- myrobot_pkg/scripts/controller.py | 125 ++++++++++++++++++++---------- 1 file changed, 83 insertions(+), 42 deletions(-) diff --git a/myrobot_pkg/scripts/controller.py b/myrobot_pkg/scripts/controller.py index 1435dda..d8626a1 100644 --- a/myrobot_pkg/scripts/controller.py +++ b/myrobot_pkg/scripts/controller.py @@ -5,7 +5,8 @@ import rospy import actionlib import threading from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped, PointStamped, Twist +from geometry_msgs.msg import PoseStamped, PointStamped, Twist, Point +from visualization_msgs.msg import Marker, MarkerArray class MissionController: def __init__(self): @@ -14,37 +15,38 @@ class MissionController: # --- 設定 --- self.waypoint_queue = [] self.current_goal_idx = 0 - self.mission_complete = False # 任務完成旗標 + self.mission_complete = False - # --- ROS 通訊 --- - # 1. 速度控制 (用來強制煞車) + # --- ROS Publisher --- + # 1. 速度控制 (強制停車用) self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) - - # 2. 訂閱 RViz 點擊 (保留手動加點功能) + # 2. [新增] 視覺化標記 (用來畫點跟線) + self.marker_pub = rospy.Publisher('/waypoint_markers', MarkerArray, queue_size=10) + + # --- ROS Subscriber --- 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("導航系統已連線！") - # --- 從 Launch 檔讀取參數 --- + # --- 讀取參數並啟動 --- 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)} 個預設路徑點，準備執行。") + rospy.loginfo(f"讀取到 {len(point_list)} 個路徑點。") for p in point_list: self.add_waypoint(p[0], p[1]) else: - rospy.logwarn("未讀取到路徑點參數，請使用 RViz 點擊或檢查 Launch 檔。") + rospy.logwarn("無預設路徑點。") def add_waypoint(self, x, y): target_pose = PoseStamped() @@ -52,23 +54,73 @@ class MissionController: target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = x target_pose.pose.position.y = y - # 設定 orientation w=1.0 (朝向地圖前方)，避免機器人到點後瘋狂旋轉找角度 target_pose.pose.orientation.w = 1.0 self.waypoint_queue.append(target_pose) - rospy.loginfo(f"已加入路徑點: ({x}, {y})") + rospy.loginfo(f"加入點: ({x}, {y})") + + # 每次加點都更新 RViz 畫面 + self.publish_markers() def click_callback(self, msg): - self.add_waypoint(msg.point.x, msg.point.y) - # 如果任務已經結束，允許手動點擊後重新開始 if self.mission_complete: - self.mission_complete = False + self.mission_complete = False # 重置狀態允許新任務 + self.current_goal_idx = 0 + 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 + marker_array.markers.append(delete_all) + 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 + + # 只畫出「還沒到達」的點 + 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.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 + 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 - stop_msg.angular.z = 0 self.cmd_vel_pub.publish(stop_msg) def execute_navigation(self, target_pose): @@ -79,44 +131,33 @@ class MissionController: return self.client.get_state() == actionlib.GoalStatus.SUCCEEDED def mission_loop(self): - rate = rospy.Rate(5) # 5Hz - rospy.sleep(1.0) # 等待系統穩定 + rate = rospy.Rate(5) + rospy.sleep(1.0) while not rospy.is_shutdown(): - # 情況 A: 還有點沒跑完 if self.current_goal_idx < len(self.waypoint_queue): + # 更新 RViz 顯示 (讓已到達的點消失) + self.publish_markers() + target = self.waypoint_queue[self.current_goal_idx] - rospy.loginfo(f"--- 前往第 {self.current_goal_idx + 1}/{len(self.waypoint_queue)} 個點 ---") + rospy.loginfo(f"--- 前往 P{self.current_goal_idx + 1} ---") success = self.execute_navigation(target) if success: - rospy.loginfo("到達目標！") - else: - rospy.logwarn("導航受阻，跳至下一個點。") + rospy.loginfo(f"P{self.current_goal_idx + 1} 到達！") self.current_goal_idx += 1 - - # 情況 B: 全部跑完 + else: if not self.mission_complete and len(self.waypoint_queue) > 0: - rospy.loginfo(">>> 所有路徑點執行完畢，停止機器人。") - - # 1. 取消任何殘留的導航目標 + rospy.loginfo(">>> 任務全部完成，停止。") + self.publish_markers() # 清除所有標記 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()