131 lines
5.0 KiB
Python
131 lines
5.0 KiB
Python
import time
|
||
import sys
|
||
import VisionCaptureApi
|
||
import PX4MavCtrlV4 as PX4MavCtrl
|
||
import cv2
|
||
import UE4CtrlAPI
|
||
import json
|
||
import numpy as np
|
||
import os
|
||
# xxx
|
||
ue = UE4CtrlAPI.UE4CtrlAPI()
|
||
|
||
#Create a new MAVLink communication instance, UDP sending port (CopterSim’s receving port) is 20100
|
||
mav = PX4MavCtrl.PX4MavCtrler(1)
|
||
|
||
# The IP should be specified by the other computer
|
||
vis = VisionCaptureApi.VisionCaptureApi()
|
||
|
||
# Send command to UE4 Window 1 to change resolution
|
||
ue.sendUE4Cmd('r.setres 1280x720w',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
|
||
ue.sendUE4Cmd('t.MaxFPS 30',0) # 设置UE4最大刷新频率，同时也是取图频率
|
||
ue.sendUE4Cmd('')
|
||
time.sleep(2)
|
||
|
||
# VisionCaptureApi 中的配置函数
|
||
vis.jsonLoad() # 加载Config.json中的传感器配置文件
|
||
|
||
# vis.RemotSendIP = '192.168.3.80'
|
||
# 注意，手动修改RemotSendIP的值，可以将图片发送到本地址
|
||
# 如果不修改这个值，那么发送的IP地址为json文件中SendProtocol[1:4]定义的IP
|
||
# 图片的发送端口，为json中SendProtocol[5]定义好的。
|
||
|
||
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
|
||
if not isSuss: # 如果请求取图失败，则退出
|
||
sys.exit(0)
|
||
vis.startImgCap() # 开启取图，并启用共享内存图像转发，转发到填写的目录
|
||
|
||
|
||
#mav.InitMavLoop(UDPMode), where UDPMode=0,1,2,3,4
|
||
# Use MAVLink_Full Mode to control PX4
|
||
# In this mode, this script will send MAVLinkOffboard message to PX4 directly
|
||
# and receive MAVLink data from PX4
|
||
# Obviously, MAVLink_Full mode is slower than UDP mode, but the functions and data are more comprehensive
|
||
mav.InitMavLoop() # the same as mav.InitMavLoop() in other PythonVision demos
|
||
|
||
time.sleep(1)
|
||
print('Start Offboard Send!')
|
||
mav.initOffboard()
|
||
time.sleep(1)
|
||
|
||
|
||
# Check if the PX4'EKF has correctlly initialized, which is necessary for offboard control
|
||
if not mav.isPX4Ekf3DFixed:
|
||
print('CopterSim/PX4 still not 3DFxied, please wait and try again.')
|
||
sys.exit(0)
|
||
else:
|
||
print('CopterSim/PX4 3D Fixed, ready to fly.')
|
||
|
||
mav.SendMavArm(True)
|
||
print('Fly to pos 0, 0, -2!')
|
||
mav.SendPosNED(0, 0, -2, 0)
|
||
|
||
lastTime = time.time()
|
||
|
||
# 获取当前文件所在目录
|
||
current_directory = os.path.dirname(os.path.abspath(__file__))
|
||
# 构建完整的文件路径
|
||
config_file_path = os.path.join(current_directory, 'Config.json')
|
||
|
||
# 加载测距传感器配置文件
|
||
with open(config_file_path, 'r', encoding='utf-8') as config_file:
|
||
config = json.load(config_file)
|
||
sensors = config['VisionSensors']
|
||
sensor = sensors[0]
|
||
sensor_distance = sensor['SensorPosXYZ'][0]
|
||
|
||
|
||
while True:
|
||
if vis.hasData[0] and vis.hasData[1]: # 是否成功取图和获取测距传感器
|
||
|
||
img = vis.Img[0] # 获取0号传感器，图像数据指针，格式为opencv图像格式
|
||
obj_distance = vis.Img[1] # 获取1号传感器，距离数据指针，格式见VisionCaptureApi.DistanceSensor()
|
||
|
||
|
||
# 获取图像尺寸
|
||
height, width, _ = img.shape
|
||
# 计算中心坐标
|
||
center_x = width // 2
|
||
center_y = height // 2
|
||
|
||
crosshair_length = 20
|
||
crosshair_thickness = 2
|
||
cv2.line(img, (center_x - crosshair_length, center_y), (center_x + crosshair_length, center_y), (0, 0, 255), crosshair_thickness)
|
||
cv2.line(img, (center_x, center_y - crosshair_length), (center_x, center_y + crosshair_length), (0, 0, 255), crosshair_thickness)
|
||
|
||
# 绘制距离信息
|
||
distance_text = f"Distance: {obj_distance.Distance} m"
|
||
cv2.putText(img, distance_text, (center_x - 50, center_y + crosshair_length + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
|
||
|
||
# 显示图像
|
||
cv2.imshow('Image', img)
|
||
cv2.waitKey(1)
|
||
|
||
time.sleep(0.01)
|
||
|
||
# 注意：距离传感器的数据定义如下
|
||
# class DistanceSensor:
|
||
# ## @brief DistanceSensor的构造函数
|
||
# # @param 初始化类属性
|
||
# def __init__(self):
|
||
# ## @var DistanceSensor.TimeStamp
|
||
# # @brief 这是当前消息的时间戳，初始化为 0
|
||
# self.TimeStamp = 0
|
||
# ## @var DistanceSensor.Distance
|
||
# # @brief 这是距离传感器测量到的距离，初始化为 0
|
||
# self.Distance = 0
|
||
# ## @var DistanceSensor.CopterID
|
||
# # @brief 用于标识直升机的ID，初始化为 0
|
||
# self.CopterID = 0
|
||
# ## @var DistanceSensor.RayStart
|
||
# # @brief 这是射线起点的坐标，初始化为[0,0,0]
|
||
# self.RayStart = [0,0,0]
|
||
# ## @var DistanceSensor.AngEular
|
||
# # @brief 这是传感器的欧拉角（Euler Angles），初始化为[0,0,0]
|
||
# self.AngEular = [0,0,0]
|
||
# ## @var DistanceSensor.ImpactPoint
|
||
# # @brief 这是碰撞点的坐标，初始化为[0,0,0]
|
||
# self.ImpactPoint = [0,0,0]
|
||
# ## @var DistanceSensor.BoxOri
|
||
# # @brief 这是盒子的原点或参考点，初始化为[0,0,0]
|
||
# self.BoxOri = [0,0,0] |