sushu/RflySimhighschool
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix：新增例程

Browse Source
This commit is contained in:
sushu
2025-07-25 17:54:28 +08:00
parent 78481c265e
commit 637343d716
108 changed files with 5755 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

188
基础智能/e1.集群Python接口演示实验/HITLMAVLinkFull4Swarm.bat Normal file
View File

@@ -0,0 +1,188 @@
@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 Set the path of the RflySim tools
if not defined PSP_PATH (
SET PSP_PATH=C:\PX4PSP
SET PSP_PATH_LINUX=/mnt/c/PX4PSP
)
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 Auto determine CopterID according to SysID of Pixhawk
SET /a IsSysID=0
REM Total vehicle Number to auto arrange position
REM SET /a TOTOAL_COPTER=8
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 Check if DLLModel is a name string, if yes, copy the DLL file to CopterSim folder
SET /A DLLModelVal=DLLModel
if %DLLModelVal% NEQ %DLLModel% (
REM Copy the latest dll file to CopterSim folder
copy /Y "%~dp0"\%DLLModel%.dll %PSP_PATH%\CopterSim\external\model\%DLLModel%.dll
)
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=Grasslands
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; IS_BROADCAST=0: only this computer, IS_BROADCAST=1: broadcast;
REM or use IP address to increase speed, e.g., IS_BROADCAST=192.168.3.1
REM Note: in IP mode, IS_BROADCAST=0 equals to IS_BROADCAST=127.0.0.1, IS_BROADCAST=1 equals to IS_BROADCAST=255.255.255.255
REM You can also use a IP list with seperator "," or ";" to specify IPs to send, e.g., 127.0.0.1,192.168.1.4,192.168.1.5
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=Mavlink_Full
ECHO.
ECHO ---------------------------------------
REM Get the Com port number
for /f "delims=" %%t in ('%PSP_PATH%\CopterSim\GetComList.exe 2 %BaudRate%') do set ComNumExe=%%t
REM Get the Com port list
for /f "delims=" %%t in ('%PSP_PATH%\CopterSim\GetComList.exe 0 %BaudRate%') do set ComNameList=%%t
REM Get the Com port info
for /f "delims=" %%t in ('%PSP_PATH%\CopterSim\GetComList.exe 1 %BaudRate%') do set ComInfoList=%%t
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% %IsSysID%
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"
ECHO Start End.

135
基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.bat Normal file
View File

@@ -0,0 +1,135 @@
@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=Grasslands
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
)
SET /a VehicleNum=4
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
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"
ECHO Start End.

82
基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.py Normal file
View File

@@ -0,0 +1,82 @@
import time
import math
import sys
import PX4MavCtrlV4 as PX4MavCtrl
# Create four MAVLink API instances, the CopterSim UDP ports are 20100+2*i, where i=1,2,3,4
mav = PX4MavCtrl.PX4MavCtrler(1)
mav1 = PX4MavCtrl.PX4MavCtrler(2)
mav2 = PX4MavCtrl.PX4MavCtrler(3)
mav3 = PX4MavCtrl.PX4MavCtrler(4)
# Start MAVLink listening loop from CopterSim/Pixhawk
# MAVLink Full mode is adopted here
mav.InitMavLoop()
mav1.InitMavLoop()
mav2.InitMavLoop()
mav3.InitMavLoop()
time.sleep(2)
# Display the position, velocity, angle, angular rate from Pixhawk/CopterSIm
print((mav.uavPosNED,mav.uavVelNED,mav.uavAngEular,mav.uavAngRate,mav.uavPosGPSHome,mav.uavPosGPS,mav.uavGlobalPos))
print((mav1.uavPosNED,mav1.uavVelNED,mav1.uavAngEular,mav1.uavAngRate,mav1.uavPosGPSHome,mav1.uavPosGPS,mav1.uavGlobalPos))
print((mav2.uavPosNED,mav2.uavVelNED,mav2.uavAngEular,mav2.uavAngRate,mav2.uavPosGPSHome,mav2.uavPosGPS,mav2.uavGlobalPos))
print((mav3.uavPosNED,mav3.uavVelNED,mav3.uavAngEular,mav3.uavAngRate,mav3.uavPosGPSHome,mav3.uavPosGPS,mav3.uavGlobalPos))
# Start Offboard mode of Pixhawk
mav.initOffboard()
mav1.initOffboard()
mav2.initOffboard()
mav3.initOffboard()
# send desired poisition control command, fly to target postion 0,0,-1.7m with yaw 0 degree
mav.SendPosNED(0, 0, -1.7, 0)
mav1.SendPosNED(2, 2, -1.7, 0)
mav2.SendPosNED(-2, -2, -1.7, 0)
mav3.SendPosNED(0, 0, -3, 0)
print("Send target Pos")
time.sleep(0.5)
# Send arm command
mav.SendMavArm(True)
time.sleep(2)
mav1.SendMavArm(True)
time.sleep(2)
mav2.SendMavArm(True)
time.sleep(2)
mav3.SendMavArm(True)
print("Send Arm Command")
time.sleep(10)
# send desired NED velocity signals, 0.2m/s download
mav.SendVelNED(0, 0, 0.2, 0)
mav1.SendVelNED(0, 0, 0.2, 0)
mav2.SendVelNED(0, 0, 0.2, 0)
mav3.SendVelNED(0, 0, 0.2, 0)
print("Send Velocity Speed")
time.sleep(10)
# exit from Offboard control mode of Pixhawk
print("Send offboard stop")
mav.endOffboard()
mav1.endOffboard()
mav2.endOffboard()
mav3.endOffboard()
time.sleep(1)
# exit from MAVLink listening loop
print("Send Mavlink stop")
mav.stopRun()
mav1.stopRun()
mav2.stopRun()
mav3.stopRun()
time.sleep(1)
#while True:
# print(mav.uavPosNED)
# time.sleep(2)

5
基础智能/e1.集群Python接口演示实验/Python38Run.bat Normal file
View 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
基础智能/e1.集群Python接口演示实验/Readme.pdf Normal file
View File

Binary file not shown.

0
基础智能/e1.集群Python接口演示实验/名称-集群接口实验.txt Normal file
View File

36
基础智能/e10.距离传感器数据获取实验/Config.json Normal file
View 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
View 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
View 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
View 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
View File

Binary file not shown.

12
基础智能/e10.距离传感器数据获取实验/WinWSL.bat Normal file
View 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
View File

22
基础智能/e11.无人机跟随圆形案板移动实验/Config.json Normal file
View File

@@ -0,0 +1,22 @@
{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0，则自动递增排序。
"TypeID":1, //视觉传感器类型，1:RGB，2:深度，3:灰度，4:分割，5:测距，7:深度转点云，20-23:激光雷达，40-41:热力红外（收费版）
"TargetCopter":1, //传感器绑定的CopterID号，注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式，0:固定飞机几何中心， 1:固定飞机底部中心，2:固定地面上，3:弱固定飞机上姿态不随动，4:绑定其他视觉传感器上
"JointType":1,
"DataWidth":640, //图像像素长度
"DataHeight":480,//图像像素宽度
"DataCheckFreq":30,//图像检查更新频率，如果发现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.3,0,0], //视觉传感器的安装位置，和TargetMountType对应偏移中心，单位米
"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] //其他参数
}
]
}

5
基础智能/e11.无人机跟随圆形案板移动实验/Python38Run.bat Normal file
View 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
基础智能/e11.无人机跟随圆形案板移动实验/Readme.pdf Normal file
View File

Binary file not shown.

12
基础智能/e11.无人机跟随圆形案板移动实验/WinWSL.bat Normal file
View 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

20
基础智能/e11.无人机跟随圆形案板移动实验/WslGUI.bat Normal file
View File

@@ -0,0 +1,20 @@
@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
)
tasklist|find /i "vcxsrv.exe" >nul && taskkill /f /IM "vcxsrv.exe" >nul
tasklist|find /i "vcxsrv.exe" >nul && taskkill /f /IM "vcxsrv.exe" >nul
cd /d %PSP_PATH%\VcXsrv
Xlaunch.exe -run config.xlaunch
choice /t 1 /d y /n >nul
cd /d %~dp0
wsl -d RflySim-20.04 ~/StartUI.sh
choice /t 1 /d y /n >nul
tasklist|find /i "vcxsrv.exe" && taskkill /f /IM "vcxsrv.exe"

