【RflySim仿真平台】视觉感知与避障决策例程学习
本文介绍了RflySim平台下视觉感知与避障决策的基础接口实验,主要包括:1)通过Python接口实现RflySim 3D图像获取、相机参数调整及多类型图像(RGB、灰度、深度)采集;2)点云数据处理与显示方法,包括共享内存和UDP传输两种模式;3)激光雷达点云数据获取与大疆Livox激光雷达应用;4)无人机位置信息与碰撞数据获取;5)基于ROS系统的自定义tf树实现。实验覆盖了Windows和L
目录
11.获取 RflySim3D 内所有动态创建物体位置、碰撞数据
0.ApiExps:视觉感知与避障决策基础接口类实验
一、RflySim 视觉接口使用入门
1.Python接口获取 RflySim3D图像并实时控制
本实验只支持 Windows 下 Python 环境运行,通过 Python 接口 VisionCaptureApi.py(见 RflySimAPIs\RflySimSDK\vision 目录)获取 RflySim3D 图像并实时更新相机参数(姿态、位置、FOV 等)。
![]()


重点:
其中获取json配置文件的传感器数据并通过ROS发布话题的API函数为VisionCaptureApi.py,详细的可以看startImgCap()函数,具体说明了各个传感器的数据格式、话题名称等信息。
2.python 接口获取 RGB、灰度、深度三个相机图像
在config文件中进行了传感器配置,参数需要好好看,后面会用到
3.无 CopterSim 取图

4.深度图获取


5.飞机、物体、相机信息获取


6.点云分割(传感器类型:分割和激光雷达)



7.点云图像共享内存方式显示(激光雷达)

关键知识点 1: SendProtocol[0]决定了图像的传出模式。SendProtocol[0]=0:共 享内存(仅限 Windows 下获取图像),1:UDP 直传 png 压缩。 如果是激光 雷达数据只有 0 或 1 (共享内存和 UDP 网络传输)。
{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0,则自动递增排序。
"TypeID":20, //视觉传感器类型,1:RGB,2:深度,3:灰度,4:分割,5:测距,7:深度转点云,20-23:激光雷达,40-41:热力红外(收费版)
"TargetCopter":1, //传感器绑定的CopterID号,注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式,0:固定飞机几何中心, 1:固定飞机底部中心,2:固定地面上,3:弱固定飞机上姿态不随动,4:绑定其他视觉传感器上
"DataWidth":900, //图像像素长度
"DataHeight":32,//图像像素宽度
"DataCheckFreq":10,//图像检查更新频率,如果发现UE渲染更新了(取决于UE刷新帧率),会立刻发出数据。UE刷新率+DataCheckFreq检查频率,共同决定图像延迟。
"SendProtocol":[0,0,0,0,0,0,0,0],// SendProtocol[0],表示传输协议,0:共享内存(仅限Windows下获取图像),1:UDP网络传输模式(图片使用jpeg压缩,点云直传),2:UDP直传图片不压缩,3:UDP直传图片png压缩。注:0-1适用所有传感器,2-3选项仅限图像类传感器。
//SendProtocol[1-4]位对应,IP地址位,表示请求返回的IP地址。默认都填0(或127.0.0.1),会自动请求UE返回图像到本电脑;SendProtocol[5]端口位,指定传感器图像回传端口,需要为每个传感器设置不同端口。默认填0,会自动使用9999+SeqID的递增端口号。
"CameraFOV":90, //视觉传感器的FOV视场角,和焦距呈现一定数值关系,能间接修改焦距。
"EularOrQuat":0, //使用欧拉角SensorAngEular还是四元数SensorAngQuat来设置视觉传感器姿态,默认使用0欧拉角。
"SensorPosXYZ":[0,0,-0.3], //视觉传感器的安装位置,和TargetMountType对应偏移中心,单位米
"SensorAngQuat":[0,0,0,0], // 视觉传感器的安装姿态,用四元数方式表示
"SensorAngEular":[0,0,0],// 视觉传感器的安装姿态,用欧拉角方式表示,单位角度制
"otherParams":[200,0.05,-45,45,-20,20,0,0,0,0,0,0,0,0,0,0] //其他参数
}
]
}


