200 lines
8.4 KiB
Python
200 lines
8.4 KiB
Python
import rclpy
|
||
from rclpy.node import Node
|
||
from rm_msgs.msg import DataRef, DataNav, MoveGoal, DataAI
|
||
import threading
|
||
import time
|
||
|
||
SEARCH = 0b00010000
|
||
TRACK = 0b00000000
|
||
SHOOT = 0b00001000
|
||
|
||
FAST_SPEED = 10.0
|
||
SLOW_SPEED = 1.0
|
||
STOP_SPEED = 0.0
|
||
|
||
front = 0.0
|
||
back = 3.14
|
||
left = 1.57
|
||
right = -1.57
|
||
|
||
|
||
class ModeController(Node):
|
||
|
||
def __init__(self):
|
||
super().__init__('mode_controller')
|
||
self.subscription_ref = self.create_subscription(DataRef,'/chassis/data_ref',self.ref_callback,10)
|
||
self.subscription_nav = self.create_subscription(DataNav,'chassis/data_nav',self.nav_callback,10)
|
||
self.subscription_ai = self.create_subscription(DataAI,'gimbal_left/data_ai',self.ai_left_callback,10)
|
||
self.subscription_ai = self.create_subscription(DataAI,'gimbal_right/data_ai',self.ai_right_callback,10)
|
||
self.publisher_ = self.create_publisher(MoveGoal, '/move_goal', 10)
|
||
self.goal_poses = [
|
||
{'x': -0.36, 'y': 0.8}, # 基地补给区（启动点）0
|
||
{'x': -0.43, 'y': 1.5},# 基地补给区（中心点）1
|
||
{'x': 4.82, 'y': -1.62},# 中央增益区（中心点）2
|
||
{'x': 5.32, 'y': -1.62},# 中央增益区（a点）3
|
||
{'x': 4.82, 'y': -1.12},# 中央增益区（b点）4
|
||
{'x': 4.32, 'y': -1.62},# 中央增益区（c点）5
|
||
{'x': 4.82, 'y': -2.12},# 中央增益区（d点）6
|
||
{'x': 1.6, 'y': -2.0},# 快速站模式途径点 7
|
||
{'x': 4.7, 'y': 1.7},# 回血模式途径点 8
|
||
{'x': 4.71, 'y': 1.71},# 回血模式途径点 9
|
||
{'x': -0.359, 'y': 0.81}, # 基地补给区（测试点）10
|
||
{'x': 4.81, 'y': -1.11},# 中央增益区（测试点）11
|
||
]
|
||
self.game_progress = 0 # 4: 比赛开始
|
||
self.remain_hp = 400 # 剩余血量
|
||
self.reached = False # 是否到达目标点
|
||
self.mode = '等待比赛开始' # 模式
|
||
self.goal_index = 0 # 目标点索引
|
||
self.last_goal_pose = None # 上一个目标点
|
||
self.tracker_left = False # 左侧是否有目标
|
||
self.tracker_right = False # 右侧是否有目标
|
||
self.allowance_17mm = 750 # 17mm允许发弹量
|
||
self.countdown_thread = None
|
||
self.publish_goal_pose(10, SLOW_SPEED, front, 0.1, True)
|
||
self.get_logger().info('Mode controller initialized')
|
||
|
||
def ref_callback(self, msg):
|
||
self.game_progress = msg.game_progress # 4: 比赛开始 # 3: 准备比赛
|
||
# self.game_progress = 4
|
||
self.remain_hp = msg.remain_hp # 剩余血量
|
||
self.stage_remain_time = msg.stage_remain_time # 比赛时间
|
||
# self.allowance_17mm = msg.allowance_17mm # 17mm允许发弹量
|
||
self.update_mode()
|
||
return
|
||
|
||
def nav_callback(self, msg):
|
||
self.get_logger().info(f'mode: {self.mode}, goal_index: {self.goal_index}, reached: {msg.reached}')
|
||
self.reached = msg.reached # 是否到达目标点
|
||
#快速站点：
|
||
if self.goal_index == 7 and self.reached and self.mode == '快速占点':
|
||
self.publish_goal_pose(2, FAST_SPEED, front, 0.5, False)
|
||
time.sleep(0.5)
|
||
return
|
||
if self.goal_index == 2 and self.reached and self.mode == '快速占点':
|
||
self.mode = '站点搜敌模式'
|
||
self.publish_goal_pose(3, SLOW_SPEED, front, 0.1, True)
|
||
return
|
||
#站点搜敌模式：
|
||
if self.mode == '站点搜敌模式' :
|
||
if self.tracker_left or self.tracker_right:
|
||
self.publish_goal_pose(self.goal_index, STOP_SPEED, front, 0.1, True)
|
||
return
|
||
else:
|
||
if self.reached:
|
||
self.goal_index = self.goal_index + 1
|
||
if self.goal_index > 6:
|
||
self.goal_index = 3
|
||
self.publish_goal_pose(self.goal_index, SLOW_SPEED, front, 0.2, True)
|
||
return
|
||
else:
|
||
self.publish_goal_pose(self.goal_index, SLOW_SPEED, front, 0.2, True)
|
||
return
|
||
|
||
#站点模式：
|
||
if self.mode == '站点模式' and self.reached:
|
||
self.goal_index = self.goal_index + 1
|
||
if self.goal_index > 6:
|
||
self.goal_index = 3
|
||
self.publish_goal_pose(self.goal_index, SLOW_SPEED, front, 0.1, True)
|
||
return
|
||
|
||
#回血模式：
|
||
if self.goal_index == 8 and self.reached and self.mode == '回血模式':
|
||
self.publish_goal_pose(1, FAST_SPEED, front, 0.1, True)
|
||
return
|
||
if self.goal_index == 1 and self.mode == '回血模式' and self.remain_hp >= 390:
|
||
self.publish_goal_pose(9, FAST_SPEED, front, 0.2, True)
|
||
return
|
||
if self.goal_index == 9 and self.reached and self.mode == '回血模式':
|
||
self.publish_goal_pose(2, FAST_SPEED, front, 0.1, True)
|
||
return
|
||
if self.goal_index == 2 and self.reached and self.mode == '回血模式':
|
||
self.mode = '站点搜敌模式'
|
||
self.publish_goal_pose(3, FAST_SPEED, front, 0.1, True)
|
||
return
|
||
return
|
||
|
||
def ai_left_callback(self, msg):
|
||
if msg.notice == SEARCH:
|
||
self.tracker_left = False
|
||
else:
|
||
self.tracker_left = True
|
||
return
|
||
|
||
def ai_right_callback(self, msg):
|
||
if msg.notice == SEARCH:
|
||
self.tracker_right = False
|
||
else:
|
||
self.tracker_right = True
|
||
return
|
||
|
||
def update_mode(self):
|
||
if self.game_progress == 4:
|
||
if self.mode == '等待比赛开始':
|
||
self.mode = '快速占点'
|
||
self.publish_goal_pose(7, FAST_SPEED, front, 0.4, False)
|
||
return
|
||
if self.remain_hp < 200 and self.mode != '回血模式':
|
||
self.mode = '回血模式'
|
||
self.publish_goal_pose(8, FAST_SPEED, front, 0.5, True)
|
||
return
|
||
elif self.game_progress == 3:
|
||
if self.countdown_thread is None:
|
||
self.countdown_thread = threading.Thread(target=self.start_countdown, args=(self.stage_remain_time,))
|
||
self.countdown_thread.start()
|
||
return
|
||
else:
|
||
self.publish_goal_pose(0, SLOW_SPEED, front, 0.1, False)
|
||
self.get_logger().info('Invalid game progress')
|
||
return
|
||
|
||
def start_countdown(self, countdown_time):
|
||
self.get_logger().info(f'Countdown started: {countdown_time} seconds remaining.')
|
||
while countdown_time > 0:
|
||
self.get_logger().info(f'Time remaining: {countdown_time} seconds.')
|
||
time.sleep(1)
|
||
countdown_time -= 1
|
||
self.mode = '快速占点'
|
||
self.publish_goal_pose(7, FAST_SPEED, front, 0.8, False)
|
||
self.get_logger().info('Countdown finished. Mode changed to 快速占点.')
|
||
return
|
||
|
||
# 发布目标点
|
||
def publish_goal_pose(self, index, max_speed, angle, tolerance, rotor):
|
||
if index < 0 or index >= len(self.goal_poses):
|
||
self.get_logger().error('Invalid index')
|
||
return
|
||
|
||
goal_pose = MoveGoal()
|
||
goal_pose.x = self.goal_poses[index]['x']
|
||
goal_pose.y = self.goal_poses[index]['y']
|
||
goal_pose.angle = angle
|
||
goal_pose.max_speed = max_speed
|
||
goal_pose.tolerance = tolerance
|
||
goal_pose.rotor = rotor
|
||
|
||
# 检查新发布的目标点信息是否与上一次发布的一样
|
||
if self.goal_index > 3 and self.goal_index < 6:
|
||
if self.last_goal_pose and self.last_goal_pose.x == goal_pose.x and self.last_goal_pose.y == goal_pose.y and self.last_goal_pose.angle == goal_pose.angle and self.last_goal_pose.max_speed == goal_pose.max_speed and self.last_goal_pose.tolerance == goal_pose.tolerance and self.last_goal_pose.rotor == goal_pose.rotor:
|
||
# self.get_logger().info('Goal pose is the same as the last one, not publishing.')
|
||
return
|
||
if self.goal_index == 7 or self.goal_index == 0:
|
||
self.publisher_.publish(goal_pose)
|
||
time.sleep(0.5)
|
||
|
||
self.publisher_.publish(goal_pose)
|
||
self.goal_index = index
|
||
self.last_goal_pose = goal_pose
|
||
self.get_logger().info(f'Published goal pose: {goal_pose}')
|
||
return
|
||
|
||
def main(args=None):
|
||
rclpy.init(args=args)
|
||
mode_controller = ModeController()
|
||
rclpy.spin(mode_controller)
|
||
mode_controller.destroy_node()
|
||
rclpy.shutdown()
|
||
|
||
if __name__ == '__main__':
|
||
main() |