162
基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.bat Normal file
View 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=VisionRingBlank
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.

199
基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.py Normal file
View File

@@ -0,0 +1,199 @@
"""
视觉跟随圆移动控制飞机飞行。
圆形案板移动有前后，左右，上下移动
"""
import cv2
import threading
import time
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import sys
import numpy as np
import keyboard
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()
"""
初始化飞机，起飞,创建圆形目标
"""
vis = VisionCaptureApi.VisionCaptureApi()
# ue.sendUE4Cmd(b"r.setres 720x405w", 0)
ue.sendUE4Cmd(b"t.MaxFPS 30", 0)
ue.sendUE4PosNew(3, vehicleType=809, PosE=[2, 0, -2])
vis.jsonLoad()
time.sleep(1)
ue.sendUE4ExtAct(3, ActExt=[1, 0.8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
if not isSuss: # 如果请求取图失败，则退出
sys.exit(0)
vis.startImgCap() # 开启取图，并启用共享内存图像转发，转发到填写的目录
time.sleep(2)
mav = PX4MavCtrl.PX4MavCtrler()
mav.InitMavLoop()
time.sleep(2)
mav.initOffboard()
time.sleep(2)
mav.SendMavArm(True)
mav.SendPosNED(0, 0, -2, 0)
time.sleep(10)
detect_flag = False
"""
开辟线线程,控制圆形目标移动
"""
Circle_pose = [2, 0, -2, 0]
def MoveCircle(val):
"""
圆板目标移动没有旋转，只有平移，且始终朝向飞机
"""
global vis
global detect_flag
global CalCirle_pose
offset = 0.01 # 每次移动的距离
yaw_offset = np.deg2rad(1)
drone_pose = ue.getUE4Pos()
# if not detect_flag:
# time.sleep(0.001)
# continue
if val == "right": # 如果按键按下，这个值为1
# 向右移动
Circle_pose[1] += offset
if val == "left":
# 向左移动
Circle_pose[1] -= offset
if val == "up":
# 向上移动
Circle_pose[2] -= offset
if val == "down":
# 向下移动
Circle_pose[2] += offset
if val == "front":
# 向前移动
Circle_pose[0] += offset
if val == "back":
# 向后移动
Circle_pose[0] -= offset
ue.sendUE4PosNew(
3,
vehicleType=809,
PosE=[Circle_pose[0], Circle_pose[1], Circle_pose[2]],
AngEuler=[0, 0, Circle_pose[3]],
)
# move_circle_th = threading.Thread(target=MoveCircle)
# move_circle_th.start()
"""
开辟线程,获取图像,并控制飞机移动
"""
def CalCirle(img: np.array) -> tuple():
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
low_hsv = np.array([0, 43, 46])
high_hsv = np.array([10, 255, 255])
mask1 = cv2.inRange(hsv, lowerb=low_hsv, upperb=high_hsv)
lower2 = np.array([156, 43, 46])
upper2 = np.array([180, 255, 255])
mask2 = cv2.inRange(hsv, lower2, upper2)
mask = mask1 + mask2
nonzero = np.nonzero(mask)
if np.size(nonzero) == 0:
return [-1, -1], [-1, -1], [-1, -1]
row_min = np.min(nonzero[0])
row_max = np.max(nonzero[0])
col_min = np.min(nonzero[1])
col_max = np.max(nonzero[1])
mid_row = int((row_max + row_min) / 2)
mid_col = int((col_max + col_min) / 2)
cv2.line(
img,
(col_min, mid_row),
(col_max, mid_row),
(255, 255, 255),
1,
)
cv2.line(
img,
(mid_col, row_min),
(mid_col, row_max),
(255, 255, 255),
1,
)
cv2.imshow("img ", img)
cv2.waitKey(1)
# cv2.imshow("test", mask1 + mask2)
# cv2.waitKey(1)
return [col_min, col_max], [row_min, row_max], [mid_col, mid_row]
def CtrlDrone():
global vis
global mav
global detect_flag
img = None
init_dist_x = 0
init_dist_y = 0
img_cnt_x = 320
img_cnt_y = 240
tolerance_cnt = 15
tolerance_y = 15
tolerance_x = 20
kx = 0.015
ky = 0.009
kz = 0.005
while True:
if vis.hasData[0]:
img = vis.Img[0]
vis.hasData[0] = False
# cv2.imshow("img", vis.Img[0])
cv2.waitKey(1)
[x_min, x_max], [y_min, y_max], [cnt_x, cnt_y] = CalCirle(img)
if x_min == -1:
time.sleep(0.001)
continue
if not detect_flag:
detect_flag = True
dist_x = x_max - x_min
dist_y = y_max - y_min
if init_dist_x == 0:
init_dist_x = dist_x
if init_dist_y == 0:
init_dist_y = dist_y
vx = 0
vy = 0
vz = 0
yaw_rate = 0
if abs(cnt_x - img_cnt_x) > tolerance_cnt:
vy = ky * (cnt_x - img_cnt_x)
if abs(cnt_y - img_cnt_y) > tolerance_cnt:
vz = kz * (cnt_y - img_cnt_y)
if abs(dist_y - init_dist_y) > tolerance_y:
vx = kx * (init_dist_y - dist_y)
mav.SendVelFRD(vx, vy, vz, 0)
time.sleep(0.001)
ctrl_drone_th = threading.Thread(target=CtrlDrone)
ctrl_drone_th.start()
keyboard.add_hotkey("ctrl+up", MoveCircle, args=("front",))
keyboard.add_hotkey("ctrl+down", MoveCircle, args=("back",))
keyboard.add_hotkey("up", MoveCircle, args=("up",))
keyboard.add_hotkey("down", MoveCircle, args=("down",))
keyboard.add_hotkey("right", MoveCircle, args=("right",))
keyboard.add_hotkey("left", MoveCircle, args=("left",))
keyboard.wait()

0
基础智能/e11.无人机跟随圆形案板移动实验/名称-无人机跟随圆形案板移动实验.txt Normal file
View File

5
基础智能/e12.撞击小球实验/Python38Run.bat Normal file
View 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
基础智能/e12.撞击小球实验/Readme.pdf Normal file
View File

Binary file not shown.

197
基础智能/e12.撞击小球实验/ShootBall3.py Normal file
View File

@@ -0,0 +1,197 @@
# import required libraries
import time
import numpy as np
import cv2
# import RflySim APIs
import PX4MavCtrlV4 as PX4MavCtrl
import ScreenCapApiV4 as sca
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()
# Function to calculate the location and radius of red ball
def calc_centroid(img):
"""Get the centroid and area of Red in the image"""
low_range = np.array([0,0,80])
high_range = np.array([100,100,255])
th = cv2.inRange(img, low_range, high_range)
dilated = cv2.dilate(th, cv2.getStructuringElement(
cv2.MORPH_ELLIPSE, (3, 3)), iterations=2)
cv2.imshow("dilated", dilated)
cv2.waitKey(1)
M = cv2.moments(dilated, binaryImage=True)
if M["m00"] >= min_prop*width*height:
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
return [cx, cy, M["m00"]]
else:
return [-1, -1, -1]
# Function to obtain velocity commands for Pixhawk
# according to the image processing results
def controller(p_i):
# if the object is not in the image, search in clockwise
if p_i[0] < 0 or p_i[1] < 0:
return [0, 0, 0, 1]
# found
ex = p_i[0] - width / 2
ey = p_i[1] - height / 2
vx = 2 if p_i[2] < max_prop*width*height else 0
vy = 0
vz = K_z * ey
yawrate = K_yawrate * ex
# return forward, rightward, downward, and rightward-yaw
# velocity control sigals
return [vx, vy, vz, yawrate]
# Process image to obtain vehicle velocity control command
def procssImage():
img_bgr=sca.getCVImg(ImgInfo1)
p_i = calc_centroid(img_bgr)
ctrl = controller(p_i)
return ctrl
# saturation function to limit the maximum velocity
def sat(inPwm,thres=1):
outPwm= inPwm
for i in range(len(inPwm)):
if inPwm[i]>thres:
outPwm[i] = thres
elif inPwm[i]<-thres:
outPwm[i] = -thres
return outPwm
# Create MAVLink control API instance
mav = PX4MavCtrl.PX4MavCtrler(1)
# Init MAVLink data receiving loop
mav.InitMavLoop()
isEmptyData = False
lastTime = time.time()
startTime = time.time()
# time interval of the timer
timeInterval = 1/30.0 #here is 0.0333s (30Hz)
flag = 0
# parameters
width = 720
height = 405
channel = 4
min_prop = 0.000001
max_prop = 0.3
K_z = 0.003 * 640 / height
K_yawrate = 0.005 * 480 / width
# send command to all RflySim3D windows to switch to the blank grass scene
# by default, there are two windows, the first is front camera
# for vison control, and the second window for observation
ue.sendUE4Cmd('RflyChangeMapbyName VisionRingBlank')
time.sleep(1)
# create a ball, set its position and altitude, use the default red color
ue.sendUE4Pos(100,152,0,[3,0,-2],[0,0,0])
time.sleep(0.5)
# Change the target vehicle to copterID=1's vehicle
ue.sendUE4Cmd('RflyChangeViewKeyCmd B 1',0)
time.sleep(0.5)
# Switch its viewpoint to oboard #1 (front camera view)
ue.sendUE4Cmd('RflyChangeViewKeyCmd V 1',0)
time.sleep(0.5)
# move the camera to the position [0.3,0,0.05] related to the body
ue.sendUE4Cmd('RflyCameraPosAng 0.3 0 0.05 0 0 0',0)
time.sleep(0.5)
# set the RflySim3D window size to 720x405
# Send command to UE4 Window 1 to change resolution
ue.sendUE4Cmd('r.setres 720x405w',1)
time.sleep(2)
# Send command to UE4 Window 1 to change resolution
ue.sendUE4Cmd('r.setres 720x405w',0)
time.sleep(2)
# Get handles of all UE4/RflySim3D windows
window_hwnds = sca.getWndHandls()
try:
# Move UE4 windows 0 to desired position and keep top
xx,yy=sca.moveWd(window_hwnds[0],0,0,True)
time.sleep(0.5)
# Move UE4 windows 0 to desired position (next to windows 0) and keep top
xx,yy=sca.moveWd(window_hwnds[1],xx,0,False)
except Exception as e :
print('Please move the window manually')
xx=720
yy=405
# Get the info of the first window (index=0)
ImgInfo1 = sca.getHwndInfo(window_hwnds[0])
num=0
lastClock=time.time()
# Start a endless loop with 30Hz, timeInterval=1/30.0
ctrlLast = [0,0,0,0]
while True:
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)
num=num+1
if num%100==0:
tiem=time.time()
print('MainThreadFPS: '+str(100/(tiem-lastClock)))
lastClock=tiem
if time.time() - startTime > 5 and flag == 0:
# The following code will be executed at 5s
print("5s, Arm the drone")
mav.initOffboard()
flag = 1
mav.SendMavArm(True) # Arm the drone
print("Arm the drone!, and fly to NED 0,0,-5")
mav.SendPosNED(0, 0, -5, 0) # Fly to target position [0, 0, -5], i.e., take off to 5m
if time.time() - startTime > 15 and flag == 1:
flag = 2
# The following code will be executed at 15s
mav.SendPosNED(-30,-5, -5, 0) # Fly to target position [-30,-5, -5]
print("15s, fly to pos: -30,-5, -5")
if time.time() - startTime > 25 and flag == 2:
flag = 3
# Show CV image and set the position
img_bgr=sca.getCVImg(ImgInfo1)
cv2.imshow("dilated", img_bgr)
cv2.waitKey(1)
time.sleep(0.5)
cv2.moveWindow("dilated",0,yy)
print("25s, start to shoot the ball.")
if time.time() - startTime > 25 and flag == 3:
ctrlNow = procssImage()
ctrl = sat(ctrlNow,5)
# add a inertial component here to restrain the speed variation rate
if ctrl[0]-ctrlLast[0] > 0.5:
ctrl[0]=ctrlLast[0]+0.05
elif ctrl[0]-ctrlLast[0] < -0.5:
ctrl[0]=ctrlLast[0]-0.05
if ctrl[1]-ctrlLast[1] > 0.5:
ctrl[1]=ctrlLast[1]+0.05
elif ctrl[1]-ctrlLast[1] < -0.5:
ctrl[1]=ctrlLast[1]-0.05
ctrlLast = ctrl
# if control signals is obtained, send to Pixhawk
if not isEmptyData:
mav.SendVelFRD(ctrl[0], ctrl[1], ctrl[2], ctrl[3])

178
基础智能/e12.撞击小球实验/ShootBall3HITL.bat Normal file
View File

@@ -0,0 +1,178 @@
@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=VisionRingBlank
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
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.

163
基础智能/e12.撞击小球实验/ShootBall3SITL.bat Normal file
View File

@@ -0,0 +1,163 @@
@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=VisionRingBlank
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
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.

12
基础智能/e12.撞击小球实验/WinWSL.bat Normal file
View 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

20
基础智能/e12.撞击小球实验/WslGUI.bat Normal file
View File

@@ -0,0 +1,20 @@
@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
)
tasklist|find /i "vcxsrv.exe" >nul && taskkill /f /IM "vcxsrv.exe" >nul
tasklist|find /i "vcxsrv.exe" >nul && taskkill /f /IM "vcxsrv.exe" >nul
cd /d %PSP_PATH%\VcXsrv
Xlaunch.exe -run config.xlaunch
choice /t 1 /d y /n >nul
cd /d %~dp0
wsl -d RflySim-20.04 ~/StartUI.sh
choice /t 1 /d y /n >nul
tasklist|find /i "vcxsrv.exe" && taskkill /f /IM "vcxsrv.exe"

