#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import rospy
import time
import cv2
import numpy as np
import os
import math
import re
from tta_m3e_rtsp.srv import takeoffOrLanding, gimbalControl
from tta_m3e_rtsp.msg import flightByVel
##########################################################################################################################################################
##########################################################################################################################################################
# 初始化服务进程
class UAVController:
############################## 记得改IP ######################
def __init__(self, ip_address='192.168.58.7'):
rospy.init_node('uav_python_controller')
# 初始化服务代理
rospy.wait_for_service('takeoffOrLanding')
rospy.wait_for_service('gimbalControl')
self.takeoff_landing_srv = rospy.ServiceProxy('takeoffOrLanding', takeoffOrLanding)
self.gimbal_control_srv = rospy.ServiceProxy('gimbalControl', gimbalControl)
# 初始化速度控制发布者
self.vel_pub = rospy.Publisher('flightByVel', flightByVel, queue_size=10)
# 视频流相关参数
self.ip_address = ip_address
self.rtsp_url = f"rtsp://{self.ip_address}:8554/live"
self.cap = None
self.video_active = False
# 微调参数
self.adjustment_threshold = 0.90 # 这是降落基准点
self.micro_adjust_speed = 0.1 # 这是微调移动速度
self.micro_adjust_time = 0.2 # 这是微调移动时间
self.descend_speed = 0.05 # 这是微调下降速度
# 图片保存路径
self.fruits_dir = "/home/forlinx/Pictures/Fruits"
self.land_dir = "/home/forlinx/Pictures/Land"
self.lines_dir = "/home/forlinx/Pictures/Lines"
# 记录上一次降落的水果类型
self.last_fruit_type = None
# 创建必要的目录
os.makedirs(self.fruits_dir, exist_ok=True)
os.makedirs(self.land_dir, exist_ok=True)
os.makedirs(self.lines_dir, exist_ok=True)
rospy.loginfo("UAV控制器初始化完成")
##########################################################################################################################################################
##########################################################################################################################################################
# 起飞
def takeoff(self, retries=3, interval=4):
"""带重试机制的起飞功能"""
rospy.loginfo("等待10秒后起飞...")
rospy.sleep(10)
for attempt in range(retries):
try:
resp = self.takeoff_landing_srv(1)
if resp.ack == 1:
rospy.loginfo("起飞成功")
return True
else:
rospy.logwarn(f"起飞失败，第{attempt+1}次重试...")
rospy.sleep(interval)
except rospy.ServiceException as e:
rospy.logerr(f"起飞服务调用失败: {str(e)}")
rospy.sleep(interval)
rospy.logerr("起飞失败，请重启无人机")
return False
##########################################################################################################################################################
# 降落
def landing(self):
"""发送降落指令"""
try:
resp = self.takeoff_landing_srv(2) # 2表示降落
rospy.loginfo("降落指令发送成功，响应: %d", resp.ack)
return resp.ack == 1
except rospy.ServiceException as e:
rospy.logerr("降落服务调用失败: %s", str(e))
return False
##########################################################################################################################################################
# 移动
def move_by_velocity(self, vel_x, vel_y, vel_z, fly_time=2.0, target_yaw=0):
"""通过速度控制无人机移动"""
msg = flightByVel()
msg.vel_x = vel_x
msg.vel_y = vel_y
msg.vel_z = vel_z
msg.fly_time = fly_time
msg.targetYaw = target_yaw
self.vel_pub.publish(msg)
rospy.loginfo("速度控制指令已发送: X=%.2f, Y=%.2f, Z=%.2f", vel_x, vel_y, vel_z)
##########################################################################################################################################################
# 云台控制
def control_gimbal(self, pitch, roll=0, yaw=0):
"""控制云台角度"""
try:
resp = self.gimbal_control_srv(pitch, roll, yaw)
rospy.loginfo("云台控制成功，角度: pitch=%.2f", pitch)
return resp.ack == 1
except rospy.ServiceException as e:
rospy.logerr("云台控制失败: %s", str(e))
return False
##########################################################################################################################################################
# 云台校准与向右偏转
def gimbal_calibration_right_task(self):
"""云台校准任务(向右偏转)"""
rospy.loginfo("开始云台校准任务(向右)")
for _ in range(2):
# 无人机右转45°
self.move_by_velocity(0, 0, 0, 2.0, 45)
rospy.sleep(2)
# 云台向上35°
self.control_gimbal(35)
rospy.sleep(2)
# 云台回中
self.control_gimbal(0)
rospy.sleep(2)
rospy.loginfo("云台校准任务(向右)完成")
##########################################################################################################################################################
#云台校准与向左偏转
def gimbal_calibration_left_task(self):
"""云台校准任务(向左偏转)"""
rospy.loginfo("开始云台校准任务(向左)")
for _ in range(2):
# 无人机左转45°
self.move_by_velocity(0, 0, 0, 2.0, -45)
rospy.sleep(2)
# 云台向上35°
self.control_gimbal(35)
rospy.sleep(2)
# 云台回中
self.control_gimbal(0)
rospy.sleep(2)
rospy.loginfo("云台校准任务(向左)完成")
##########################################################################################################################################################
# 启动视频流
def start_video_stream(self):
"""启动视频流"""
if not self.video_active:
self.cap = cv2.VideoCapture(self.rtsp_url, cv2.CAP_FFMPEG)
if not self.cap.isOpened():
rospy.logerr("无法打开摄像头，请检查IP地址或网络连接")
return False
self.video_active = True
rospy.loginfo("视频流已启动")
return True
##########################################################################################################################################################
# 关闭视频流
def stop_video_stream(self):
"""停止视频流"""
if self.video_active and self.cap is not None:
self.cap.release()
self.video_active = False
rospy.loginfo("视频流已停止")
##########################################################################################################################################################
# 拍照
def capture_and_save_image(self, image_name):
"""捕获并保存图像到指定目录"""
if not self.video_active:
self.start_video_stream()
ret, frame = self.cap.read()
if ret:
filename = os.path.join(self.lines_dir, f"{image_name}.jpg")
cv2.imwrite(filename, frame)
rospy.loginfo(f"已保存图像: {filename}")
return True
return False
##########################################################################################################################################################
# 检测水果
def detect_fruit_type(self, image_path):
"""根据图片文件名检测水果类型"""
filename = os.path.basename(image_path)
if "apple" in filename.lower():
return "apple"
elif "watermelon" in filename.lower():
return "watermelon"
elif "banana" in filename.lower():
return "banana"
elif "orange" in filename.lower():
return "orange"
return None
##########################################################################################################################################################
# 寻找对应水果
def get_initial_movement(self, filename):
"""根据文件名获取初始移动方向和距离"""
# 提取位置编号 (如1-1, 1-2等)
match = re.search(r'(\d)-(\d)_', filename)
if not match:
return 0.0
group = int(match.group(1))
position = int(match.group(2))
# 根据位置编号确定移动距离
if position in [1, 5]:
if "watermelon" in filename.lower():
return 3.6
elif "apple" in filename.lower():
return 2.8
elif "orange" in filename.lower():
return 1.5
elif "banana" in filename.lower():
return 0.0
elif position in [2, 6]:
if "watermelon" in filename.lower():
return 2.3
elif "apple" in filename.lower():
return 1.3
elif "orange" in filename.lower():
return 0.0
elif "banana" in filename.lower():
return -1.6
elif position in [3, 7]:
if "watermelon" in filename.lower():
return 1.0
elif "apple" in filename.lower():
return 0.0
elif "orange" in filename.lower():
return -1.6
elif "banana" in filename.lower():
return -2.8
elif position in [4, 8]:
if "watermelon" in filename.lower():
return 0.0
elif "apple" in filename.lower():
return -1.0
elif "orange" in filename.lower():
return -2.3
elif "banana" in filename.lower():
return -3.6
return 0.0
##########################################################################################################################################################
# 寻找水果中心点
def process_frame_for_fruit(self, frame, fruit_type):
"""根据水果类型处理视频帧"""
height, width = frame.shape[:2]
center_x, center_y = width // 2, height // 2
cv2.drawMarker(frame, (center_x, center_y), (0, 0, 255), cv2.MARKER_CROSS, 20, 2)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# 根据水果类型设置颜色范围
if fruit_type == "apple":
lower1 = np.array([0, 120, 80])
upper1 = np.array([10, 255, 255])
lower2 = np.array([160, 120, 80])
upper2 = np.array([180, 255, 255])
elif fruit_type == "watermelon":
lower1 = np.array([35, 50, 30])
upper1 = np.array([90, 255, 200])
lower2 = upper2 = None
elif fruit_type == "banana":
lower1 = np.array([20, 150, 120])
upper1 = np.array([30, 255, 200])
lower2 = np.array([15, 130, 100])
upper2 = np.array([35, 255, 180])
elif fruit_type == "orange":
lower1 = np.array([10, 120, 100])
upper1 = np.array([20, 255, 255])
lower2 = np.array([5, 100, 80])
upper2 = np.array([25, 255, 255])
else:
return frame, None, (center_x, center_y), 0.0
# 创建颜色掩膜
mask1 = cv2.inRange(hsv, lower1, upper1)
if lower2 is not None and upper2 is not None:
mask2 = cv2.inRange(hsv, lower2, upper2)
mask = cv2.bitwise_or(mask1, mask2)
else:
mask = mask1
# 形态学处理
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (9, 9))
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)))
# 寻找最大轮廓
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
red_center = None
red_area = 0
if contours:
largest_contour = max(contours, key=cv2.contourArea)
red_area = cv2.contourArea(largest_contour)
if red_area > 100: # 最小面积阈值
# 绘制边界框
x, y, w, h = cv2.boundingRect(largest_contour)
cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 0, 0), 2)
# 计算中心点
M = cv2.moments(largest_contour)
if M["m00"] != 0:
red_center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
cv2.drawMarker(frame, red_center, (255, 0, 0), cv2.MARKER_CROSS, 20, 2)
# 绘制中心点连线
cv2.line(frame, (center_x, center_y), red_center, (0, 255, 255), 2)
# 计算距离和接近度
distance = math.sqrt((red_center[0]-center_x)**2 + (red_center[1]-center_y)**2)
max_distance = math.sqrt(center_x**2 + center_y**2)
closeness = 1 - (distance / max_distance)
# 显示接近度
cv2.putText(frame, f"Closeness: {closeness:.2f}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
return frame, red_center, (center_x, center_y), closeness
return frame, red_center, (center_x, center_y), 0.0
##########################################################################################################################################################
# 根据水果微调降落
def adjust_position_based_on_fruit(self, fruit_type):
"""根据水果类型调整无人机位置"""
try:
if not self.start_video_stream():
return False
frame_count = 0
processed_frame = None
fruit_center = None
frame_center = None
closeness = 0.0
start_time = time.time()
while time.time() - start_time < 5:
ret, frame = self.cap.read()
if not ret:
rospy.logerr("无法读取视频帧")
continue
frame_count += 1
if frame_count % 30 == 0: #每30帧处理一次视频流
processed_frame, fruit_center, frame_center, closeness = self.process_frame_for_fruit(frame, fruit_type)
if fruit_center is not None:
if closeness >= self.adjustment_threshold:
# 保存对准图像
filename = os.path.join(self.land_dir, f"Land_{fruit_type}.jpg")
cv2.imwrite(filename, processed_frame)
rospy.loginfo(f"已保存对准图像到: {filename}")
self.stop_video_stream()
# 降落前向前微调0.1米
rospy.loginfo("降落前向前微调0.1米")
self.move_by_velocity(0.1, 0, 0, 1.0)
rospy.sleep(1.5)
self.landing()
self.last_fruit_type = fruit_type
return True
dx = fruit_center[0] - frame_center[0]
dy = fruit_center[1] - frame_center[1]
distance = math.sqrt(dx**2 + dy**2)
if distance > 0:
norm_dx = dx / distance
norm_dy = dy / distance
else:
norm_dx, norm_dy = 0, 0
vel_x = norm_dy * self.micro_adjust_speed
vel_y = norm_dx * self.micro_adjust_speed
vel_z = -self.descend_speed
direction_x = "向前" if vel_x > 0 else "向后" if vel_x < 0 else ""
direction_y = "向右" if vel_y > 0 else "向左" if vel_y < 0 else ""
direction_z = "下降"
directions = []
if abs(vel_x) > 0.01:
directions.append(f"{direction_x} {abs(vel_x):.2f}m/s")
if abs(vel_y) > 0.01:
directions.append(f"{direction_y} {abs(vel_y):.2f}m/s")
if abs(vel_z) > 0.01:
directions.append(f"{direction_z} {abs(vel_z):.2f}m/s")
direction_str = ", ".join(directions) if directions else "保持位置"
rospy.loginfo(f"微调指令: {direction_str} | 接近度: {closeness:.2f}/{self.adjustment_threshold}")
self.move_by_velocity(-vel_x, vel_y, vel_z, self.micro_adjust_time)
rospy.sleep(0.1)
self.stop_video_stream()
rospy.sleep(4)
if fruit_center is None:
rospy.loginfo("本次采集周期未检测到目标水果")
return False
return False
except Exception as e:
rospy.logerr(f"处理视频帧时发生错误: {str(e)}")
self.stop_video_stream()
return False
##########################################################################################################################################################
##########################################################################################################################################################
# 在小车停机坪上的降落
def process_frame(self, frame):
"""处理视频帧，寻找并标记最大红色区域，并标记中心点"""
height, width = frame.shape[:2]
center_x, center_y = width // 2, height // 2
cv2.drawMarker(frame, (center_x, center_y), (0, 0, 255), cv2.MARKER_CROSS, 20, 2)
# 寻找红色区域
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([0, 120, 70])
upper_red1 = np.array([10, 255, 255])
lower_red2 = np.array([170, 120, 70])
upper_red2 = np.array([180, 255, 255])
mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
mask = cv2.bitwise_or(mask1, mask2)
# 寻找最大轮廓
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
red_center = None
red_area = 0
if contours:
largest_contour = max(contours, key=cv2.contourArea)
red_area = cv2.contourArea(largest_contour)
if red_area > 100: # 最小面积阈值
x, y, w, h = cv2.boundingRect(largest_contour)
cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 0, 0), 2)
M = cv2.moments(largest_contour)
if M["m00"] != 0:
red_center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
cv2.drawMarker(frame, red_center, (255, 0, 0), cv2.MARKER_CROSS, 20, 2)
cv2.line(frame, (center_x, center_y), red_center, (0, 255, 255), 2)
distance = math.sqrt((red_center[0]-center_x)**2 + (red_center[1]-center_y)**2)
max_distance = math.sqrt(center_x**2 + center_y**2)
closeness = 1 - (distance / max_distance)
cv2.putText(frame, f"Closeness: {closeness:.2f}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
return frame, red_center, (center_x, center_y), closeness
return frame, red_center, (center_x, center_y), 0.0
##########################################################################################################################################################
##########################################################################################################################################################
# 小车红色区域微调的降落
def adjust_position_based_on_LandArea(self):
"""根据视频帧调整无人机位置"""
try:
# 创建Land图片保存目录
land_save_path = "/home/forlinx/Pictures/Land"
os.makedirs(land_save_path, exist_ok=True)
if not self.start_video_stream():
return False
frame_count = 0
processed_frame = None
red_center = None
frame_center = None
closeness = 0.0
start_time = time.time()
while time.time() - start_time < 5:
ret, frame = self.cap.read()
if not ret:
rospy.logerr("无法读取视频帧")
continue
frame_count += 1
if frame_count % 30 == 0: #每30帧处理一次视频流换言之即为 每30帧控制一次飞机微调
processed_frame, red_center, frame_center, closeness = self.process_frame(frame)
if red_center is not None:
if closeness >= self.adjustment_threshold:
# 修改保存路径到Land文件夹
filename = os.path.join(land_save_path, "Land.jpg")
cv2.imwrite(filename, processed_frame)
rospy.loginfo(f"已保存对准图像到: {filename}")
self.stop_video_stream()
self.landing()
return True
dx = red_center[0] - frame_center[0]
dy = red_center[1] - frame_center[1]
distance = math.sqrt(dx**2 + dy**2)
if distance > 0:
norm_dx = dx / distance
norm_dy = dy / distance
else:
norm_dx, norm_dy = 0, 0
vel_x = norm_dy * self.micro_adjust_speed
vel_y = norm_dx * self.micro_adjust_speed
vel_z = -self.descend_speed
direction_x = "向前" if vel_x > 0 else "向后" if vel_x < 0 else ""
direction_y = "向右" if vel_y > 0 else "向左" if vel_y < 0 else ""
direction_z = "下降"
directions = []
if abs(vel_x) > 0.01:
directions.append(f"{direction_x} {abs(vel_x):.2f}m/s")
if abs(vel_y) > 0.01:
directions.append(f"{direction_y} {abs(vel_y):.2f}m/s")
if abs(vel_z) > 0.01:
directions.append(f"{direction_z} {abs(vel_z):.2f}m/s")
direction_str = ", ".join(directions) if directions else "保持位置"
rospy.loginfo(f"微调指令: {direction_str} | 接近度: {closeness:.2f}/{self.adjustment_threshold}")
self.move_by_velocity(-vel_x, vel_y, vel_z, self.micro_adjust_time)
rospy.sleep(0.1)
self.stop_video_stream()
rospy.sleep(4)
if red_center is None:
rospy.loginfo("本次采集周期未检测到红色区域")
return False
return False
except Exception as e:
rospy.logerr(f"处理视频帧时发生错误: {str(e)}")
self.stop_video_stream()
return False
##########################################################################################################################################################
##########################################################################################################################################################
# 从处理的文件夹内判断降落水果
def run_fruit_mission(self, line_name):
"""执行水果降落任务，只处理与航线名称对应的图片"""
rospy.loginfo(f"开始执行{line_name}的水果降落任务")
# 1. 检查Fruits目录是否存在
if not os.path.exists(self.fruits_dir):
rospy.logerr(f"水果图片目录不存在: {self.fruits_dir}")
return
# 2. 获取目录中与航线名称匹配的图片文件
fruit_files = [f for f in os.listdir(self.fruits_dir)
if f.lower().startswith(line_name.lower()) and
f.lower().endswith(('.jpg', '.jpeg', '.png'))]
if not fruit_files:
rospy.logerr(f"未找到与航线{line_name}匹配的水果图片")
return
fruit_file = fruit_files[0]
fruit_path = os.path.join(self.fruits_dir, fruit_file)
fruit_type = self.detect_fruit_type(fruit_path)
if not fruit_type:
rospy.logerr(f"无法从文件名识别水果类型: {fruit_file}")
return
rospy.loginfo(f"识别到水果类型: {fruit_type}, 文件: {fruit_file}")
# 3. 根据水果类型微调位置并降落
self.adjust_position_based_on_fruit(fruit_type)
rospy.loginfo(f"{line_name}水果降落任务完成")
##########################################################################################################################################################
##########################################################################################################################################################
# =============================================
# 航线1系列任务（完整修改版）
# =============================================
def line_A1_1(self):
"""航线A1-1任务"""
rospy.loginfo("执行A1-1飞行路线")
# 向上0.25米
self.move_by_velocity(0, 0, 0.25)
rospy.sleep(2)
# 向左0.72米
self.move_by_velocity(0, -0.72, 0)
rospy.sleep(2)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-1")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-1")
def line_A1_2(self):
"""航线A1-2任务"""
rospy.loginfo("执行A1-2飞行路线")
# 向右0.72米
self.move_by_velocity(0, 0.72, 0)
rospy.sleep(2)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-2")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-2")
def line_A1_3(self):
"""航线A1-3任务"""
rospy.loginfo("执行A1-3飞行路线")
# 向上0.25米
self.move_by_velocity(0, 0, 0.25)
rospy.sleep(2)
# 向右1.87米
self.move_by_velocity(0, 1.87, 0)
rospy.sleep(3)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-3")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-3")
def line_A1_4(self):
"""航线A1-4任务"""
rospy.loginfo("执行A1-4飞行路线")
# 向上0.3米
self.move_by_velocity(0, 0, 0.3)
rospy.sleep(2)
# 向右3.0米
self.move_by_velocity(0, 3.0, 0)
rospy.sleep(4)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-4")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-4")
def line_A1_5(self):
"""航线A1-5任务"""
rospy.loginfo("执行A1-5飞行路线")
# 向下0.3米
self.move_by_velocity(0, 0, -0.3)
rospy.sleep(2)
# 向左0.72米
self.move_by_velocity(0, -0.72, 0)
rospy.sleep(2)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-5")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-5")
def line_A1_6(self):
"""航线A1-6任务"""
rospy.loginfo("执行A1-6飞行路线")
# 向下0.3米
self.move_by_velocity(0, 0, -0.3)
rospy.sleep(2)
# 向右0.72米
self.move_by_velocity(0, 0.72, 0)
rospy.sleep(2)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-6")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-6")
def line_A1_7(self):
"""航线A1-7任务"""
rospy.loginfo("执行A1-7飞行路线")
# 向下0.3米
self.move_by_velocity(0, 0, -0.3)
rospy.sleep(2)
# 向右2.0米
self.move_by_velocity(0, 2.0, 0)
rospy.sleep(3)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-7")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-7")
def line_A1_8(self):
"""航线A1-8任务"""
rospy.loginfo("执行A1-8飞行路线")
# 向下0.3米
self.move_by_velocity(0, 0, -0.3)
rospy.sleep(2)
# 向右3.0米
self.move_by_velocity(0, 3.0, 0)
rospy.sleep(4)
# 向后0.35米
self.move_by_velocity(-0.35, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("A1-8")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_right_task()
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("A1-8")
# =============================================
# 航线2系列任务（完整修改版）
# =============================================
def line_B2_1(self):
"""航线B2-1任务"""
rospy.loginfo("执行B2-1飞行路线")
# 向上0.2米
self.move_by_velocity(0, 0, 0.2)
rospy.sleep(2)
# 向后0.2米
self.move_by_velocity(-0.2, 0, 0)
rospy.sleep(2)
# 向右2.95米
self.move_by_velocity(0, 2.95, 0)
rospy.sleep(4)
# 拍摄并保存图片
self.capture_and_save_image("B2-1")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-1")
def line_B2_2(self):
"""航线B2-2任务"""
rospy.loginfo("执行B2-2飞行路线")
# 向上0.2米
self.move_by_velocity(0, 0, 0.2)
rospy.sleep(2)
# 向后0.2米
self.move_by_velocity(-0.2, 0, 0)
rospy.sleep(2)
# 向右1.6米
self.move_by_velocity(0, 1.6, 0)
rospy.sleep(3)
# 拍摄并保存图片
self.capture_and_save_image("B2-2")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-2")
def line_B2_3(self):
"""航线B2-3任务"""
rospy.loginfo("执行B2-3飞行路线")
# 向上0.2米
self.move_by_velocity(0, 0, 0.2)
rospy.sleep(2)
# 向后0.2米
self.move_by_velocity(-0.2, 0, 0)
rospy.sleep(2)
# 向右0.6米
self.move_by_velocity(0, 0.6, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("B2-3")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-3")
def line_B2_4(self):
"""航线B2-4任务"""
rospy.loginfo("执行B2-4飞行路线")
# 向上0.2米
self.move_by_velocity(0, 0, 0.2)
rospy.sleep(2)
# 向后0.2米
self.move_by_velocity(-0.2, 0, 0)
rospy.sleep(2)
# 向左0.5米
self.move_by_velocity(0, -0.5, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("B2-4")
rospy.sleep(15)
# 向上0.8米
self.move_by_velocity(0, 0, 0.8)
rospy.sleep(2)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-4")
def line_B2_5(self):
"""航线B2-5任务"""
rospy.loginfo("执行B2-5飞行路线")
# 向下0.2米
self.move_by_velocity(0, 0, -0.2)
rospy.sleep(2)
# 向右3.0米
self.move_by_velocity(0, 3.0, 0)
rospy.sleep(4)
# 向后0.5米
self.move_by_velocity(-0.5, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("B2-5")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-5")
def line_B2_6(self):
"""航线B2-6任务"""
rospy.loginfo("执行B2-6飞行路线")
# 向下0.3米
self.move_by_velocity(0, 0, -0.3)
rospy.sleep(2)
# 向右1.7米
self.move_by_velocity(0, 1.7, 0)
rospy.sleep(3)
# 向后0.5米
self.move_by_velocity(-0.5, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("B2-6")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-6")
def line_B2_7(self):
"""航线B2-7任务"""
rospy.loginfo("执行B2-7飞行路线")
# 向下0.3米
self.move_by_velocity(0, 0, -0.3)
rospy.sleep(2)
# 向右0.5米
self.move_by_velocity(0, 0.5, 0)
rospy.sleep(2)
# 向后0.5米
self.move_by_velocity(-0.5, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("B2-7")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-7")
def line_B2_8(self):
"""航线B2-8任务"""
rospy.loginfo("执行B2-8飞行路线")
# 向下0.3米
self.move_by_velocity(0, 0, -0.3)
rospy.sleep(2)
# 向左0.5米
self.move_by_velocity(0, -0.5, 0)
rospy.sleep(2)
# 向后0.5米
self.move_by_velocity(-0.5, 0, 0)
rospy.sleep(2)
# 拍摄并保存图片
self.capture_and_save_image("B2-8")
rospy.sleep(15)
# 向上1.2米
self.move_by_velocity(0, 0, 1.2)
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_left_task()
# 向右0.7米
self.move_by_velocity(0, 0.7, 0)
rospy.sleep(2)
# 执行水果降落任务
self.run_fruit_mission("B2-8")
# =============================================
# 过渡任务
# =============================================
def transition_operation_1(self):
"""过渡操作一"""
rospy.loginfo("开始执行过渡操作一")
# 重新起飞
if not self.takeoff():
return False
# 向右2.8米
self.move_by_velocity(0, 2.8, 0)
rospy.sleep(3)
# 向下1.6米
self.move_by_velocity(0, 0, -1.6)
rospy.sleep(3)
# 根据上一次降落水果类型移动
if self.last_fruit_type == "apple":
self.move_by_velocity(0.3, 0, 0) # 向前飞0.3米
elif self.last_fruit_type == "watermelon":
self.move_by_velocity(-0.3, 0, 0) # 向后飞0.3米
elif self.last_fruit_type == "orange":
self.move_by_velocity(1.8, 0, 0) # 向前飞1.8米
elif self.last_fruit_type == "banana":
self.move_by_velocity(3.1, 0, 0) # 向前飞3.1米
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_right_task()
# 等待10秒
rospy.sleep(10)
rospy.loginfo("过渡操作一完成")
def transition_operation_2(self):
"""过渡操作二"""
rospy.loginfo("开始执行过渡操作二")
# 重新起飞
if not self.takeoff():
return False
# 向左0.7米
self.move_by_velocity(0, -0.7, 0)
rospy.sleep(2)
# 向下1.6米
self.move_by_velocity(0, 0, -1.6)
rospy.sleep(3)
# 根据上一次降落水果类型移动
if self.last_fruit_type == "apple":
self.move_by_velocity(0.3, 0, 0) # 向前飞0.3米
elif self.last_fruit_type == "watermelon":
self.move_by_velocity(-0.3, 0, 0) # 向后飞0.3米
elif self.last_fruit_type == "orange":
self.move_by_velocity(1.8, 0, 0) # 向前飞1.8米
elif self.last_fruit_type == "banana":
self.move_by_velocity(3.1, 0, 0) # 向前飞3.1米
rospy.sleep(3)
# 执行云台校准
self.gimbal_calibration_right_task()
# 等待5秒
rospy.sleep(5)
rospy.loginfo("过渡操作二完成")
##########################################################################################################################################################
# 最终降落
def run_land_mission(self):
"""执行降落任务"""
rospy.loginfo("开始执行降落任务")
# 控制云台向下90°
self.control_gimbal(-90)
rospy.sleep(2)
# 启动视频流并微调位置
if self.start_video_stream():
while not rospy.is_shutdown():
if self.adjust_position_based_on_LandArea():
break
rospy.sleep(0.1)
self.stop_video_stream()
# 在最终降落前向前飞行0.1米
rospy.loginfo("降落前向前微调0.1米")
self.move_by_velocity(0.1, 0, 0, 1.0)
rospy.sleep(1.5)
# 最终降落
if not self.landing():
rospy.logerr("最终降落失败")
return
rospy.loginfo("降落任务完成")
##########################################################################################################################################################
# 执行航线
def run_mission(self, line_names):
"""执行航线任务"""
# 带重试的起飞功能
if not self.takeoff():
return
# 执行航线任务
for i, line_name in enumerate(line_names):
# 航线1系列任务
if line_name == "A1-1":
self.line_A1_1()
elif line_name == "A1-2":
self.line_A1_2()
elif line_name == "A1-3":
self.line_A1_3()
elif line_name == "A1-4":
self.line_A1_4()
elif line_name == "A1-5":
self.line_A1_5()
elif line_name == "A1-6":
self.line_A1_6()
elif line_name == "A1-7":
self.line_A1_7()
elif line_name == "A1-8":
self.line_A1_8()
# 航线2系列任务
elif line_name == "B2-1":
self.line_B2_1()
elif line_name == "B2-2":
self.line_B2_2()
elif line_name == "B2-3":
self.line_B2_3()
elif line_name == "B2-4":
self.line_B2_4()
elif line_name == "B2-5":
self.line_B2_5()
elif line_name == "B2-6":
self.line_B2_6()
elif line_name == "B2-7":
self.line_B2_7()
elif line_name == "B2-8":
self.line_B2_8()
# 如果不是最后一个任务，执行过渡飞行
if i < len(line_names) - 1:
# 判断下一个任务是1系列还是2系列
next_task = line_names[i+1]
if next_task.startswith('A1-'):
self.transition_to_line1()
elif next_task.startswith('B2-'):
self.transition_to_line2()
# 如果是2系列最后一个任务，执行红色区域降落
if line_names[-1].startswith('B2-'):
self.run_land_mission()
rospy.loginfo("所有航线任务执行完成")
##########################################################################################################################################################
##########################################################################################################################################################
if __name__ == '__main__':
try:
controller = UAVController()
line_names = []
# 解析命令行参数
for param in rospy.myargv():
if "LineName=" in param:
line_names = param.split("=")[1].split(",")
break
if not line_names:
rospy.logerr("未指定航线名称，请使用 LineName=A1-1,B2-1 格式指定")
else:
controller.run_mission(line_names)
except rospy.ROSInterruptException:
controller.stop_video_stream()
rospy.loginfo("程序中断")
优化
本文介绍了一种基于二分搜索和离线查询的游戏路径覆盖问题解决方案，通过构建树状结构并利用离线查询优化算法，实现了高效地判断一个路径是否被另一个路径完全覆盖。
扫一扫
247
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