import time
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import UE4CtrlAPI
import numpy as np
import Open3DShow
show3d=Open3DShow.Open3DShow()
ue = UE4CtrlAPI.UE4CtrlAPI()
#创建一个新的MAVLink通信实例,UDP发送端口(CopterSim的接收端口)为20100
mav = PX4MavCtrl.PX4MavCtrler(1)
vis = VisionCaptureApi.VisionCaptureApi()
ue.sendUE4Cmd('r.setres 1280x720w',0) # 设置UE4窗口分辨率,注意本窗口仅限于显示,取图分辨率在json中配置,本窗口设置越小,资源需求越少。
ue.sendUE4Cmd('t.MaxFPS 30',0) # 设置UE4最大刷新频率,同时也是取图频率
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() # 开启取图,并启用共享内存图像转发,转发到填写的目录
#创建点云显示窗口
show3d.CreatShow(0)
lastTime = time.time()
while True:
lastTime += 1 / 10.0
sleepTime = lastTime - time.time()
if sleepTime > 0:
time.sleep(sleepTime)
else:
lastTime = time.time()
if vis.hasData[0]:
# 更新点云
show3d.UpdateShow(vis.Img[0])
# 标记
vis.hasData[0]=False
这里调用了Open3DShow.py进行点云显示!
#!/bin/python3
import numpy as np
import sys
try:
import open3d
except:
print('Failed to Load open3d module')
print('Please run the following command:')
print('pip install open3d==0.10.0')
sys.exit(0)
## @file
# 这是一个主要用来显示点云图像的模块。
# @anchor Open3DShow接口库文件
# 对应例程链接见
## @class Open3DShow
# @brief Open3DShow结构体类.
class Open3DShow:
"""This is the API class for python to request image from UE4"""
## @brief 在对象销毁时调用。它确保在对象被销毁时关闭显示窗口,释放资源
# # - @anchor __del__
# @param 无
# @return 无
def __del__(self):
self.CloseShow()
## @brief 这是Open3DShow的构造函数初始化了一个 Open3D 的 Visualizer 对象用于显示点云
def __init__(self):
#点云显示
self.visualizer = open3d.visualization.Visualizer()
# 构建 Open3D 中提供的 gemoetry 点云类
self.pcd = open3d.geometry.PointCloud()
self.pcdTmp = open3d.geometry.PointCloud()
## @brief 创建一个 Open3D 的窗口,用于显示点云
# # - @anchor CreatShow
# @param idx(默认值为 0) 窗口号
# @return 无
def CreatShow(self,idx=0):
# 创建显示创建
self.visualizer.create_window(window_name="PointCloud_"+str(idx), width=1280, height=720)
self.visualizer.get_render_option().background_color = np.asarray([0.8, 0.8, 0.8])
## @brief 更新并显示新的点云数据
# # - @anchor UpdateShow
# param Cloud 点云数据
# @return 无
def UpdateShow(self,Cloud):
self.visualizer.remove_geometry(self.pcd)
# 添加点云到可视化
p3d = np.array(Cloud)[:, :3] # 从4D*n向量提取3D*n点云。
self.pcd.points = open3d.utility.Vector3dVector(p3d)
self.visualizer.add_geometry(self.pcd)
# # 设置观察视角
# self.visualizer.get_view_control().set_front([1, 0, -1])
# self.visualizer.get_view_control().set_lookat([-2, 0, 5])
# self.visualizer.get_view_control().set_up([1, 0, 1])
# self.visualizer.get_view_control().set_front([1, 0, 0])
# self.visualizer.get_view_control().set_lookat([0, 0, 0])
# self.visualizer.get_view_control().set_up([0, 0, 1])
# 更新渲染
self.visualizer.update_geometry(self.pcd)
self.visualizer.poll_events()
self.visualizer.update_renderer()
## @brief 更新点云显示,使用 pcdTmp 中的数据替换 pcd 中的数据
# # - @anchor UpdatePCD
# @param 无
# @return 无
def UpdatePCD(self):
self.visualizer.remove_geometry(self.pcd)
# 添加点云到可视化
self.pcd.points = self.pcdTmp.points
self.pcd.colors = self.pcdTmp.colors
self.visualizer.add_geometry(self.pcd)
# 更新渲染
self.visualizer.update_geometry(self.pcd)
self.visualizer.poll_events()
self.visualizer.update_renderer()
## @brief 关闭显示窗口,释放资源
# # - @anchor CloseShow
# @param 无
# @return 无
def CloseShow(self):
self.visualizer.destroy_window()
8.大疆 Livox 激光雷达点云图像显示