0
基础智能/e12.撞击小球实验/名称-屏幕截图接口、撞击小球实验.txt Normal file
View File

20
基础智能/e13.四旋翼无人机穿环实验/Config1.json Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View File

Binary file not shown.

12
基础智能/e13.四旋翼无人机穿环实验/readme.txt Normal file
View 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
View File

21
基础智能/e14.四旋翼无人机跟踪小球实验/Config.json Normal file
View File

@@ -0,0 +1,21 @@
{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0，则自动递增排序。
"TypeID":1, //视觉传感器类型，1:RGB，2:深度，3:灰度，4:分割，5:测距，7:深度转点云，20-23:激光雷达，40-41:热力红外（收费版）
"TargetCopter":1, //传感器绑定的CopterID号，注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式，0:固定飞机几何中心， 1:固定飞机底部中心，2:固定地面上，3:弱固定飞机上姿态不随动，4:绑定其他视觉传感器上
"DataWidth":640, //图像像素长度
"DataHeight":480,//图像像素宽度
"DataCheckFreq":30,//图像检查更新频率，如果发现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.3,0,0], //视觉传感器的安装位置，和TargetMountType对应偏移中心，单位米
"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] //其他参数
}
]
}

5
基础智能/e14.四旋翼无人机跟踪小球实验/Python38Run.bat Normal file
View 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
基础智能/e14.四旋翼无人机跟踪小球实验/Readme.pdf Normal file
View File

Binary file not shown.

173
基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3.py Normal file
View File

