#!/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("程序中断")
优化
文件持久化技巧
本文介绍了一种确保文件中数组元素正确读写的技巧。通过使用line_as_stream方法来逐行读取数据，即使文件中出现损坏行，也能防止数据错位，确保其他元素的正确性。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
