joy/ROS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add schedule vision

Browse Source
This commit is contained in:
joyHuang 2025-12-10 23:37:06 +09:00
parent 9825a32e2b
commit ddf60c1fa3
Signed by: joy
GPG Key ID: FFD99A1CA77D33C8
1 changed files with 83 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
myrobot_pkg/scripts/controller.py
View File

@ -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()