@@ -0,0 +1,173 @@
# 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() # 加载Config.json中的传感器配置文件
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
if not isSuss: # 如果请求取图失败，则退出
sys.exit(0)
vis.startImgCap() # 开启共享内存取图
# 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)
# Create MAVLink control API instance
mav = PX4MavCtrl.PX4MavCtrler(1)
# Init MAVLink data receiving loop
mav.InitMavLoop()
# create a ball, set its position and altitude, use the default red color
ue.sendUE4Pos(100,152,0,[3,0,-2],[0,0,0])
time.sleep(0.5)
# Function to calculate the location and radius of red ball
def calc_centroid(img):
"""Get the centroid and area of Red in the image"""
low_range = np.array([0,0,80])
high_range = np.array([100,100,255])
th = cv2.inRange(img, low_range, high_range)
dilated = cv2.dilate(th, cv2.getStructuringElement(
cv2.MORPH_ELLIPSE, (3, 3)), iterations=2)
cv2.imshow("dilated", dilated)
cv2.waitKey(1)
M = cv2.moments(dilated, binaryImage=True)
if M["m00"] >= min_prop*width*height:
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
return [cx, cy, M["m00"]]
else:
return [-1, -1, -1]
# Function to obtain velocity commands for Pixhawk
# according to the image processing results
def controller(p_i):
# if the object is not in the image, search in clockwise
if p_i[0] < 0 or p_i[1] < 0:
return [0, 0, 0, 1]
# found
ex = p_i[0] - width / 2
ey = p_i[1] - height / 2
vx = 2 if p_i[2] < max_prop*width*height else 0
vy = 0
vz = K_z * ey
yawrate = K_yawrate * ex
# return forward, rightward, downward, and rightward-yaw
# velocity control sigals
return [vx, vy, vz, yawrate]
# Process image to obtain vehicle velocity control command
def procssImage():
global ctrl_last
if vis.hasData[0]:
img_bgr=vis.Img[0]
p_i = calc_centroid(img_bgr)
ctrl = controller(p_i)
else:
ctrl=[0,0,0,0]
return ctrl
# saturation function to limit the maximum velocity
def sat(inPwm,thres=1):
outPwm= inPwm
for i in range(len(inPwm)):
if inPwm[i]>thres:
outPwm[i] = thres
elif inPwm[i]<-thres:
outPwm[i] = -thres
return outPwm
lastTime = time.time()
startTime = time.time()
# time interval of the timer
timeInterval = 1/30.0 #here is 0.0333s (30Hz)
flag = 0
# parameters
width = 640
height = 480
channel = 4
min_prop = 0.000001
max_prop = 0.3
K_z = 0.003 * 640 / height
K_yawrate = 0.005 * 480 / width
num=0
lastClock=time.time()
# Start a endless loop with 30Hz, timeInterval=1/30.0
ctrlLast = [0,0,0,0]
while True:
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)
num=num+1
if num%100==0:
tiem=time.time()
print('MainThreadFPS: '+str(100/(tiem-lastClock)))
lastClock=tiem
if time.time() - startTime > 5 and flag == 0:
# The following code will be executed at 5s
print("5s, Arm the drone")
mav.initOffboard()
flag = 1
mav.SendMavArm(True) # Arm the drone
print("Arm the drone!, and fly to NED 0,0,-5")
mav.SendPosNED(0, 0, -5, 0) # Fly to target position [0, 0, -5], i.e., take off to 5m
if time.time() - startTime > 15 and flag == 1:
flag = 2
# The following code will be executed at 15s
mav.SendPosNED(-30,-5, -5, 0) # Fly to target position [-30,-5, -5]
print("15s, fly to pos: -30,-5, -5")
if time.time() - startTime > 25 and flag == 2:
flag = 3
# Show CV image and set the position
if vis.hasData[0]:
img_bgr=vis.Img[0]
cv2.imshow("dilated", img_bgr)
cv2.waitKey(1)
#time.sleep(0.5)
print("25s, start to shoot the ball.")
if time.time() - startTime > 25 and flag == 3:
ctrlNow = procssImage()
ctrl = sat(ctrlNow,5)
# add a inertial component here to restrain the speed variation rate
if ctrl[0]-ctrlLast[0] > 0.5:
ctrl[0]=ctrlLast[0]+0.05
elif ctrl[0]-ctrlLast[0] < -0.5:
ctrl[0]=ctrlLast[0]-0.05
if ctrl[1]-ctrlLast[1] > 0.5:
ctrl[1]=ctrlLast[1]+0.05
elif ctrl[1]-ctrlLast[1] < -0.5:
ctrl[1]=ctrlLast[1]-0.05
ctrlLast = ctrl
# if control signals is obtained, send to Pixhawk
mav.SendVelFRD(ctrl[0], ctrl[1], ctrl[2], ctrl[3])

177
基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3HITL.bat Normal file
View 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=VisionRingBlank
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.

162
基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3SITL.bat Normal file
View 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=VisionRingBlank
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.

12
基础智能/e14.四旋翼无人机跟踪小球实验/WinWSL.bat Normal file
View 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

20
基础智能/e14.四旋翼无人机跟踪小球实验/WslGUI.bat Normal file
View File

@@ -0,0 +1,20 @@
@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
)
tasklist|find /i "vcxsrv.exe" >nul && taskkill /f /IM "vcxsrv.exe" >nul
tasklist|find /i "vcxsrv.exe" >nul && taskkill /f /IM "vcxsrv.exe" >nul
cd /d %PSP_PATH%\VcXsrv
Xlaunch.exe -run config.xlaunch
choice /t 1 /d y /n >nul
cd /d %~dp0
wsl -d RflySim-20.04 ~/StartUI.sh
choice /t 1 /d y /n >nul
tasklist|find /i "vcxsrv.exe" && taskkill /f /IM "vcxsrv.exe"

0
基础智能/e14.四旋翼无人机跟踪小球实验/名称-无人机跟踪小球实验.txt Normal file
View File

16
基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.bat Normal file
View File

@@ -0,0 +1,16 @@
@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
)
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"
REM UE4Path
cd /d %PSP_PATH%\RflySim3D
start %PSP_PATH%\RflySim3D\RflySim3D.exe

86
基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.py Normal file
View File

@@ -0,0 +1,86 @@
import time
import math
import sys
import PX4MavCtrlV4 as PX4MavCtrl
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()
# Create four MAVLink API instances, the CopterSim UDP ports are 20100+2*i, where i=1,2,3,4
# Although no CopterSim is required in NoPX4Mode, the port 20100+2*i is required to identify the CopterID
mav = PX4MavCtrl.PX4MavCtrler(1)
mav1 = PX4MavCtrl.PX4MavCtrler(2)
mav2 = PX4MavCtrl.PX4MavCtrler(3)
mav3 = PX4MavCtrl.PX4MavCtrler(4)
# 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 Grasslands')
time.sleep(2)
# Start MAVLink listening loop from CopterSim/Pixhawk
# initPointMassModel(intAlt=0,intState=[0,0,0]):
# intAlt is the ground high which should be acquired from the UE4 map
# intState=PosX, PosY, PosZ, which can obtain intAlt at CopterSim UI
mav.initPointMassModel(-8.086,[0,0,0])
mav1.initPointMassModel(-8.29,[2,0,0])
mav2.initPointMassModel(-7.835,[0,2,0])
mav3.initPointMassModel(-7.788,[2,2,0])
time.sleep(2)
# Display the position, velocity, angle, angular rate from Pixhawk/CopterSIm
print((mav.uavPosNED,mav.truePosNED,mav.uavVelNED,mav.uavAngEular,mav.uavAngRate,mav.uavGlobalPos))
print((mav1.uavPosNED,mav1.truePosNED,mav1.uavVelNED,mav1.uavAngEular,mav1.uavAngRate,mav1.uavGlobalPos))
print((mav2.uavPosNED,mav2.truePosNED,mav2.uavVelNED,mav2.uavAngEular,mav2.uavAngRate,mav2.uavPosGPS,mav2.uavGlobalPos))
print((mav3.uavPosNED,mav3.truePosNED,mav3.uavVelNED,mav3.uavAngEular,mav3.uavAngRate,mav3.uavPosGPS,mav3.uavGlobalPos))
# 设定最大速度，这个比较重要！，根据任务来匹配机型速度
mav.SendCopterSpeed(10)
mav1.SendCopterSpeed(10)
mav2.SendCopterSpeed(10)
mav3.SendCopterSpeed(10)
time.sleep(2)
# send desired poisition control command, fly to target postion 0,0,-1.7m with yaw 0 degree
mav.SendPosNED(0, 0, -1.7, 0)
mav1.SendPosNED(2, 2, -1.7, 0)
mav2.SendPosNED(-2, -2, -1.7, 0)
mav3.SendPosNED(0, 0, -3, 0)
print("Send target Pos")
time.sleep(0.5)
# Send arm command
mav.SendMavArm(True)
time.sleep(2)
mav1.SendMavArm(True)
time.sleep(2)
mav2.SendMavArm(True)
time.sleep(2)
mav3.SendMavArm(True)
print("Send Arm Command")
time.sleep(5)
# send desired NED velocity signals, 0.2m/s download
mav.SendVelNED(0, 0, 1, 0)
mav1.SendVelNED(0, 0, 1, 0)
mav2.SendVelNED(0, 0, 1, 0)
mav3.SendVelNED(0, 0, 1, 0)
print("Send Velocity Speed")
time.sleep(5)
# exit from MAVLink listening loop
print("End pointMass model")
mav.EndPointMassModel()
mav1.EndPointMassModel()
mav2.EndPointMassModel()
mav3.EndPointMassModel()
time.sleep(1)
# while True:
# print(mav.uavPosNED)
# time.sleep(2)

