fix：新增例程
This commit is contained in:
36
基础智能/e10.距离传感器数据获取实验/Config.json
Normal file
36
基础智能/e10.距离传感器数据获取实验/Config.json
Normal file
@@ -0,0 +1,36 @@
|
||||
{
|
||||
"VisionSensors": [
|
||||
{
|
||||
"SeqID": 0,
|
||||
"TypeID": 1,
|
||||
"TargetCopter": 1,
|
||||
"TargetMountType": 0,
|
||||
"DataWidth": 640,
|
||||
"DataHeight": 480,
|
||||
"DataCheckFreq": 30,
|
||||
"SendProtocol": [1, 0, 0, 0, 0, 0, 0, 0],
|
||||
"CameraFOV": 90,
|
||||
"EularOrQuat": 0,
|
||||
"SensorPosXYZ": [0.3, 0, 0],
|
||||
"SensorAngQuat": [0, 0, 0, 0],
|
||||
"SensorAngEular": [0, 0, 0],
|
||||
"otherParams": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
},
|
||||
{
|
||||
"SeqID": 0,
|
||||
"TypeID": 5,
|
||||
"TargetCopter": 0,
|
||||
"TargetMountType": 4,
|
||||
"DataWidth": 640,
|
||||
"DataHeight": 480,
|
||||
"DataCheckFreq": 30,
|
||||
"EularOrQuat": 0,
|
||||
"SendProtocol": [1, 0, 0, 0, 0, 0, 0, 0],
|
||||
"CameraFOV": 90,
|
||||
"SensorPosXYZ": [0, 0, 0],
|
||||
"SensorAngQuat": [0, 0, 0, 0],
|
||||
"SensorAngEular": [0, 0, 0],
|
||||
"otherParams": [200, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||
}
|
||||
]
|
||||
}
|
||||
5
基础智能/e10.距离传感器数据获取实验/Python38Run.bat
Normal file
5
基础智能/e10.距离传感器数据获取实验/Python38Run.bat
Normal file
@@ -0,0 +1,5 @@
|
||||
if not defined PSP_PATH (
|
||||
SET PSP_PATH=C:\PX4PSP
|
||||
SET PSP_PATH_LINUX=/mnt/c/PX4PSP
|
||||
)
|
||||
start cmd.exe /k "echo Python3.8 environment has been set with openCV+pymavlink+numpy+pyulog etc. && echo You can use pip or pip3 command to install other libraries && echo Put Python38Run.bat into your code folder && echo Use the command: 'python XXX.py' to run the script with Python && SET PATH=%PSP_PATH%\Python38;%PSP_PATH%\Python38\Scripts;%CD%;%PATH%"
|
||||
162
基础智能/e10.距离传感器数据获取实验/RangingImage.bat
Normal file
162
基础智能/e10.距离传感器数据获取实验/RangingImage.bat
Normal file
@@ -0,0 +1,162 @@
|
||||
@ECHO OFF
|
||||
|
||||
REM Run script as administrator
|
||||
%1 mshta vbscript:CreateObject("Shell.Application").ShellExecute("cmd.exe","/c ""%~s0"" ::","","runas",1)(window.close)&&exit
|
||||
|
||||
|
||||
REM The text start with 'REM' is annotation, the following options are corresponding to Options on CopterSim
|
||||
|
||||
REM Start index of vehicle number (should larger than 0)
|
||||
REM This option is useful for simulation with multi-computers
|
||||
SET /a START_INDEX=1
|
||||
|
||||
|
||||
REM Set the vehicleType/ClassID of vehicle 3D display in RflySim3D
|
||||
SET /a CLASS_3D_ID=-1
|
||||
|
||||
REM Set use DLL model name or not, use number index or name string
|
||||
REM This option is useful for simulation with other types of vehicles instead of multicopters
|
||||
set DLLModel=0
|
||||
|
||||
REM Set the simulation mode on CopterSim, use number index or name string
|
||||
REM e.g., SimMode=2 equals to SimMode=PX4_SITL_RFLY
|
||||
set SimMode=2
|
||||
|
||||
REM Set the vehicle-model (airframe) of PX4 SITL simulation, the default airframe is a quadcopter: iris
|
||||
REM Check folder Firmware\ROMFS\px4fmu_common\init.d-posix for supported airframes (Note: You can also create your airframe file here)
|
||||
REM E.g., fixed-wing aircraft: PX4SitlFrame=plane; small cars: PX4SitlFrame=rover
|
||||
set PX4SitlFrame=iris
|
||||
|
||||
|
||||
REM Set the map, use index or name of the map on CopterSim
|
||||
REM e.g., UE4_MAP=1 equals to UE4_MAP=Grasslands
|
||||
SET UE4_MAP=NeighborhoodPark
|
||||
|
||||
REM Set the origin x,y position (m) and yaw angle (degree) at the map
|
||||
SET /a ORIGIN_POS_X=0
|
||||
SET /a ORIGIN_POS_Y=0
|
||||
SET /a ORIGIN_YAW=0
|
||||
|
||||
REM Set the interval between two vehicle, unit:m
|
||||
SET /a VEHICLE_INTERVAL=2
|
||||
|
||||
|
||||
REM Set broadcast to other computer; 0: only this computer, 1: broadcast; or use IP address to increase speed
|
||||
REM e.g., IS_BROADCAST=0 equals to IS_BROADCAST=127.0.0.1, IS_BROADCAST=1 equals to IS_BROADCAST=255.255.255.255
|
||||
SET IS_BROADCAST=0
|
||||
|
||||
REM Set UDP data mode; 0: UDP_FULL, 1:UDP_Simple, 2: Mavlink_Full, 3: Mavlink_simple. input number or string
|
||||
REM 4:Mavlink_NoSend, 5:Mavlink_NoGPS, 6:Mavlink_Vision (NoGPS and set PX4 EKF)
|
||||
SET UDPSIMMODE=2
|
||||
|
||||
REM Set the path of the RflySim tools
|
||||
if not defined PSP_PATH (
|
||||
SET PSP_PATH=C:\PX4PSP
|
||||
SET PSP_PATH_LINUX=/mnt/c/PX4PSP
|
||||
)
|
||||
|
||||
:Top
|
||||
ECHO.
|
||||
ECHO ---------------------------------------
|
||||
REM Max vehicle number 50
|
||||
SET /a MAX_VEHICLE=50
|
||||
REM SET /P VehicleNum=Please input UAV swarm number:
|
||||
SET /A VehicleNum=1
|
||||
SET /A Evaluated=VehicleNum
|
||||
if %Evaluated% EQU %VehicleNum% (
|
||||
IF %VehicleNum% GTR 0 (
|
||||
IF %VehicleNum% GTR %MAX_VEHICLE% (
|
||||
ECHO The vehicle number is too large, which may cause a crash
|
||||
pause
|
||||
)
|
||||
GOTO StartSim
|
||||
)
|
||||
ECHO Not a positive integer
|
||||
GOTO Top
|
||||
) ELSE (
|
||||
ECHO Not a integer
|
||||
GOTO Top
|
||||
)
|
||||
:StartSim
|
||||
|
||||
SET /A VehicleTotalNum=%VehicleNum% + %START_INDEX% - 1
|
||||
if not defined TOTOAL_COPTER (
|
||||
SET /A TOTOAL_COPTER=%VehicleTotalNum%
|
||||
)
|
||||
|
||||
set /a sqrtNum=1
|
||||
set /a squareNum=1
|
||||
:loopSqrt
|
||||
set /a squareNum=%sqrtNum% * %sqrtNum%
|
||||
if %squareNum% EQU %TOTOAL_COPTER% (
|
||||
goto loopSqrtEnd
|
||||
)
|
||||
if %squareNum% GTR %TOTOAL_COPTER% (
|
||||
goto loopSqrtEnd
|
||||
)
|
||||
set /a sqrtNum=%sqrtNum%+1
|
||||
goto loopSqrt
|
||||
:loopSqrtEnd
|
||||
|
||||
|
||||
REM QGCPath
|
||||
tasklist|find /i "QGroundControl.exe" || start %PSP_PATH%\QGroundControl\QGroundControl.exe -noComPix
|
||||
ECHO Start QGroundControl
|
||||
|
||||
REM UE4Path
|
||||
cd /d %PSP_PATH%\RflySim3D
|
||||
tasklist|find /i "RflySim3D.exe" || start %PSP_PATH%\Rflysim3D\RflySim3D.exe
|
||||
choice /t 5 /d y /n >nul
|
||||
|
||||
|
||||
tasklist|find /i "CopterSim.exe" && taskkill /im "CopterSim.exe"
|
||||
ECHO Kill all CopterSims
|
||||
|
||||
|
||||
REM CptSmPath
|
||||
cd /d %PSP_PATH%\CopterSim
|
||||
|
||||
|
||||
set /a cntr=%START_INDEX%
|
||||
set /a endNum=%VehicleTotalNum%+1
|
||||
|
||||
:loopBegin
|
||||
set /a PosXX=((%cntr%-1) / %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_X%
|
||||
set /a PosYY=((%cntr%-1) %% %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_Y%
|
||||
start /realtime CopterSim.exe 1 %cntr% %CLASS_3D_ID% %DLLModel% %SimMode% %UE4_MAP% %IS_BROADCAST% %PosXX% %PosYY% %ORIGIN_YAW% 1 %UDPSIMMODE%
|
||||
choice /t 1 /d y /n >nul
|
||||
set /a cntr=%cntr%+1
|
||||
|
||||
if %cntr% EQU %endNum% goto loopEnd
|
||||
goto loopBegin
|
||||
:loopEnd
|
||||
|
||||
REM Set ToolChainType 1:Win10WSL 3:Cygwin
|
||||
SET /a ToolChainType=1
|
||||
|
||||
if "%IS_BROADCAST%" == "0" (
|
||||
SET IS_BROADCAST=0
|
||||
) else (
|
||||
SET IS_BROADCAST=1
|
||||
)
|
||||
|
||||
SET WINDOWSPATH=%PATH%
|
||||
if %ToolChainType% EQU 1 (
|
||||
wsl -d RflySim-20.04 echo Starting PX4 Build; cd %PSP_PATH_LINUX%/Firmware; ./BkFile/EnvOri.sh; export PATH=$HOME/ninja:$HOME/gcc-arm-none-eabi-7-2017-q4-major/bin:$PATH;make px4_sitl_default; ./Tools/sitl_multiple_run_rfly.sh %VehicleNum% %START_INDEX% %PX4SitlFrame%;echo Press any key to exit; read -n 1
|
||||
) else (
|
||||
REM CYGPath
|
||||
cd /d %PSP_PATH%\CygwinToolchain
|
||||
CALL setPX4Env.bat
|
||||
bash -l -i -c 'echo Starting SITL SIMULATION; cd %PSP_PATH_LINUX%/Firmware; ./BkFile/EnvOri.sh; pwd; make px4_sitl_default; ./Tools/sitl_multiple_run_rfly.sh %VehicleNum% %START_INDEX% %PX4SitlFrame%;echo Press any key to exit; read -n 1; pkill -x px4 || true;'
|
||||
)
|
||||
SET PATH=%WINDOWSPATH%
|
||||
|
||||
|
||||
REM kill all applications when press a key
|
||||
tasklist|find /i "CopterSim.exe" && taskkill /im "CopterSim.exe"
|
||||
tasklist|find /i "QGroundControl.exe" && taskkill /f /im "QGroundControl.exe"
|
||||
tasklist|find /i "RflySim3D.exe" && taskkill /f /im "RflySim3D.exe"
|
||||
tasklist|find /i "python.exe" && taskkill /f /im "python.exe"
|
||||
tasklist|find /i "cmd.exe" && taskkill /f /im "cmd.exe"
|
||||
|
||||
ECHO Start End.
|
||||
131
基础智能/e10.距离传感器数据获取实验/RangingImage.py
Normal file
131
基础智能/e10.距离传感器数据获取实验/RangingImage.py
Normal file
@@ -0,0 +1,131 @@
|
||||
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]
|
||||
BIN
基础智能/e10.距离传感器数据获取实验/Readme.pdf
Normal file
BIN
基础智能/e10.距离传感器数据获取实验/Readme.pdf
Normal file
Binary file not shown.
12
基础智能/e10.距离传感器数据获取实验/WinWSL.bat
Normal file
12
基础智能/e10.距离传感器数据获取实验/WinWSL.bat
Normal file
@@ -0,0 +1,12 @@
|
||||
@echo off
|
||||
REM Set the path of the RflySim tools
|
||||
if not defined PSP_PATH (
|
||||
SET PSP_PATH=C:\PX4PSP
|
||||
SET PSP_PATH_LINUX=/mnt/c/PX4PSP
|
||||
)
|
||||
cd /d %PSP_PATH%\VcXsrv
|
||||
tasklist|find /i "vcxsrv.exe" >nul || Xlaunch.exe -run config1.xlaunch
|
||||
|
||||
cd /d %~dp0
|
||||
wsl -d RflySim-20.04
|
||||
|
||||
0
基础智能/e10.距离传感器数据获取实验/名称-测距传感器实验.txt
Normal file
0
基础智能/e10.距离传感器数据获取实验/名称-测距传感器实验.txt
Normal file
Reference in New Issue
Block a user