fix:stop loop
This commit is contained in:
parent
ddf60c1fa3
commit
348e7b4691
GPG Key ID:
FFD99A1CA77D33C8
@ -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()
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user