5
基础智能/e2.4 机质点模型集群实验/Python38Run.bat Normal file
View 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
基础智能/e2.4 机质点模型集群实验/Readme.pdf Normal file
View File

Binary file not shown.

0
基础智能/e2.4 机质点模型集群实验/名称-4机质点模型集群实验.txt Normal file
View File

16
基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.bat Normal file
View File

@@ -0,0 +1,16 @@
@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
)
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"
REM UE4Path
cd /d %PSP_PATH%\RflySim3D
start %PSP_PATH%\RflySim3D\RflySim3D.exe

51
基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.py Normal file
View File

@@ -0,0 +1,51 @@
import time
import os
import math
import sys
import PX4MavCtrlV4 as PX4MavCtrl
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()
# Create a MAVLink communication instance with UDP
# port 20100 (coresponding to the port on CopterSim)
mav = PX4MavCtrl.PX4MavCtrler(1)
time.sleep(1)
# Switch to a night scene
#ue.sendUE4Cmd('RflyChangeMapbyName ModernCityNight')
ue.sendUE4Cmd('RflyChangeMapbyName LightShow')
time.sleep(4)
# send 100 vehicles to RflySim3D to form a helix
z0 = -50
x0 = 50
y0 = 40
for i in range(100):
s = 1 + i/5.0
th = math.pi/3 + math.pi/10*i
y = s*math.cos(th)
z = -s*math.sin(th)
ue.sendUE4Pos(i+1,3,0,[x0,y+y0,z0+z],[0,0,0])
# Switch viewpoint target vehicle to vehicle 1
ue.sendUE4Cmd('RflyChangeViewKeyCmd B 1')
time.sleep(0.5)
# Switch to the N2 ground observation viewpoint for target vehicle
ue.sendUE4Cmd('RflyChangeViewKeyCmd N 2')
time.sleep(0.5)
# Set the observation position and angle, 1.7m above the ground, with 30 degree pitch
ue.sendUE4Cmd('RflyCameraPosAng -10 40 -1.7 0 30 0')
time.sleep(2)
idx=1
while True:
for i in range(100):
str0 = "RflyChange3DModel "+str(i+1)+" -1"
#print(str0)
ue.sendUE4Cmd(str0.encode())
time.sleep(0.05)
sys.exit()

5
基础智能/e3.集群轨迹灯光展示实验/Python38Run.bat Normal file
View 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
基础智能/e3.集群轨迹灯光展示实验/Readme.pdf Normal file
View File

Binary file not shown.

0
基础智能/e3.集群轨迹灯光展示实验/名称-集群轨迹灯光展示实验.txt Normal file
View File

BIN
基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrl.dll Normal file
View File

Binary file not shown.

168
基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrlSITLRun.bat Normal file
View File

@@ -0,0 +1,168 @@
@ECHO OFF
REM Run script as administrator
%1 mshta vbscript:CreateObject("Shell.Application").ShellExecute("cmd.exe","/c ""%~s0"" ::","","runas",1)(window.close)&&exit
REM Set the path of the RflySim tools
if not defined PSP_PATH (
SET PSP_PATH=E:\PX4PSP
SET PSP_PATH_LINUX=/mnt/e/PX4PSP
)
REM Start index of vehicle number (should larger than 0)
REM This option is useful for simulation with multi-computers
SET /a START_INDEX=4
REM Set the start UDP port for SIMULINK/OFFBOARD API
REM This option should not be modified for swarm simulation
SET /a UDP_START_PORT=20100
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=CarNoCtrl
REM Check if DLLModel is a name string, if yes, copy the DLL file to CopterSim folder
SET /A DLLModelVal=DLLModel
if %DLLModelVal% NEQ %DLLModel% (
REM Copy the latest dll file to CopterSim folder
copy /Y "%~dp0"\%DLLModel%.dll %PSP_PATH%\CopterSim\external\model\%DLLModel%.dll
)
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=r1_rover
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=OldFactory
REM Set the origin x,y position (m) and yaw angle (degree) at the map
SET /a ORIGIN_POS_X=-250
SET /a ORIGIN_POS_Y=-119
SET /a ORIGIN_YAW=0
REM Set the interval between two vehicle, unit:m
SET /a VEHICLE_INTERVAL=3
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 e.g., UDPSIMMODE=1 equals to UDPSIMMODE=UDP_Simple
SET UDPSIMMODE=1
: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=3
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
REM tasklist|find /i "QGroundControl.exe" || start %PSP_PATH%\QGroundControl\QGroundControl.exe
ECHO Start QGroundControl
REM UE4Path
cd %PSP_PATH%\RflySim3D
REM tasklist|find /i "RflySim3D.exe" || start %PSP_PATH%\RflySim3D\RflySim3D.exe
REM choice /t 5 /d y /n >nul
REM 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 /a portNum=%UDP_START_PORT%+((%START_INDEX%-1)*2)
: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% %portNum% %DLLModel% %SimMode% %UE4_MAP% %IS_BROADCAST% %PosXX% %PosYY% %ORIGIN_YAW% 1 %UDPSIMMODE%
choice /t 1 /d y /n >nul
set /a cntr=%cntr%+1
set /a portNum=%portNum%+2
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 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 %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"
ECHO Start End.

13
基础智能/e4.四旋翼与无人车异构集群协同控制实验/MultiVehicleRunALL.bat Normal file
View File

@@ -0,0 +1,13 @@
if not defined PSP_PATH (
SET PSP_PATH=C:\PX4PSP
SET PSP_PATH_LINUX=/mnt/c/PX4PSP
)
@echo off
rem <20><><EFBFBD><EFBFBD> MulticopterNoCtrlSITLRun.bat
start "" MulticopterNoCtrlSITLRun.bat
rem <20><>ʱ 5 <20><>
timeout /t 25 /nobreak >nul
rem <20><><EFBFBD><EFBFBD> CarNoCtrlSITLRun.bat
start "" CarNoCtrlSITLRun.bat

BIN
基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrl.dll Normal file
View File

Binary file not shown.

164
基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrlSITLRun.bat Normal file
View File

@@ -0,0 +1,164 @@
@ECHO OFF
REM Run script as administrator
%1 mshta vbscript:CreateObject("Shell.Application").ShellExecute("cmd.exe","/c ""%~s0"" ::","","runas",1)(window.close)&&exit
REM Set the path of the RflySim tools
if not defined PSP_PATH (
SET PSP_PATH=E:\PX4PSP
SET PSP_PATH_LINUX=/mnt/e/PX4PSP
)
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
SET /a TOTOAL_COPTER=6
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=OldFactory
REM Set the origin x,y position (m) and yaw angle (degree) at the map
SET /a ORIGIN_POS_X=-250
SET /a ORIGIN_POS_Y=-119
SET /a ORIGIN_YAW=0
REM Set the interval between two vehicle, unit:m
SET /a VEHICLE_INTERVAL=3
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=1
:Top
ECHO.
ECHO ---------------------------------------
REM Max vehicle number 50
SET /a MAX_VEHICLE=50
SET /A VehicleNum=3
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 -key=I1
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"
SET BatName=%~n0
tasklist|find /i "%BatName%.exe" && taskkill /f /im "%BatName%.exe"
taskkill /F /IM cmd.exe /T
ECHO Start End.

