fix：新增例程
This commit is contained in:
20
基础智能/e13.四旋翼无人机穿环实验/Config1.json
Normal file
20
基础智能/e13.四旋翼无人机穿环实验/Config1.json
Normal file
@@ -0,0 +1,20 @@
|
||||
{
|
||||
"VisionSensors":[
|
||||
{
|
||||
"SeqID":0,
|
||||
"TypeID":1,
|
||||
"TargetCopter":1,
|
||||
"TargetMountType":0,
|
||||
"DataWidth":720,
|
||||
"DataHeight":405,
|
||||
"DataCheckFreq":30,
|
||||
"SendProtocol":[1,127,0,0,1,9999,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]
|
||||
}
|
||||
]
|
||||
}
|
||||
20
基础智能/e13.四旋翼无人机穿环实验/Config2.json
Normal file
20
基础智能/e13.四旋翼无人机穿环实验/Config2.json
Normal file
@@ -0,0 +1,20 @@
|
||||
{
|
||||
"VisionSensors":[
|
||||
{
|
||||
"SeqID":1,
|
||||
"TypeID":1,
|
||||
"TargetCopter":2,
|
||||
"TargetMountType":0,
|
||||
"DataWidth":720,
|
||||
"DataHeight":405,
|
||||
"DataCheckFreq":30,
|
||||
"SendProtocol":[1,127,0,0,1,10000,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]
|
||||
}
|
||||
]
|
||||
}
|
||||
177
基础智能/e13.四旋翼无人机穿环实验/CrossRing2HITL.bat
Normal file
177
基础智能/e13.四旋翼无人机穿环实验/CrossRing2HITL.bat
Normal file
@@ -0,0 +1,177 @@
|
||||
@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., 0:PX4_HITL, 1:PX4_SITL, 2:PX4_SITL_RFLY, 3:"Simulink&DLL_SIL", 4:PX4_HITL_NET, 5:EXT_HITL_COM, 6:EXT_SIM_NET
|
||||
set SimMode=0
|
||||
|
||||
REM SET the baudrate for Pixhawk COM
|
||||
SET /a BaudRate=921600
|
||||
|
||||
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=VisionRing
|
||||
|
||||
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
|
||||
)
|
||||
|
||||
|
||||
|
||||
ECHO.
|
||||
ECHO ---------------------------------------
|
||||
REM Get the Com port number
|
||||
for /f "delims=" %%t in ('%PSP_PATH%\CopterSim\GetComList.exe 3 %BaudRate%') do set ComInfoStr=%%t
|
||||
|
||||
for /f "tokens=1,2,3 delims=#" %%a in ("%ComInfoStr%") do (
|
||||
set ComNumExe=%%a
|
||||
set ComNameList=%%b
|
||||
set ComInfoList=%%c
|
||||
)
|
||||
echo Please input the Pixhawk COM port list for HIL
|
||||
echo Use ',' as the separator if more than one Pixhawk
|
||||
echo E.g., input 3 for COM3 of Pixhawk on the computer
|
||||
echo Input 3,6,7 for COM3, COM6 and COM7 of Pixhawks
|
||||
|
||||
ECHO ---------------------------------------
|
||||
set remain=%ComInfoList%
|
||||
echo All COM ports on this computer are:
|
||||
echo.
|
||||
:loopInfo
|
||||
for /f "tokens=1* delims=;" %%a in ("%remain%") do (
|
||||
echo %%a
|
||||
set remain=%%b
|
||||
)
|
||||
if defined remain goto :loopInfo
|
||||
echo.
|
||||
ECHO ---------------------------------------
|
||||
|
||||
if %ComNumExe% EQU 0 (
|
||||
echo Warning: there is no available COM port
|
||||
) else (
|
||||
echo Recommended COM list input is: %ComNameList%
|
||||
)
|
||||
|
||||
|
||||
ECHO.
|
||||
ECHO ---------------------------------------
|
||||
SET /P ComNum=My COM list for HITL simulation is:
|
||||
SET string=%ComNum%
|
||||
set subStr = ""
|
||||
set /a VehicleNum=0
|
||||
:split
|
||||
for /f "tokens=1,* delims=," %%i in ("%string%") do (
|
||||
set subStr=%%i
|
||||
set string=%%j
|
||||
)
|
||||
set /a eValue=subStr
|
||||
if not %eValue% EQU %subStr% (
|
||||
echo Error: Input '%subStr%' is not a integer!
|
||||
goto EOF
|
||||
)
|
||||
set /a VehicleNum = VehicleNum +1
|
||||
if not "%string%"=="" goto split
|
||||
REM cho total com number is %VehicleNum%
|
||||
|
||||
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 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
|
||||
|
||||
SET string=%ComNum%
|
||||
:split1
|
||||
for /f "tokens=1,* delims=," %%i in ("%string%") do (
|
||||
set subStr=%%i
|
||||
set string=%%j
|
||||
)
|
||||
set /a PosXX=((%cntr%-1) / %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_X%
|
||||
set /a PosYY=((%cntr%-1) %% %sqrtNum%)*%VEHICLE_INTERVAL% + %ORIGIN_POS_Y%
|
||||
REM echo start CopterSim
|
||||
start /realtime CopterSim.exe 1 %cntr% %CLASS_3D_ID% %DLLModel% %SimMode% %UE4_MAP% %IS_BROADCAST% %PosXX% %PosYY% %ORIGIN_YAW% %subStr%:%BaudRate% %UDPSIMMODE%
|
||||
choice /t 2 /d y /n >nul
|
||||
set /a cntr=%cntr%+1
|
||||
|
||||
REM TIMEOUT /T 1
|
||||
if not "%string%"=="" goto split1
|
||||
|
||||
REM QGCPath
|
||||
tasklist|find /i "QGroundControl.exe" || start %PSP_PATH%\QGroundControl\QGroundControl.exe -noComPix
|
||||
ECHO Start QGroundControl
|
||||
|
||||
pause
|
||||
|
||||
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.
|
||||
146
基础智能/e13.四旋翼无人机穿环实验/CrossRing2SITL.bat
Normal file
146
基础智能/e13.四旋翼无人机穿环实验/CrossRing2SITL.bat
Normal file
@@ -0,0 +1,146 @@
|
||||
@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=VisionRing
|
||||
|
||||
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=2
|
||||
|
||||
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.
|
||||
200
基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle1.py
Normal file
200
基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle1.py
Normal file
@@ -0,0 +1,200 @@
|
||||
|
||||
# import required libraries
|
||||
import time
|
||||
import numpy as np
|
||||
import cv2
|
||||
import sys
|
||||
|
||||
# import RflySim APIs
|
||||
import PX4MavCtrlV4 as PX4MavCtrl
|
||||
import VisionCaptureApi
|
||||
import UE4CtrlAPI
|
||||
ue = UE4CtrlAPI.UE4CtrlAPI()
|
||||
|
||||
vis = VisionCaptureApi.VisionCaptureApi()
|
||||
# VisionCaptureApi 中的配置函数
|
||||
vis.jsonLoad(0,'Config1.json') # 使用共享内存方式，并加载Config1.json中的传感器配置文件
|
||||
mav = PX4MavCtrl.PX4MavCtrler(1) #对应1号飞机端口
|
||||
|
||||
mav.InitMavLoop()
|
||||
print("Simulation Start.")
|
||||
|
||||
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
|
||||
if not isSuss: # 如果请求取图失败，则退出
|
||||
sys.exit(0)
|
||||
vis.startImgCap(True) # 开启取图，并启用共享内存图像转发，转发到填写的目录
|
||||
|
||||
# Send command to UE4 Window 1 to change resolution
|
||||
ue.sendUE4Cmd('r.setres 720x405w',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
|
||||
ue.sendUE4Cmd('t.MaxFPS 30',0) # 设置UE4最大刷新频率，同时也是取图频率
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
width = 720
|
||||
height = 405
|
||||
channel = 4
|
||||
|
||||
# define same functions for computaion
|
||||
def angle_cos(p0, p1, p2):
|
||||
d1, d2 = (p0-p1).astype('float'), (p2-p1).astype('float')
|
||||
return abs( np.dot(d1, d2) / np.sqrt( np.dot(d1, d1)*np.dot(d2, d2) ) )
|
||||
|
||||
def diagonal_check(p):
|
||||
d1 = np.sqrt(np.dot(p[0]-p[2], p[0]-p[2]))
|
||||
d2 = np.sqrt(np.dot(p[1]-p[3], p[1]-p[3]))
|
||||
return abs(d1-d2)*1.0/d1
|
||||
|
||||
def saturationYawRate(yaw_rate):
|
||||
yr_bound = 20.0
|
||||
if yaw_rate > yr_bound:
|
||||
yaw_rate = yr_bound
|
||||
if yaw_rate < -yr_bound:
|
||||
yaw_rate = -yr_bound
|
||||
return yaw_rate
|
||||
|
||||
def taskChange(pos_x):
|
||||
if pos_x < 40:
|
||||
task = "range1"
|
||||
elif pos_x <70:
|
||||
task = "range2"
|
||||
elif pos_x < 130:
|
||||
task = "range3"
|
||||
elif pos_x < 140:
|
||||
task = "land"
|
||||
else:
|
||||
task = "finish"
|
||||
return task
|
||||
|
||||
def sat(inPwm,thres=1):
|
||||
outPwm= inPwm
|
||||
if inPwm>thres:
|
||||
outPwm = thres
|
||||
elif inPwm<-thres:
|
||||
outPwm = -thres
|
||||
return outPwm
|
||||
|
||||
|
||||
# object detect function
|
||||
def objectDetect(task):
|
||||
"""According task to detect objects"""
|
||||
if vis.hasData[0]:
|
||||
img_bgr=vis.Img[0]
|
||||
else:
|
||||
return -1,-1,-1
|
||||
b,g,r = cv2.split(img_bgr)
|
||||
img_edge = cv2.Canny(b, 50, 100)
|
||||
if task == "range1" or task == "range2":
|
||||
return circleDetect(img_bgr, img_edge, b)
|
||||
else:
|
||||
return squareDetect(img_bgr, img_edge)
|
||||
|
||||
|
||||
# square detect for object detect function
|
||||
def squareDetect(img_bgr, img_edge):
|
||||
"""Detect Square with PolyDP and diagonal length"""
|
||||
# find contours
|
||||
squares = []
|
||||
cnts, hierarchy = cv2.findContours(img_edge, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
|
||||
for cnt in cnts:
|
||||
cnt_len = cv2.arcLength(cnt, True)
|
||||
cnt = cv2.approxPolyDP(cnt, 0.05 * cnt_len, True)
|
||||
if len(cnt) == 4 and cv2.contourArea(cnt) > 2000 and cv2.isContourConvex(cnt):
|
||||
cnt = cnt.reshape(-1, 2)
|
||||
diag_delta = diagonal_check(cnt)
|
||||
if diag_delta < 0.2:
|
||||
squares.append(cnt)
|
||||
cv2.drawContours( img_bgr, squares, -1, (0, 255, 0), 3)
|
||||
cv2.imshow("img_bgr", img_bgr)
|
||||
|
||||
cv2.waitKey(1)
|
||||
height, width, channel = img_bgr.shape
|
||||
if squares:
|
||||
return (squares[0][0][0]+squares[0][2][0])/2 - width/2, (squares[0][0][1]+squares[0][2][1])/2 - height/2, np.sqrt(np.dot(squares[0][0]-squares[0][2], squares[0][0]-squares[0][2]))
|
||||
else:
|
||||
return -1,-1,-1
|
||||
|
||||
# circle detect for object detect function
|
||||
def circleDetect(img_bgr, img_edge, img_b):
|
||||
"""Hough Circle detect"""
|
||||
circles = cv2.HoughCircles(img_b, cv2.HOUGH_GRADIENT, 1, 20, param1=100, param2=30, minRadius=0, maxRadius=0)
|
||||
if circles is not None:
|
||||
circles = np.uint16(np.around(circles))
|
||||
obj = circles[0, 0]
|
||||
cv2.circle(img_bgr, (obj[0], obj[1]), obj[2], (0,255,0), 2)
|
||||
cv2.circle(img_bgr, (obj[0], obj[1]), 2, (0,255,255), 3)
|
||||
cv2.imshow("img_bgr", img_bgr)
|
||||
|
||||
cv2.waitKey(1)
|
||||
height, width, channel = img_bgr.shape
|
||||
return obj[0]-width/2, obj[1]-height/2, obj[2]
|
||||
else:
|
||||
return -1,-1,-1
|
||||
|
||||
|
||||
# approaching Objective/ crossing rings algorithm
|
||||
def approachObjective():
|
||||
# 0. parameters
|
||||
# some parameters that work:(0.03, -0.03, 1, 5.0); (0.06, -0.04, 1, 10.0)
|
||||
K_z = 0.004 * 640 / height
|
||||
K_yawrate = 0.005 * 480 / width
|
||||
task = "range1"
|
||||
# 1. start
|
||||
startAppTime= time.time()
|
||||
lastTime = time.time()
|
||||
# time interval of the timer
|
||||
timeInterval = 1/30.0 #here is 0.0333s (30Hz)
|
||||
while (task != "finish") & (task != "land"):
|
||||
lastTime = lastTime + timeInterval
|
||||
sleepTime = lastTime - time.time()
|
||||
if sleepTime > 0:
|
||||
time.sleep(sleepTime) # sleep until the desired clock
|
||||
else:
|
||||
lastTime = time.time()
|
||||
# The following code will be executed 30Hz (0.0333s)
|
||||
|
||||
# 1.1. detect object and get error with center of image
|
||||
ex, ey, r = objectDetect(task)
|
||||
# 1.2. where is the drone
|
||||
pos_x = mav.uavPosNED[0]
|
||||
#print(time.time())
|
||||
# 1.3. update task
|
||||
task = taskChange(pos_x)
|
||||
# 1.4. attack
|
||||
if ex != -1:
|
||||
vx = sat(time.time()-startAppTime,3)
|
||||
vy = 0
|
||||
vz = K_z * ey
|
||||
yawrate = K_yawrate * ex
|
||||
mav.SendVelFRD(vx, vy, vz, yawrate)
|
||||
|
||||
lastTime = time.time()
|
||||
while task == "land":
|
||||
lastTime = lastTime + timeInterval
|
||||
sleepTime = lastTime - time.time()
|
||||
if sleepTime > 0:
|
||||
time.sleep(sleepTime) # sleep until the desired clock
|
||||
else:
|
||||
lastTime = time.time()
|
||||
|
||||
pos_x = mav.uavPosNED[0]
|
||||
task = taskChange(pos_x)
|
||||
mav.SendVelFRD(0, 0, 1, 0)
|
||||
|
||||
print("Enter Offboard mode.")
|
||||
time.sleep(5)
|
||||
mav.initOffboard()
|
||||
time.sleep(0.5)
|
||||
mav.SendMavArm(True) # Arm the drone
|
||||
mav.SendPosNED(5, 0, -5, 0) # Fly to target position 5,0，-5
|
||||
|
||||
# Show CV image and set the position
|
||||
if vis.hasData[0]:
|
||||
img_bgr=vis.Img[0]
|
||||
cv2.imshow("img_bgr", img_bgr)
|
||||
cv2.waitKey(1)
|
||||
#time.sleep(0.5)
|
||||
|
||||
time.sleep(5)
|
||||
|
||||
# start crossing ring task
|
||||
approachObjective()
|
||||
203
基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle2.py
Normal file
203
基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle2.py
Normal file
@@ -0,0 +1,203 @@
|
||||
|
||||
# import required libraries
|
||||
import time
|
||||
import numpy as np
|
||||
import cv2
|
||||
import sys
|
||||
|
||||
# import RflySim APIs
|
||||
import PX4MavCtrlV4 as PX4MavCtrl
|
||||
import VisionCaptureApi
|
||||
import UE4CtrlAPI
|
||||
ue = UE4CtrlAPI.UE4CtrlAPI()
|
||||
|
||||
vis = VisionCaptureApi.VisionCaptureApi()
|
||||
# VisionCaptureApi 中的配置函数
|
||||
vis.jsonLoad(0,'Config2.json') # 使用共享内存方式，并加载Config2.json中的传感器配置文件
|
||||
mav = PX4MavCtrl.PX4MavCtrler(2) #对应2号飞机端口
|
||||
|
||||
mav.InitMavLoop()
|
||||
print("Simulation Start.")
|
||||
|
||||
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
|
||||
if not isSuss: # 如果请求取图失败，则退出
|
||||
sys.exit(0)
|
||||
vis.startImgCap(True) # 开启取图，并启用共享内存图像转发，转发到填写的目录
|
||||
|
||||
# Send command to UE4 Window 1 to change resolution
|
||||
ue.sendUE4Cmd('r.setres 720x405w',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
|
||||
ue.sendUE4Cmd('t.MaxFPS 30',0) # 设置UE4最大刷新频率，同时也是取图频率
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
width = 720
|
||||
height = 405
|
||||
channel = 4
|
||||
|
||||
# define same functions for computaion
|
||||
def angle_cos(p0, p1, p2):
|
||||
d1, d2 = (p0-p1).astype('float'), (p2-p1).astype('float')
|
||||
return abs( np.dot(d1, d2) / np.sqrt( np.dot(d1, d1)*np.dot(d2, d2) ) )
|
||||
|
||||
def diagonal_check(p):
|
||||
d1 = np.sqrt(np.dot(p[0]-p[2], p[0]-p[2]))
|
||||
d2 = np.sqrt(np.dot(p[1]-p[3], p[1]-p[3]))
|
||||
return abs(d1-d2)*1.0/d1
|
||||
|
||||
def saturationYawRate(yaw_rate):
|
||||
yr_bound = 20.0
|
||||
if yaw_rate > yr_bound:
|
||||
yaw_rate = yr_bound
|
||||
if yaw_rate < -yr_bound:
|
||||
yaw_rate = -yr_bound
|
||||
return yaw_rate
|
||||
|
||||
def taskChange(pos_x):
|
||||
if pos_x < 40:
|
||||
task = "range1"
|
||||
elif pos_x <70:
|
||||
task = "range2"
|
||||
elif pos_x < 130:
|
||||
task = "range3"
|
||||
elif pos_x < 140:
|
||||
task = "land"
|
||||
else:
|
||||
task = "finish"
|
||||
return task
|
||||
|
||||
def sat(inPwm,thres=1):
|
||||
outPwm= inPwm
|
||||
if inPwm>thres:
|
||||
outPwm = thres
|
||||
elif inPwm<-thres:
|
||||
outPwm = -thres
|
||||
return outPwm
|
||||
|
||||
|
||||
# object detect function
|
||||
def objectDetect(task):
|
||||
"""According task to detect objects"""
|
||||
if vis.hasData[0]:
|
||||
img_bgr=vis.Img[0]
|
||||
else:
|
||||
return -1,-1,-1
|
||||
b,g,r = cv2.split(img_bgr)
|
||||
img_edge = cv2.Canny(b, 50, 100)
|
||||
if task == "range1" or task == "range2":
|
||||
return circleDetect(img_bgr, img_edge, b)
|
||||
else:
|
||||
return squareDetect(img_bgr, img_edge)
|
||||
|
||||
|
||||
# square detect for object detect function
|
||||
def squareDetect(img_bgr, img_edge):
|
||||
"""Detect Square with PolyDP and diagonal length"""
|
||||
# find contours
|
||||
squares = []
|
||||
cnts, hierarchy = cv2.findContours(img_edge, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
|
||||
for cnt in cnts:
|
||||
cnt_len = cv2.arcLength(cnt, True)
|
||||
cnt = cv2.approxPolyDP(cnt, 0.05 * cnt_len, True)
|
||||
if len(cnt) == 4 and cv2.contourArea(cnt) > 2000 and cv2.isContourConvex(cnt):
|
||||
cnt = cnt.reshape(-1, 2)
|
||||
diag_delta = diagonal_check(cnt)
|
||||
if diag_delta < 0.2:
|
||||
squares.append(cnt)
|
||||
cv2.drawContours( img_bgr, squares, -1, (0, 255, 0), 3)
|
||||
cv2.imshow("img_bgr", img_bgr)
|
||||
|
||||
cv2.waitKey(1)
|
||||
height, width, channel = img_bgr.shape
|
||||
if squares:
|
||||
return (squares[0][0][0]+squares[0][2][0])/2 - width/2, (squares[0][0][1]+squares[0][2][1])/2 - height/2, np.sqrt(np.dot(squares[0][0]-squares[0][2], squares[0][0]-squares[0][2]))
|
||||
else:
|
||||
return -1,-1,-1
|
||||
|
||||
# circle detect for object detect function
|
||||
def circleDetect(img_bgr, img_edge, img_b):
|
||||
"""Hough Circle detect"""
|
||||
circles = cv2.HoughCircles(img_b, cv2.HOUGH_GRADIENT, 1, 20, param1=100, param2=30, minRadius=0, maxRadius=0)
|
||||
if circles is not None:
|
||||
circles = np.uint16(np.around(circles))
|
||||
obj = circles[0, 0]
|
||||
cv2.circle(img_bgr, (obj[0], obj[1]), obj[2], (0,255,0), 2)
|
||||
cv2.circle(img_bgr, (obj[0], obj[1]), 2, (0,255,255), 3)
|
||||
cv2.imshow("img_bgr", img_bgr)
|
||||
|
||||
cv2.waitKey(1)
|
||||
height, width, channel = img_bgr.shape
|
||||
return obj[0]-width/2, obj[1]-height/2, obj[2]
|
||||
else:
|
||||
return -1,-1,-1
|
||||
|
||||
|
||||
# approaching Objective/ crossing rings algorithm
|
||||
def approachObjective():
|
||||
# 0. parameters
|
||||
# some parameters that work:(0.03, -0.03, 1, 5.0); (0.06, -0.04, 1, 10.0)
|
||||
K_z = 0.004 * 640 / height
|
||||
K_yawrate = 0.005 * 480 / width
|
||||
task = "range1"
|
||||
# 1. start
|
||||
startAppTime= time.time()
|
||||
lastTime = time.time()
|
||||
# time interval of the timer
|
||||
timeInterval = 1/30.0 #here is 0.0333s (30Hz)
|
||||
while (task != "finish") & (task != "land"):
|
||||
lastTime = lastTime + timeInterval
|
||||
sleepTime = lastTime - time.time()
|
||||
if sleepTime > 0:
|
||||
time.sleep(sleepTime) # sleep until the desired clock
|
||||
else:
|
||||
lastTime = time.time()
|
||||
# The following code will be executed 30Hz (0.0333s)
|
||||
|
||||
# 1.1. detect object and get error with center of image
|
||||
ex, ey, r = objectDetect(task)
|
||||
# 1.2. where is the drone
|
||||
pos_x = mav.uavPosNED[0]
|
||||
#print(time.time())
|
||||
# 1.3. update task
|
||||
task = taskChange(pos_x)
|
||||
# 1.4. attack
|
||||
if ex != -1:
|
||||
vx = sat(time.time()-startAppTime,3)
|
||||
vy = 0
|
||||
vz = K_z * ey
|
||||
yawrate = K_yawrate * ex
|
||||
mav.SendVelFRD(vx, vy, vz, yawrate)
|
||||
|
||||
lastTime = time.time()
|
||||
while task == "land":
|
||||
lastTime = lastTime + timeInterval
|
||||
sleepTime = lastTime - time.time()
|
||||
if sleepTime > 0:
|
||||
time.sleep(sleepTime) # sleep until the desired clock
|
||||
else:
|
||||
lastTime = time.time()
|
||||
|
||||
pos_x = mav.uavPosNED[0]
|
||||
task = taskChange(pos_x)
|
||||
mav.SendVelFRD(0, 0, 1, 0)
|
||||
|
||||
|
||||
|
||||
|
||||
print("Enter Offboard mode.")
|
||||
time.sleep(5)
|
||||
mav.initOffboard()
|
||||
time.sleep(0.5)
|
||||
mav.SendMavArm(True) # Arm the drone
|
||||
mav.SendPosNED(5, 0, -5, 0) # Fly to target position 5,0，-5
|
||||
|
||||
# Show CV image and set the position
|
||||
if vis.hasData[0]:
|
||||
img_bgr=vis.Img[0]
|
||||
cv2.imshow("img_bgr", img_bgr)
|
||||
cv2.waitKey(1)
|
||||
#time.sleep(0.5)
|
||||
|
||||
time.sleep(5)
|
||||
|
||||
# start crossing ring task
|
||||
approachObjective()
|
||||
5
基础智能/e13.四旋翼无人机穿环实验/Python38Run.bat
Normal file
5
基础智能/e13.四旋翼无人机穿环实验/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%"
|
||||
BIN
基础智能/e13.四旋翼无人机穿环实验/Readme.pdf
Normal file
BIN
基础智能/e13.四旋翼无人机穿环实验/Readme.pdf
Normal file
Binary file not shown.
12
基础智能/e13.四旋翼无人机穿环实验/readme.txt
Normal file
12
基础智能/e13.四旋翼无人机穿环实验/readme.txt
Normal file
@@ -0,0 +1,12 @@
|
||||
In English
|
||||
1. Double-click the "CrossRing3SITL.bat" to start two multicopters (two CopterSim and two RflySim3D windows). Wait for QGC showing that two vehicles are both in Hold mode.
|
||||
2. Double click the "Python38Run.bat" to open a Python38 Environment in this folder, and enter command "python CrossRing3_vehicle1.py" to start the first vehicle's crossing ring vision control task.
|
||||
3. A few seconds later, double click the "Python38Run.bat" to open a Python38 Environment in this folder, and enter command "python CrossRing3_vehicle2.py" to start the second vehicle's crossing ring vision control task.
|
||||
4. Now you can observe two vehicle crossing the ring in sequence.
|
||||
|
||||
|
||||
In Chinese
|
||||
1. 双击运行“CrossRing3SITL.bat”开启两个飞机（2个CopterSim和2个RflySim3D）
|
||||
2. 双击“Python38Run.bat”打开一个Python环境，等待QGC显示两个飞机都进入Hold模式，再运行命令“python CrossRing3_vehicle1.py”.
|
||||
3. 几秒钟后，双击“Python38Run.bat”再打开一个Python环境，再运行“python CrossRing3_vehicle2.py”
|
||||
4. 可以看到两个飞机依次起飞。
|
||||
0
基础智能/e13.四旋翼无人机穿环实验/名称-双无人机分布式控制.txt
Normal file
0
基础智能/e13.四旋翼无人机穿环实验/名称-双无人机分布式控制.txt
Normal file
Reference in New Issue
Block a user