103 lines
3.6 KiB
Python
103 lines
3.6 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
import rospy
|
|
import actionlib
|
|
import threading
|
|
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
|
|
from geometry_msgs.msg import PoseStamped, PointStamped
|
|
|
|
class MissionController:
|
|
def __init__(self):
|
|
rospy.init_node('nav_controller')
|
|
|
|
# --- 資料結構 ---
|
|
self.waypoint_queue = []
|
|
self.current_goal_idx = 0
|
|
|
|
# --- 連接導航系統 ---
|
|
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
|
|
rospy.loginfo("正在連接 Move Base Server...")
|
|
self.client.wait_for_server()
|
|
rospy.loginfo("導航系統已連線！")
|
|
|
|
# --- [新增] 從 Parameter Server 讀取路徑點 ---
|
|
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)} 個預設路徑點，準備執行...")
|
|
for p in point_list:
|
|
# 假設格式是 [[x1, y1], [x2, y2], ...]
|
|
x, y = p[0], p[1]
|
|
self.add_waypoint(x, y)
|
|
else:
|
|
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 # 預設朝向前方
|
|
|
|
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)
|
|
|
|
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)
|
|
|
|
while not rospy.is_shutdown():
|
|
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} 個點 ---")
|
|
success = self.execute_navigation(target)
|
|
|
|
if success:
|
|
rospy.loginfo("到達目標！")
|
|
else:
|
|
rospy.logwarn("導航失敗或受阻，跳至下一點。")
|
|
|
|
self.current_goal_idx += 1
|
|
else:
|
|
# 跑完所有點後，在這裡空轉等待新指令
|
|
rate.sleep()
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
MissionController()
|
|
rospy.spin()
|
|
except rospy.ROSInterruptException:
|
|
pass |