5
基础智能/e4.四旋翼与无人车异构集群协同控制实验/Python38Run.bat Normal file
View 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
基础智能/e4.四旋翼与无人车异构集群协同控制实验/Readme.pdf Normal file
View File

Binary file not shown.

83
基础智能/e4.四旋翼与无人车异构集群协同控制实验/UDPSimpleMultiVehicleSwarm.py Normal file
View File

@@ -0,0 +1,83 @@
import time
import math
import sys
import numpy as np
import PX4MavCtrlV4 as PX4MavCtrl
VehilceNum = 3
CarNum = 3
MavList=[]
flagI = np.zeros(VehilceNum, dtype=bool)
# Create MAV instance
for i in range(VehilceNum + CarNum):
MavList=MavList+[PX4MavCtrl.PX4MavCtrler(1+i)]
time.sleep(2)
# Start MAV loop with UDP mode: Udp_Simple
for i in range(VehilceNum+CarNum):
MavList[i].InitMavLoop(1)
# Enter Offboard mode to start vehicle control
time.sleep(2)
for i in range(VehilceNum+CarNum):
MavList[i].initOffboard()
time.sleep(0.5)
time.sleep(2)
for i in range(VehilceNum+CarNum):
MavList[i].SendMavArm(True)
# Get the takeoff position of each vehicle to the UE4 Map
# this can be adopted to obtain the global position of a vehicle in swarm simulation
time.sleep(5)
Error2UE4Map=[]
for i in range(VehilceNum):
mav=MavList[i]
Error2UE4Map = Error2UE4Map+[-np.array([mav.uavGlobalPos[0]-mav.uavPosNED[0],mav.uavGlobalPos[1]-mav.uavPosNED[1],mav.uavGlobalPos[2]-mav.uavPosNED[2]])]
# fly to 10m high above its takeoff position
for i in range(VehilceNum):
MavList[i].SendPosNED(0, 0, -10, 0)
time.sleep(10)
lastTime = time.time()
startTime = time.time()
timeInterval = 1/30.0 #here is 0.0333s (30Hz)
while True:
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)
t=time.time()-startTime
for j in range(VehilceNum):
mav=MavList[j]
if abs(mav.uavPosNED[2] - (-10)) <2 and (flagI[j] == False):
flagI[j] = True
print(str(j)+"号飞机已到达指定高度")
if flagI[j] == True:
mav.SendPosNED(MavList[j+3].uavPosNED[0],MavList[j+3].uavPosNED[1],mav.uavPosNED[2],0)
for i in range(VehilceNum,VehilceNum+CarNum):
if flagI[i-3] == True:
# print(f"编号 {i-3}：北 {MavList[i-3].uavPosNED[0]:.2f} m，东 {MavList[i-3].uavPosNED[1]:.2f} m，地 {MavList[i-3].uavPosNED[2]:.2f} m")
MavList[i].SendPosNEDNoYaw(MavList[i].uavPosNED[0]+2.5,MavList[i].uavPosNED[1],0)
if t>200: # end simulation when 200s
break
for i in range(VehilceNum):
MavList[i].endOffboard()
time.sleep(1)
for i in range(VehilceNum):
MavList[i].stopRun()

162
基础智能/e5.2机碰撞实验/CollisionDemo.bat Normal file
View 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=VisionRingBlank
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 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"
ECHO Start End.

95
基础智能/e5.2机碰撞实验/CollisionDemo.py Normal file
View File

@@ -0,0 +1,95 @@
import time
import math
import sys
import PX4MavCtrlV4 as PX4MavCtrl
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()
#Create a new MAVLink communication instance, UDP sending port (CopterSim’s receving port) is 20100
mav = PX4MavCtrl.PX4MavCtrler(1)
mav2 = PX4MavCtrl.PX4MavCtrler(2)
ue.sendUE4Cmd('RflyChangeViewKeyCmd S',0) #开启标号显示
time.sleep(0.5)
ue.sendUE4Cmd('RflyChangeViewKeyCmd T',0) #开启标号显示
time.sleep(0.5)
ue.sendUE4Cmd('RflyChangeViewKeyCmd P',0) #开启碰撞引擎
time.sleep(5)
#Turn on MAVLink to monitor CopterSim data and update it in real time.
mav.InitMavLoop()
mav2.InitMavLoop()
time.sleep(0.5)
#Display Position information received from CopterSim
print(mav.uavPosNED)
#Turn on Offboard mode
mav.initOffboard()
mav2.initOffboard()
# Send the desired position signal, fly to the target point 0,0, -1.7 position, the yaw angle is 0
mav.SendPosNED(0, 0, -1.7, 0)
mav2.SendPosNED(2, -2, -1.7, 0)
print("Send target Pos")
#Send arm command to arm the drone
mav.SendMavArm(True)
mav2.SendMavArm(True)
print("Send Arm Command")
time.sleep(15)
# Send the desired speed signal, 0.2m/s downwards, the z-axis downward is positive
mav.SendVelNED(1, 0, 0, 0)
mav2.SendVelNED(-1, 0, 0, 0)
print("Send Velocity Speed")
hasV1Crashed=False
hasV2Crashed=False
startTime = time.time()
lastTime = time.time()
timeInterval = 1/30.0 # time interval of the timer
while True:
# The above code will be executed 30Hz (0.033333s)
lastTime = lastTime + timeInterval
sleepTime = lastTime - time.time()
if sleepTime > 0:
time.sleep(sleepTime)
else:
lastTime = time.time()
#使用mav.isVehicleCrash来检测飞机是否碰撞
if (not hasV1Crashed) and mav.isVehicleCrash:
print('Vehicle #1 Crashed with vehicle #',mav.isVehicleCrashID)
hasV1Crashed=True
#使用mav2.isVehicleCrash来检测飞机是否碰撞
if (not hasV2Crashed) and mav2.isVehicleCrash:
print('Vehicle #2 Crashed with vehicle #',mav2.isVehicleCrashID)
hasV2Crashed=True
if hasV2Crashed and hasV1Crashed:
time.sleep(4)
break
#Exit Offboard control mode
print("Send offboard stop")
mav.endOffboard()
mav2.endOffboard()
time.sleep(1)
#Exit MAVLink data receiving mode
print("Send Mavlink stop")
mav.stopRun()
mav2.stopRun()
time.sleep(1)
#while True:
# print(mav.uavPosNED)
# time.sleep(2)

5
基础智能/e5.2机碰撞实验/Python38Run.bat Normal file
View 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
基础智能/e5.2机碰撞实验/Readme.pdf Normal file
View File

Binary file not shown.

0
基础智能/e5.2机碰撞实验/名称-MAVLink模式2机碰撞实验。.txt Normal file
View File

21
基础智能/e6.无人机视觉传感器介绍实验/Config (2).json Normal file
View File

@@ -0,0 +1,21 @@
{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0，则自动递增排序。
"TypeID":1, //视觉传感器类型，1:RGB，2:深度，3:灰度，4:分割，5:测距，7:深度转点云，20-23:激光雷达，40-41:热力红外（收费版）
"TargetCopter":1, //传感器绑定的CopterID号，注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式，0:固定飞机几何中心， 1:固定飞机底部中心，2:固定地面上，3:弱固定飞机上姿态不随动，4:绑定其他视觉传感器上
"DataWidth":640, //图像像素长度
"DataHeight":480,//图像像素宽度
"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.3,0,0], //视觉传感器的安装位置，和TargetMountType对应偏移中心，单位米
"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] //其他参数
}
]
}

21
基础智能/e6.无人机视觉传感器介绍实验/Config.json Normal file
View File