通过平台取图 python 接口实现大疆 Livox 激光雷达扫描功能并获取点云数据进行实时显示。
{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0,则自动递增排序。
"TypeID":22, //视觉传感器类型,1:RGB,2:深度,3:灰度,4:分割,5:测距,7:深度转点云,20-23:激光雷达,40-41:热力红外(收费版)
"TargetCopter":1, //传感器绑定的CopterID号,注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式,0:固定飞机几何中心, 1:固定飞机底部中心,2:固定地面上,3:弱固定飞机上姿态不随动,4:绑定其他视觉传感器上
"DataWidth":250, //图像像素长度
"DataHeight":40,//图像像素宽度
"DataCheckFreq":10,//图像检查更新频率,如果发现UE渲染更新了(取决于UE刷新帧率),会立刻发出数据。UE刷新率+DataCheckFreq检查频率,共同决定图像延迟。
"SendProtocol":[1,0,0,0,0,0,0,0],// SendProtocol[0],表示传输协议,0:共享内存(仅限Windows下获取图像),1:UDP网络传输模式(图片使用jpeg压缩,点云直传),2:UDP直传图片不压缩,3:UDP直传图片png压缩。注:0-1适用所有传感器,2-3选项仅限图像类传感器。
//SendProtocol[1-4]位对应,IP地址位,表示请求返回的IP地址。默认都填0(或127.0.0.1),会自动请求UE返回图像到本电脑;SendProtocol[5]端口位,指定传感器图像回传端口,需要为每个传感器设置不同端口。默认填0,会自动使用9999+SeqID的递增端口号。
"CameraFOV":70.432, //视觉传感器的FOV视场角,和焦距呈现一定数值关系,能间接修改焦距。
"EularOrQuat":0, //使用欧拉角SensorAngEular还是四元数SensorAngQuat来设置视觉传感器姿态,默认使用0欧拉角。
"SensorPosXYZ":[0,0,-0.3], //视觉传感器的安装位置,和TargetMountType对应偏移中心,单位米
"SensorAngQuat":[0,0,0,0], // 视觉传感器的安装姿态,用四元数方式表示
"SensorAngEular":[0,0,0],// 视觉传感器的安装姿态,用欧拉角方式表示,单位角度制
"otherParams":[600,2.956,1.4,1.18,10,5,1,0,0,0,0,0,0,0,0,0] //其他参数
}
]
}