@@ -0,0 +1,21 @@
{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0，则自动递增排序。
"TypeID":1, //视觉传感器类型，1:RGB，2:深度，3:灰度，4:分割，5:测距，7:深度转点云，20-23:激光雷达，40-41:热力红外（收费版）
"TargetCopter":1, //传感器绑定的CopterID号，注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式，0:固定飞机几何中心， 1:固定飞机底部中心，2:固定地面上，3:弱固定飞机上姿态不随动，4:绑定其他视觉传感器上
"DataWidth":640, //图像像素长度
"DataHeight":480,//图像像素宽度
"DataCheckFreq":30,//图像检查更新频率，如果发现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.3,0,0], //视觉传感器的安装位置，和TargetMountType对应偏移中心，单位米
"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] //其他参数
}
]
}

BIN
基础智能/e6.无人机视觉传感器介绍实验/Intro.pdf Normal file
View File

Binary file not shown.

5
基础智能/e6.无人机视觉传感器介绍实验/Python38Run (2).bat Normal file
View 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%"

5
基础智能/e6.无人机视觉传感器介绍实验/Python38Run.bat Normal file
View 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
基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.bat Normal file
View 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=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=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.

62
基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.py Normal file
View File

@@ -0,0 +1,62 @@
# -*- coding: utf-8 -*-
import cv2
import sys
import time
import VisionCaptureApi
import os
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()
# 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 720x405w',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
ue.sendUE4Cmd('t.MaxFPS 200',0) # 设置UE4最大刷新频率，同时也是取图频率
ue.sendUE4Cmd('stat fps',0) #在UE4窗口显示当前刷新频率
time.sleep(2)
# VisionCaptureApi 中的配置函数
vis.jsonLoad() # 加载Config.json中的传感器配置文件
# vis.RemotSendIP = '192.168.3.80'
# 注意，手动修改RemotSendIP的值，可以将图片发送到远端Linux电脑的IP地址
# 如果不修改这个值，那么发送的IP地址为json文件中SendProtocol[1:4]定义的IP
# 图片的发送端口，为json中SendProtocol[5]定义好的。
#vis.isUE4DirectUDP=True
# 注意，手动修改本命令能强制将图片发送机制为UE4直接发出UDP图片到指定IP地址
# 如果不修改这个值，那么发送机制由json文件中SendProtocol[0]中定义
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
if not isSuss: # 如果请求取图失败，则退出
sys.exit(0)
vis.sendImuReqCopterSim() # 发送请求，从目标飞机CopterSim读取IMU数据,回传地址为127.0.0.1，默认频率为200Hz
# 执行本语句之后，会自动开启数据监听，已经可以通过vis.imu读取到IMU数据了。
vis.startImgCap() # 开启取图循环，执行本语句之后，已经可以通过vis.Img[i]读取到图片了
# 启用时间戳打印功能
#vis.isPrintTime=True
print('Start Image Reciver')
time.sleep(5)
for i in range(len(vis.Img)):
print('Data for #',vis.VisSensor[i].SeqID)
print('rflyStartStmp',vis.rflyStartStmp[i]) # rflyStartStmp[i]是CopterSim启动
#（TargetCopter对应的飞机）时，py所在电脑的时间（系统时间或ROS时间）
# 注意：如果isEnableRosTrans被启用，rflyStartStmp[i]使用的是ROS时间，反之使用的是系统UNIX时间
print('timeStmp',vis.timeStmp[i]) # timeStmp[i]是从CopterSim启动到当前数据生成的时间
print('imgStmp',vis.imgStmp[i]) # imgStmp[i] = rflyStartStmp[i]+timeStmp[i]是图像真实时间戳
print('imu.rflyStartStmp',vis.imu.rflyStartStmp)# rflyStartStmp是CopterSim启动时，py所在电脑的系统或ROS时间戳
print('imu.timestmp',vis.imu.timestmp) # timeStmp是从CopterSim启动到当前数据生成的时间
print('imu.imuStmp',vis.imu.imuStmp) # imuStmp=rflyStartStmp+timeStmp是IMU真实时间戳
print('')

BIN
基础智能/e6.无人机视觉传感器介绍实验/Readme (2).pdf Normal file
View File

Binary file not shown.

BIN
基础智能/e6.无人机视觉传感器介绍实验/Readme.pdf Normal file
View File

Binary file not shown.

162
基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.bat Normal file
View 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=Grasslands
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.

100
基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.py Normal file
View File

@@ -0,0 +1,100 @@
import time
import math
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import cv2
import UE4CtrlAPI
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 90',0) # 设置UE4最大刷新频率90hz，注意，在json中图片帧率为90Hz，因此，UE每1个Tick取一次图，每次取图延迟1Tick=1/90.0s
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() # 开启取图，并启用共享内存图像转发，转发到填写的目录
time.sleep(1)
# 请求1号飞机的IMU数据，回传IP如果RemotSendIP赋值了就传给这个IP，否则传给127.0.0.1
vis.sendImuReqCopterSim(1)
#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, -10!')
mav.SendPosNED(0, 0, -10, 0)
# 下面的程序非必需，仅用于观察图像用，在电脑性能不足时，请删除
lastTime = time.time()
num=0
lastClock=time.time()
while True:
lastTime = lastTime + 1/30.0
sleepTime = lastTime - time.time()
if sleepTime > 0:
time.sleep(sleepTime)
else:
lastTime = time.time()
if vis.hasIMUData:
print('IMU: ',vis.imu.timestmp,vis.imu.acc,vis.imu.rate)
for i in range(len(vis.hasData)):
if vis.hasData[i]:
# Process your image here
cv2.imshow('Img'+str(i),vis.Img[i])
#print('Img'+str(i),': ',vis.timeStmp[i])
cv2.waitKey(1)
# if i==0: # 更新0号相机的参数
# # 以下代码用于实时更新相机参数（位置、焦距、角度、装载飞机和形式）
# vs = vis.VisSensor[0] #获取第0号相机基本参数
# # 修改其中的可变部分，只修改需要改变的部分即可
# vs.TargetCopter=1 #修改视角绑定的飞机ID
# vs.TargetMountType=0 # 修改视角绑定类型，固连飞机还是地面
# vs.CameraFOV=90 # 修改视角的视场角（焦距），可以模拟对焦相机
# vs.SensorPosXYZ=[0.3,-0.15,0] # 修改相机的位置，可以调整相机初始位置
# vs.SensorAngEular=[0,0,0] # 修改相机的姿态，可以模拟云台转动
# vis.sendUpdateUEImage(vs) # 发送更新数据
#mav.endOffboard()
#mav.stopRun()

0
基础智能/e6.无人机视觉传感器介绍实验/名称-Imu和相机数据获取实验.txt Normal file
View File

0
基础智能/e6.无人机视觉传感器介绍实验/名称-时间戳获取实验.txt Normal file
View File

46
基础智能/e7.相机取图实验/Config.json Normal file
View File

@@ -0,0 +1,46 @@
{
"VisionSensors":[
{
"SeqID":0,
"TypeID":1,
"TargetCopter":1,
"TargetMountType":0,
"DataWidth":640,
"DataHeight":480,
"DataCheckFreq":30,
"SendProtocol":[0,127,0,0,1,9999,0,0],
"CameraFOV":90,
"SensorPosXYZ":[0.3,-0.15,0],
"SensorAngEular":[0,0,0],
"otherParams":[0,0,0,0,0,0,0,0]
},
{
"SeqID":1,
"TypeID":3,
"TargetCopter":1,
"TargetMountType":0,
"DataWidth":640,
"DataHeight":480,
"DataCheckFreq":30,
"SendProtocol":[0,127,0,0,1,1000,0,0],
"CameraFOV":90,
"SensorPosXYZ":[0.3,0.15,0],
"SensorAngEular":[0,0,0],
"otherParams":[0,0,0,0,0,0,0,0]
},
{
"SeqID":3,
"TypeID":2,
"TargetCopter":1,
"TargetMountType":0,
"DataWidth":640,
"DataHeight":480,
"DataCheckFreq":30,
"SendProtocol":[0,127,0,0,1,1000,0,0],
"CameraFOV":90,
"SensorPosXYZ":[0.3,0,0],
"SensorAngEular":[0,0,0],
"otherParams":[0,0,0,0,0,0,0,0]
}
]
}