import time
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import UE4CtrlAPI
import numpy as np
import Open3DShow
show3d=Open3DShow.Open3DShow()
ue = UE4CtrlAPI.UE4CtrlAPI()
#创建一个新的MAVLink通信实例,UDP发送端口(CopterSim的接收端口)为20100
mav = PX4MavCtrl.PX4MavCtrler(1)
# The IP should be specified by the other computer
vis = VisionCaptureApi.VisionCaptureApi()
time.sleep(0.5)
# sendUE4Cmd: RflySim3D API to modify scene display style
# Format: ue.sendUE4Cmd(cmd,windowID=-1), where cmd is a command string, windowID is the received window number (assuming multiple RflySim3D windows are opened at the same time), windowID =-1 means sent to all windows
# Augument: RflyChangeMapbyName command means to switch the map (scene), the following string is the map name, here will switch all open windows to the grass map
ue.sendUE4Cmd('RflyChangeMapbyName SLAMScene')
time.sleep(6)
# Send command to UE4 Window 1 to change resolution
# 设置UE4窗口分辨率,注意本窗口仅限于显示,取图分辨率在json中配置,本窗口设置越小,资源需求越少。
ue.sendUE4Cmd('r.setres 1280x720w', 0)
ue.sendUE4Cmd('t.MaxFPS 30', 0) # 设置UE4最大刷新频率,同时也是取图频率
time.sleep(2)
ue.sendUE4Pos(1, 3, 0, [5.5, 0, -0.5])
# 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() # 开启取图,并启用共享内存图像转发,转发到填写的目录
#创建点云显示窗口
show3d.CreatShow(0)
lastTime = time.time()
while True:
lastTime += 1 / 10.0 #目标时间 间隔0.1S
sleepTime = lastTime - time.time()#目标时间减去现在的实际时间
if sleepTime > 0:
time.sleep(sleepTime)#说明还没达到目标时间,通过休眠来补足
else:
lastTime = time.time()
if vis.hasData[0]:
# 更新点云
show3d.UpdateShow(vis.Img[0])
# 标记
vis.hasData[0]=False
9.深度图转点云(传感器类型:深度转点云)


{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0,则自动递增排序。
"TypeID":7, //视觉传感器类型,1:RGB,2:深度,3:灰度,4:分割,5:测距,7:深度转点云,20-23:激光雷达,40-41:热力红外(收费版)
"TargetCopter":1, //传感器绑定的CopterID号,注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式,0:固定飞机几何中心, 1:固定飞机底部中心,2:固定地面上,3:弱固定飞机上姿态不随动,4:绑定其他视觉传感器上
"DataWidth":1024, //图像像素长度
"DataHeight":1024,//图像像素宽度
"DataCheckFreq":10,//图像检查更新频率,如果发现UE渲染更新了(取决于UE刷新帧率),会立刻发出数据。UE刷新率+DataCheckFreq检查频率,共同决定图像延迟。
"SendProtocol":[1,0,0,0,0,0,0,0],// SendProtocol[0],表示传输协议,0:共享内存(仅限Windows下获取图像),1:UDP网络传输模式(图片使用jpeg压缩,点云直传),2:UDP直传图片不压缩,3:UDP直传图片png压缩。注:0-1适用所有传感器,2-3选项仅限图像类传感器。
//SendProtocol[1-4]位对应,IP地址位,表示请求返回的IP地址。默认都填0(或127.0.0.1),会自动请求UE返回图像到本电脑;SendProtocol[5]端口位,指定传感器图像回传端口,需要为每个传感器设置不同端口。默认填0,会自动使用9999+SeqID的递增端口号。
"CameraFOV":120, //视觉传感器的FOV视场角,和焦距呈现一定数值关系,能间接修改焦距。
"EularOrQuat":0, //使用欧拉角SensorAngEular还是四元数SensorAngQuat来设置视觉传感器姿态,默认使用0欧拉角。
"SensorPosXYZ":[0,0,-0.3], //视觉传感器的安装位置,和TargetMountType对应偏移中心,单位米
"SensorAngQuat":[0,0,0,0], // 视觉传感器的安装姿态,用四元数方式表示
"SensorAngEular":[0,0,0],// 视觉传感器的安装姿态,用欧拉角方式表示,单位角度制
"otherParams":[100,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0] //其他参数
}
]
}
import time
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import UE4CtrlAPI
import numpy as np
import Open3DShow
show3d=Open3DShow.Open3DShow()# 创建一个点云显示实例
ue = UE4CtrlAPI.UE4CtrlAPI()# 创建UE控制实例
#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, -1!')
mav.SendPosNED(0, 0, -1)
# 窗口大小
window_width = 800
window_height = 600
#创建点云显示窗口
show3d.CreatShow(0)
lastTime = time.time()
while True:
lastTime = lastTime + 1/10.0 #0.5S
sleepTime = lastTime - time.time()
if sleepTime > 0:
time.sleep(sleepTime)
else:
lastTime = time.time()
if vis.hasData[0]:# 图片 i 数据是否更新
# 更新点云
show3d.UpdateShow(vis.Img[0]) #从 vis.Img[i]可以直接读取深度图像矩阵
# 标记
vis.hasData[0]=False