BIN
基础智能/e7.相机取图实验/Readme.pdf Normal file
View File

Binary file not shown.

BIN
基础智能/e7.相机取图实验/Readme_EN .pdf Normal file
View File

Binary file not shown.

BIN
基础智能/e7.相机取图实验/SmallFixedWingUAV.dll Normal file
View File

Binary file not shown.

173
基础智能/e7.相机取图实验/VisionCapAPIDemo.bat Normal file
View File

@@ -0,0 +1,173 @@
@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 Set the path of the RflySim tools
if not defined PSP_PATH (
SET PSP_PATH=C:\PX4PSP
SET PSP_PATH_LINUX=/mnt/c/PX4PSP
)
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 Total vehicle Number to auto arrange position
REM SET /a TOTOAL_COPTER=8
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=SmallFixedWingUAV
REM Check if DLLModel is a name string, if yes, copy the DLL file to CopterSim folder
SET /A DLLModelVal=DLLModel
if %DLLModelVal% NEQ %DLLModel% (
REM Copy the latest dll file to CopterSim folder
copy /Y "%~dp0"\%DLLModel%.dll %PSP_PATH%\CopterSim\external\model\%DLLModel%.dll
)
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=standard_plane
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=OldFactory
REM Set the origin x,y position (m) and yaw angle (degree) at the map
SET /a ORIGIN_POS_X=-250
SET /a ORIGIN_POS_Y=-119
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; IS_BROADCAST=0: only this computer, IS_BROADCAST=1: broadcast;
REM or use IP address to increase speed, e.g., IS_BROADCAST=192.168.3.1
REM Note: in IP mode, IS_BROADCAST=0 equals to IS_BROADCAST=127.0.0.1, IS_BROADCAST=1 equals to IS_BROADCAST=255.255.255.255
REM You can also use a IP list with seperator "," or ";" to specify IPs to send, e.g., 127.0.0.1,192.168.1.4,192.168.1.5
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
: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"
ECHO Start End.

91
基础智能/e7.相机取图实验/VisionCapAPIDemo.py Normal file
View File

@@ -0,0 +1,91 @@
import time
import math
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import cv2
import UE4CtrlAPI
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 1280x720',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() # 开启取图，并启用共享内存图像转发，转发到填写的目录
#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.initOffboard2()
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 10, 0,-100!')
time.sleep(1)
mav.sendMavTakeOff(10,0,-100)
# 下面的程序非必需，仅用于观察图像用，在电脑性能不足时，请删除
lastTime = time.time()
num=0
lastClock=time.time()
while True:
lastTime = lastTime + 1/30.0
sleepTime = lastTime - time.time()
if sleepTime > 0:
time.sleep(sleepTime)
else:
lastTime = time.time()
for i in range(len(vis.hasData)):
if vis.hasData[i]:
# Process your image here
cv2.imshow('Img'+str(i),vis.Img[i])
cv2.waitKey(1)
if i==0: # 更新0号相机的参数
# 以下代码用于实时更新相机参数（位置、焦距、角度、装载飞机和形式）
vs = vis.VisSensor[0] #获取第0号相机基本参数
# 修改其中的可变部分，只修改需要改变的部分即可
vs.TargetCopter=1 #修改视角绑定的飞机ID
vs.TargetMountType=0 # 修改视角绑定类型，固连飞机还是地面
vs.CameraFOV=90 # 修改视角的视场角（焦距），可以模拟对焦相机
vs.SensorPosXYZ=[0,0,0] # 修改相机的位置，可以调整相机初始位置
vs.SensorAngEular=[0,0,0] # 修改相机的姿态，可以模拟云台转动
vis.sendUpdateUEImage(vs) # 发送更新数据
#mav.endOffboard()
#mav.stopRun()

0
基础智能/e7.相机取图实验/名称-相机取图实验.txt Normal file
View File

53
基础智能/e8.多目相机实验取图实验/Config.json Normal file
View File

@@ -0,0 +1,53 @@
{
"VisionSensors":[
{
"SeqID":0, //视觉传感器序号0 1 2 3 ...排序。如果填0，则自动递增排序。
"TypeID":1, //视觉传感器类型，1:RGB，2:深度，3:灰度，4:分割，5:测距，7:深度转点云，20-23:激光雷达，40-41:热力红外（收费版）
"TargetCopter":1, //传感器绑定的CopterID号，注:免费版只支持绑定1号飞机
"TargetMountType":0, //绑定方式，0:固定飞机几何中心， 1:固定飞机底部中心，2:固定地面上，3:弱固定飞机上姿态不随动，4:绑定其他视觉传感器上
"DataWidth":640, //图像像素长度
"DataHeight":480,//图像像素宽度
"DataCheckFreq":30,//图像检查更新频率，如果发现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.3,-0.15,0], //视觉传感器的安装位置，和TargetMountType对应偏移中心，单位米
"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, //自动确认序号，这里会自动识别为1
"TypeID":2,
"TargetCopter":1,
"TargetMountType":0,
"DataWidth":640,
"DataHeight":480,
"DataCheckFreq":30,
"EularOrQuat":0,
"SendProtocol":[0,0,0,0,0,0,0,0], //使用共享内存机制，并自动确定IP和端口号
"CameraFOV":90,
"SensorPosXYZ":[0.3,0.15,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":3,
"TargetCopter":1,
"TargetMountType":0,
"DataWidth":640,
"DataHeight":480,
"DataCheckFreq":30,
"SendProtocol":[0,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]
}
]
}

5
基础智能/e8.多目相机实验取图实验/Python38Run.bat Normal file
View 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
基础智能/e8.多目相机实验取图实验/Readme.pdf Normal file
View File

Binary file not shown.

162
基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.bat Normal file
View 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=Grasslands
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.

90
基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.py Normal file
View File

@@ -0,0 +1,90 @@
import time
import math
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import cv2
import UE4CtrlAPI
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 1280x720',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() # 开启取图，并启用共享内存图像转发，转发到填写的目录
#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, 0)
# 下面的程序非必需，仅用于观察图像用，在电脑性能不足时，请删除
lastTime = time.time()
num=0
lastClock=time.time()
while True:
lastTime = lastTime + 1/30.0
sleepTime = lastTime - time.time()
if sleepTime > 0:
time.sleep(sleepTime)
else:
lastTime = time.time()
for i in range(len(vis.hasData)):
if vis.hasData[i]:
# Process your image here
cv2.imshow('Img'+str(i),vis.Img[i])
cv2.waitKey(1)
if i==0: # 更新0号相机的参数
# 以下代码用于实时更新相机参数（位置、焦距、角度、装载飞机和形式）
vs = vis.VisSensor[0] #获取第0号相机基本参数
# 修改其中的可变部分，只修改需要改变的部分即可
vs.TargetCopter=1 #修改视角绑定的飞机ID
vs.TargetMountType=0 # 修改视角绑定类型，固连飞机还是地面
vs.CameraFOV=90 # 修改视角的视场角（焦距），可以模拟对焦相机
vs.SensorPosXYZ=[0.3,-0.15,0] # 修改相机的位置，可以调整相机初始位置
vs.SensorAngEular=[0,0,0] # 修改相机的姿态，可以模拟云台转动
vis.sendUpdateUEImage(vs) # 发送更新数据
#mav.endOffboard()
#mav.stopRun()

0
基础智能/e8.多目相机实验取图实验/名称-多目相机实验取图实验.txt Normal file
View File

21
基础智能/e9.雷达传感器数据获取实验/Config.json Normal file
View File

@@ -0,0 +1,21 @@
{
"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":[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":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] //其他参数
}
]
}

5
基础智能/e9.雷达传感器数据获取实验/Python38Run.bat Normal file
View 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
基础智能/e9.雷达传感器数据获取实验/Readme.pdf Normal file
View File

Binary file not shown.

Some files were not shown because too many files have changed in this diff Show More