10.数据 UDP 直传 png 压缩

11.获取 RflySim3D 内所有动态创建物体位置、碰撞数据


二、livox mid360 激光雷达例程

{
"VisionSensors":[
{
"SeqID":0,
"TypeID":23,
"TargetCopter":1,
"TargetMountType":0,
"DataWidth":64,
"DataHeight":272,
"DataCheckFreq":10,
"SendProtocol":[0,127,0,0,1,9999,0,0],
"CameraFOV":0,
"SensorPosXYZ":[0,0,-0.3],
"SensorAngEular":[0,0,0],
"otherParams":[100,0,0,0,0,0,0,0]
}
]
}
# import required libraries
# pip3 install pymavlink pyserial
import cv2
import numpy as np
import time
import VisionCaptureApi
import math
import sys
import ReqCopterSim
import PX4MavCtrlV4
################## ReqCopterSim 接口应用#############################
#该接口的应用减少很多参数配置环节,直接使用接口配置
CopterId = 1 #需要获取平台里运行飞机的CopterID
req = ReqCopterSim.ReqCopterSim()
req.sendReSimIP(CopterId)
RflySim_IP = req.getSimIpID(CopterId) #获取制定copter_id 所在电脑上的ip
print(RflySim_IP)
#设置通信协议2: mavlink_full
udp_mode = 2
req.sendReSimUdpMode(CopterId,udp_mode)
host_ip = req.getHostIP() # 获取本机IP
################## VisionCaptureApi 接口的应用####################
VisionCaptureApi.isEnableRosTrans = True #开启接口ROS功能,发布传感器话题数据
vis = VisionCaptureApi.VisionCaptureApi(RflySim_IP)
vis.jsonLoad(1) #通过读取Config.json传感器配置,创建传感器
vis.RemotSendIP = host_ip
isSuss = vis.sendReqToUE4(IP=RflySim_IP) # 向RflySim3D发送取图请求,并验证
if not isSuss: # 如果请求取图失败,则退出
sys.exit(0)
vis.startImgCap()
################### PX4MavCtrlV4 接口控制飞机######################
mav = PX4MavCtrlV4.PX4MavCtrler(ID=1,ip=RflySim_IP)
mav.InitMavLoop(udp_mode) #指定通信协议,初始化
time.sleep(3) #给远程的RflySim平台一个程序响应时间
mav.initOffboard() #切换飞控模式
mav.SendMavArm(True) #解锁
mav.SendPosNED(0,0,-1,0) #飞机起飞到1米的位置
先运行脚本,然后打开WslGUI,再打开终端切换到linux的环境下
cd /mnt/e/PX4PSP/RflySimAPIs/8.RflySimVision/0.ApiExps/10.Mid360Demo
./run.sh ~catkin_ws
编译运行后,结果和上述一样。
三、共享内存传输点云数据

四、基于 python 接口自定义 ROS 系统 tf 树


五、大疆Livox 激光雷达点云数据 UDP 直传模式

六、UDP 传输点云数据



七、UDP 直传方式发布相机以及云台数据

1.BasicExps:视觉感知与避障决策基础功能性实验

可以自己在windows文件夹maintest.py中创建静态或者动态的障碍物

然后在bat文件中修改地图


2.AdvExps:视觉感知与避障决策进阶性实验
无人机室内三维空间巡航的控制系统,主要功能包括获取 CopterSim 仿真环境、控制无人机按预设轨迹飞行、处理视觉数据等。可以详细看视觉SLAM和激光SLAM两个实验,了解json配置传感器数据以及是如何进行建图。


魔乐社区(Modelers.cn) 是一个中立、公益的人工智能社区,提供人工智能工具、模型、数据的托管、展示与应用协同服务,为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作,由全产业链共同建设、共同运营、共同享有,推动国产AI生态繁荣发展。
更多推荐


所有评论(0)