diff --git a/基础智能/e1.集群Python接口演示实验/HITLMAVLinkFull4Swarm.bat b/基础智能/e1.集群Python接口演示实验/HITLMAVLinkFull4Swarm.bat new file mode 100644 index 0000000..5c86b1c --- /dev/null +++ b/基础智能/e1.集群Python接口演示实验/HITLMAVLinkFull4Swarm.bat @@ -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. diff --git a/基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.bat b/基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.bat new file mode 100644 index 0000000..007c31d --- /dev/null +++ b/基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.bat @@ -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. diff --git a/基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.py b/基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.py new file mode 100644 index 0000000..7dfc2c4 --- /dev/null +++ b/基础智能/e1.集群Python接口演示实验/MAVLinkFull4Swarm.py @@ -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) diff --git a/基础智能/e1.集群Python接口演示实验/Python38Run.bat b/基础智能/e1.集群Python接口演示实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e1.集群Python接口演示实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e1.集群Python接口演示实验/Readme.pdf b/基础智能/e1.集群Python接口演示实验/Readme.pdf new file mode 100644 index 0000000..5c11869 Binary files /dev/null and b/基础智能/e1.集群Python接口演示实验/Readme.pdf differ diff --git a/基础智能/e1.集群Python接口演示实验/名称-集群接口实验.txt b/基础智能/e1.集群Python接口演示实验/名称-集群接口实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e10.距离传感器数据获取实验/Config.json b/基础智能/e10.距离传感器数据获取实验/Config.json new file mode 100644 index 0000000..a3d43a4 --- /dev/null +++ b/基础智能/e10.距离传感器数据获取实验/Config.json @@ -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] + } + ] +} \ No newline at end of file diff --git a/基础智能/e10.距离传感器数据获取实验/Python38Run.bat b/基础智能/e10.距离传感器数据获取实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e10.距离传感器数据获取实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e10.距离传感器数据获取实验/RangingImage.bat b/基础智能/e10.距离传感器数据获取实验/RangingImage.bat new file mode 100644 index 0000000..32e5d09 --- /dev/null +++ b/基础智能/e10.距离传感器数据获取实验/RangingImage.bat @@ -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. diff --git a/基础智能/e10.距离传感器数据获取实验/RangingImage.py b/基础智能/e10.距离传感器数据获取实验/RangingImage.py new file mode 100644 index 0000000..3003c3f --- /dev/null +++ b/基础智能/e10.距离传感器数据获取实验/RangingImage.py @@ -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] \ No newline at end of file diff --git a/基础智能/e10.距离传感器数据获取实验/Readme.pdf b/基础智能/e10.距离传感器数据获取实验/Readme.pdf new file mode 100644 index 0000000..5f19484 Binary files /dev/null and b/基础智能/e10.距离传感器数据获取实验/Readme.pdf differ diff --git a/基础智能/e10.距离传感器数据获取实验/WinWSL.bat b/基础智能/e10.距离传感器数据获取实验/WinWSL.bat new file mode 100644 index 0000000..3cd2942 --- /dev/null +++ b/基础智能/e10.距离传感器数据获取实验/WinWSL.bat @@ -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 + diff --git a/基础智能/e10.距离传感器数据获取实验/名称-测距传感器实验.txt b/基础智能/e10.距离传感器数据获取实验/名称-测距传感器实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/Config.json b/基础智能/e11.无人机跟随圆形案板移动实验/Config.json new file mode 100644 index 0000000..26bfa09 --- /dev/null +++ b/基础智能/e11.无人机跟随圆形案板移动实验/Config.json @@ -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] //其他参数 + } + ] +} diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/Python38Run.bat b/基础智能/e11.无人机跟随圆形案板移动实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e11.无人机跟随圆形案板移动实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/Readme.pdf b/基础智能/e11.无人机跟随圆形案板移动实验/Readme.pdf new file mode 100644 index 0000000..530e59f Binary files /dev/null and b/基础智能/e11.无人机跟随圆形案板移动实验/Readme.pdf differ diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/WinWSL.bat b/基础智能/e11.无人机跟随圆形案板移动实验/WinWSL.bat new file mode 100644 index 0000000..3cd2942 --- /dev/null +++ b/基础智能/e11.无人机跟随圆形案板移动实验/WinWSL.bat @@ -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 + diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/WslGUI.bat b/基础智能/e11.无人机跟随圆形案板移动实验/WslGUI.bat new file mode 100644 index 0000000..babe367 --- /dev/null +++ b/基础智能/e11.无人机跟随圆形案板移动实验/WslGUI.bat @@ -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" diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.bat b/基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.bat new file mode 100644 index 0000000..d0c1247 --- /dev/null +++ b/基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.bat @@ -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. diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.py b/基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.py new file mode 100644 index 0000000..57ca917 --- /dev/null +++ b/基础智能/e11.无人机跟随圆形案板移动实验/circle_follow.py @@ -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() diff --git a/基础智能/e11.无人机跟随圆形案板移动实验/名称-无人机跟随圆形案板移动实验.txt b/基础智能/e11.无人机跟随圆形案板移动实验/名称-无人机跟随圆形案板移动实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e12.撞击小球实验/Python38Run.bat b/基础智能/e12.撞击小球实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e12.撞击小球实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e12.撞击小球实验/Readme.pdf b/基础智能/e12.撞击小球实验/Readme.pdf new file mode 100644 index 0000000..f9f8629 Binary files /dev/null and b/基础智能/e12.撞击小球实验/Readme.pdf differ diff --git a/基础智能/e12.撞击小球实验/ShootBall3.py b/基础智能/e12.撞击小球实验/ShootBall3.py new file mode 100644 index 0000000..0d5c4f2 --- /dev/null +++ b/基础智能/e12.撞击小球实验/ShootBall3.py @@ -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]) \ No newline at end of file diff --git a/基础智能/e12.撞击小球实验/ShootBall3HITL.bat b/基础智能/e12.撞击小球实验/ShootBall3HITL.bat new file mode 100644 index 0000000..a2cdc31 --- /dev/null +++ b/基础智能/e12.撞击小球实验/ShootBall3HITL.bat @@ -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. diff --git a/基础智能/e12.撞击小球实验/ShootBall3SITL.bat b/基础智能/e12.撞击小球实验/ShootBall3SITL.bat new file mode 100644 index 0000000..4027641 --- /dev/null +++ b/基础智能/e12.撞击小球实验/ShootBall3SITL.bat @@ -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. diff --git a/基础智能/e12.撞击小球实验/WinWSL.bat b/基础智能/e12.撞击小球实验/WinWSL.bat new file mode 100644 index 0000000..3cd2942 --- /dev/null +++ b/基础智能/e12.撞击小球实验/WinWSL.bat @@ -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 + diff --git a/基础智能/e12.撞击小球实验/WslGUI.bat b/基础智能/e12.撞击小球实验/WslGUI.bat new file mode 100644 index 0000000..babe367 --- /dev/null +++ b/基础智能/e12.撞击小球实验/WslGUI.bat @@ -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" diff --git a/基础智能/e12.撞击小球实验/名称-屏幕截图接口、撞击小球实验.txt b/基础智能/e12.撞击小球实验/名称-屏幕截图接口、撞击小球实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e13.四旋翼无人机穿环实验/Config1.json b/基础智能/e13.四旋翼无人机穿环实验/Config1.json new file mode 100644 index 0000000..144ff65 --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/Config1.json @@ -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] + } + ] +} diff --git a/基础智能/e13.四旋翼无人机穿环实验/Config2.json b/基础智能/e13.四旋翼无人机穿环实验/Config2.json new file mode 100644 index 0000000..c6d3c30 --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/Config2.json @@ -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] + } + ] +} diff --git a/基础智能/e13.四旋翼无人机穿环实验/CrossRing2HITL.bat b/基础智能/e13.四旋翼无人机穿环实验/CrossRing2HITL.bat new file mode 100644 index 0000000..7d5552a --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/CrossRing2HITL.bat @@ -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. diff --git a/基础智能/e13.四旋翼无人机穿环实验/CrossRing2SITL.bat b/基础智能/e13.四旋翼无人机穿环实验/CrossRing2SITL.bat new file mode 100644 index 0000000..7e3aef9 --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/CrossRing2SITL.bat @@ -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. diff --git a/基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle1.py b/基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle1.py new file mode 100644 index 0000000..0501706 --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle1.py @@ -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() \ No newline at end of file diff --git a/基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle2.py b/基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle2.py new file mode 100644 index 0000000..df2027c --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/CrossRing3_vehicle2.py @@ -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() \ No newline at end of file diff --git a/基础智能/e13.四旋翼无人机穿环实验/Python38Run.bat b/基础智能/e13.四旋翼无人机穿环实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e13.四旋翼无人机穿环实验/Readme.pdf b/基础智能/e13.四旋翼无人机穿环实验/Readme.pdf new file mode 100644 index 0000000..d850091 Binary files /dev/null and b/基础智能/e13.四旋翼无人机穿环实验/Readme.pdf differ diff --git a/基础智能/e13.四旋翼无人机穿环实验/readme.txt b/基础智能/e13.四旋翼无人机穿环实验/readme.txt new file mode 100644 index 0000000..47f71c6 --- /dev/null +++ b/基础智能/e13.四旋翼无人机穿环实验/readme.txt @@ -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. 可以看到两个飞机依次起飞。 \ No newline at end of file diff --git a/基础智能/e13.四旋翼无人机穿环实验/名称-双无人机分布式控制.txt b/基础智能/e13.四旋翼无人机穿环实验/名称-双无人机分布式控制.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/Config.json b/基础智能/e14.四旋翼无人机跟踪小球实验/Config.json new file mode 100644 index 0000000..2476e11 --- /dev/null +++ b/基础智能/e14.四旋翼无人机跟踪小球实验/Config.json @@ -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] //其他参数 + } + ] +} diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/Python38Run.bat b/基础智能/e14.四旋翼无人机跟踪小球实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e14.四旋翼无人机跟踪小球实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/Readme.pdf b/基础智能/e14.四旋翼无人机跟踪小球实验/Readme.pdf new file mode 100644 index 0000000..fa0ba17 Binary files /dev/null and b/基础智能/e14.四旋翼无人机跟踪小球实验/Readme.pdf differ diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3.py b/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3.py new file mode 100644 index 0000000..c855965 --- /dev/null +++ b/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3.py @@ -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]) \ No newline at end of file diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3HITL.bat b/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3HITL.bat new file mode 100644 index 0000000..38ef4ce --- /dev/null +++ b/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3HITL.bat @@ -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. diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3SITL.bat b/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3SITL.bat new file mode 100644 index 0000000..844b95c --- /dev/null +++ b/基础智能/e14.四旋翼无人机跟踪小球实验/ShootBall3SITL.bat @@ -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. diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/WinWSL.bat b/基础智能/e14.四旋翼无人机跟踪小球实验/WinWSL.bat new file mode 100644 index 0000000..3cd2942 --- /dev/null +++ b/基础智能/e14.四旋翼无人机跟踪小球实验/WinWSL.bat @@ -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 + diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/WslGUI.bat b/基础智能/e14.四旋翼无人机跟踪小球实验/WslGUI.bat new file mode 100644 index 0000000..babe367 --- /dev/null +++ b/基础智能/e14.四旋翼无人机跟踪小球实验/WslGUI.bat @@ -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" diff --git a/基础智能/e14.四旋翼无人机跟踪小球实验/名称-无人机跟踪小球实验.txt b/基础智能/e14.四旋翼无人机跟踪小球实验/名称-无人机跟踪小球实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.bat b/基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.bat new file mode 100644 index 0000000..f124aa5 --- /dev/null +++ b/基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.bat @@ -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 diff --git a/基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.py b/基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.py new file mode 100644 index 0000000..2e82cd7 --- /dev/null +++ b/基础智能/e2.4 机质点模型集群实验/NoPX4SITL4Swarm.py @@ -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) diff --git a/基础智能/e2.4 机质点模型集群实验/Python38Run.bat b/基础智能/e2.4 机质点模型集群实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e2.4 机质点模型集群实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e2.4 机质点模型集群实验/Readme.pdf b/基础智能/e2.4 机质点模型集群实验/Readme.pdf new file mode 100644 index 0000000..c28b9ad Binary files /dev/null and b/基础智能/e2.4 机质点模型集群实验/Readme.pdf differ diff --git a/基础智能/e2.4 机质点模型集群实验/名称-4机质点模型集群实验.txt b/基础智能/e2.4 机质点模型集群实验/名称-4机质点模型集群实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.bat b/基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.bat new file mode 100644 index 0000000..f124aa5 --- /dev/null +++ b/基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.bat @@ -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 diff --git a/基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.py b/基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.py new file mode 100644 index 0000000..c88b233 --- /dev/null +++ b/基础智能/e3.集群轨迹灯光展示实验/NightCitySwarm4.py @@ -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() diff --git a/基础智能/e3.集群轨迹灯光展示实验/Python38Run.bat b/基础智能/e3.集群轨迹灯光展示实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e3.集群轨迹灯光展示实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e3.集群轨迹灯光展示实验/Readme.pdf b/基础智能/e3.集群轨迹灯光展示实验/Readme.pdf new file mode 100644 index 0000000..cb50042 Binary files /dev/null and b/基础智能/e3.集群轨迹灯光展示实验/Readme.pdf differ diff --git a/基础智能/e3.集群轨迹灯光展示实验/名称-集群轨迹灯光展示实验.txt b/基础智能/e3.集群轨迹灯光展示实验/名称-集群轨迹灯光展示实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrl.dll b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrl.dll new file mode 100644 index 0000000..ac2f0b2 Binary files /dev/null and b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrl.dll differ diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrlSITLRun.bat b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrlSITLRun.bat new file mode 100644 index 0000000..614ccfe --- /dev/null +++ b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/CarNoCtrlSITLRun.bat @@ -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. diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MultiVehicleRunALL.bat b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MultiVehicleRunALL.bat new file mode 100644 index 0000000..a1b5215 --- /dev/null +++ b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MultiVehicleRunALL.bat @@ -0,0 +1,13 @@ +if not defined PSP_PATH ( + SET PSP_PATH=C:\PX4PSP + SET PSP_PATH_LINUX=/mnt/c/PX4PSP +) +@echo off +rem MulticopterNoCtrlSITLRun.bat +start "" MulticopterNoCtrlSITLRun.bat + +rem ʱ 5 +timeout /t 25 /nobreak >nul + +rem CarNoCtrlSITLRun.bat +start "" CarNoCtrlSITLRun.bat diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrl.dll b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrl.dll new file mode 100644 index 0000000..518edfa Binary files /dev/null and b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrl.dll differ diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrlSITLRun.bat b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrlSITLRun.bat new file mode 100644 index 0000000..134d8e9 --- /dev/null +++ b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/MulticopterNoCtrlSITLRun.bat @@ -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. diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/Python38Run.bat b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/Readme.pdf b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/Readme.pdf new file mode 100644 index 0000000..304a663 Binary files /dev/null and b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/Readme.pdf differ diff --git a/基础智能/e4.四旋翼与无人车异构集群协同控制实验/UDPSimpleMultiVehicleSwarm.py b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/UDPSimpleMultiVehicleSwarm.py new file mode 100644 index 0000000..6b158f3 --- /dev/null +++ b/基础智能/e4.四旋翼与无人车异构集群协同控制实验/UDPSimpleMultiVehicleSwarm.py @@ -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() diff --git a/基础智能/e5.2机碰撞实验/CollisionDemo.bat b/基础智能/e5.2机碰撞实验/CollisionDemo.bat new file mode 100644 index 0000000..8fa0130 --- /dev/null +++ b/基础智能/e5.2机碰撞实验/CollisionDemo.bat @@ -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. diff --git a/基础智能/e5.2机碰撞实验/CollisionDemo.py b/基础智能/e5.2机碰撞实验/CollisionDemo.py new file mode 100644 index 0000000..7484cff --- /dev/null +++ b/基础智能/e5.2机碰撞实验/CollisionDemo.py @@ -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) diff --git a/基础智能/e5.2机碰撞实验/Python38Run.bat b/基础智能/e5.2机碰撞实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e5.2机碰撞实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e5.2机碰撞实验/Readme.pdf b/基础智能/e5.2机碰撞实验/Readme.pdf new file mode 100644 index 0000000..5997229 Binary files /dev/null and b/基础智能/e5.2机碰撞实验/Readme.pdf differ diff --git a/基础智能/e5.2机碰撞实验/名称-MAVLink模式2机碰撞实验。.txt b/基础智能/e5.2机碰撞实验/名称-MAVLink模式2机碰撞实验。.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e6.无人机视觉传感器介绍实验/Config (2).json b/基础智能/e6.无人机视觉传感器介绍实验/Config (2).json new file mode 100644 index 0000000..fafa20c --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/Config (2).json @@ -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] //其他参数 + } + ] +} diff --git a/基础智能/e6.无人机视觉传感器介绍实验/Config.json b/基础智能/e6.无人机视觉传感器介绍实验/Config.json new file mode 100644 index 0000000..2476e11 --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/Config.json @@ -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] //其他参数 + } + ] +} diff --git a/基础智能/e6.无人机视觉传感器介绍实验/Intro.pdf b/基础智能/e6.无人机视觉传感器介绍实验/Intro.pdf new file mode 100644 index 0000000..f84d1ec Binary files /dev/null and b/基础智能/e6.无人机视觉传感器介绍实验/Intro.pdf differ diff --git a/基础智能/e6.无人机视觉传感器介绍实验/Python38Run (2).bat b/基础智能/e6.无人机视觉传感器介绍实验/Python38Run (2).bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/Python38Run (2).bat @@ -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%" diff --git a/基础智能/e6.无人机视觉传感器介绍实验/Python38Run.bat b/基础智能/e6.无人机视觉传感器介绍实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.bat b/基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.bat new file mode 100644 index 0000000..31d721a --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.bat @@ -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. diff --git a/基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.py b/基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.py new file mode 100644 index 0000000..efd853d --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/ReadTimeStmp.py @@ -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('') diff --git a/基础智能/e6.无人机视觉传感器介绍实验/Readme (2).pdf b/基础智能/e6.无人机视觉传感器介绍实验/Readme (2).pdf new file mode 100644 index 0000000..e265c47 Binary files /dev/null and b/基础智能/e6.无人机视觉传感器介绍实验/Readme (2).pdf differ diff --git a/基础智能/e6.无人机视觉传感器介绍实验/Readme.pdf b/基础智能/e6.无人机视觉传感器介绍实验/Readme.pdf new file mode 100644 index 0000000..2011b41 Binary files /dev/null and b/基础智能/e6.无人机视觉传感器介绍实验/Readme.pdf differ diff --git a/基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.bat b/基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.bat new file mode 100644 index 0000000..ac34216 --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.bat @@ -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. diff --git a/基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.py b/基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.py new file mode 100644 index 0000000..a1a6c99 --- /dev/null +++ b/基础智能/e6.无人机视觉传感器介绍实验/VisionCapAPIDemo.py @@ -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() \ No newline at end of file diff --git a/基础智能/e6.无人机视觉传感器介绍实验/名称-Imu和相机数据获取实验.txt b/基础智能/e6.无人机视觉传感器介绍实验/名称-Imu和相机数据获取实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e6.无人机视觉传感器介绍实验/名称-时间戳获取实验.txt b/基础智能/e6.无人机视觉传感器介绍实验/名称-时间戳获取实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e7.相机取图实验/Config.json b/基础智能/e7.相机取图实验/Config.json new file mode 100644 index 0000000..09598c5 --- /dev/null +++ b/基础智能/e7.相机取图实验/Config.json @@ -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] + } + ] +} diff --git a/基础智能/e7.相机取图实验/Readme.pdf b/基础智能/e7.相机取图实验/Readme.pdf new file mode 100644 index 0000000..5482880 Binary files /dev/null and b/基础智能/e7.相机取图实验/Readme.pdf differ diff --git a/基础智能/e7.相机取图实验/Readme_EN .pdf b/基础智能/e7.相机取图实验/Readme_EN .pdf new file mode 100644 index 0000000..fd0b714 Binary files /dev/null and b/基础智能/e7.相机取图实验/Readme_EN .pdf differ diff --git a/基础智能/e7.相机取图实验/SmallFixedWingUAV.dll b/基础智能/e7.相机取图实验/SmallFixedWingUAV.dll new file mode 100644 index 0000000..61133af Binary files /dev/null and b/基础智能/e7.相机取图实验/SmallFixedWingUAV.dll differ diff --git a/基础智能/e7.相机取图实验/VisionCapAPIDemo.bat b/基础智能/e7.相机取图实验/VisionCapAPIDemo.bat new file mode 100644 index 0000000..bf3195b --- /dev/null +++ b/基础智能/e7.相机取图实验/VisionCapAPIDemo.bat @@ -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. diff --git a/基础智能/e7.相机取图实验/VisionCapAPIDemo.py b/基础智能/e7.相机取图实验/VisionCapAPIDemo.py new file mode 100644 index 0000000..00d2ddc --- /dev/null +++ b/基础智能/e7.相机取图实验/VisionCapAPIDemo.py @@ -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() \ No newline at end of file diff --git a/基础智能/e7.相机取图实验/名称-相机取图实验.txt b/基础智能/e7.相机取图实验/名称-相机取图实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e8.多目相机实验取图实验/Config.json b/基础智能/e8.多目相机实验取图实验/Config.json new file mode 100644 index 0000000..89f336d --- /dev/null +++ b/基础智能/e8.多目相机实验取图实验/Config.json @@ -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] + } + ] +} diff --git a/基础智能/e8.多目相机实验取图实验/Python38Run.bat b/基础智能/e8.多目相机实验取图实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e8.多目相机实验取图实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e8.多目相机实验取图实验/Readme.pdf b/基础智能/e8.多目相机实验取图实验/Readme.pdf new file mode 100644 index 0000000..6400838 Binary files /dev/null and b/基础智能/e8.多目相机实验取图实验/Readme.pdf differ diff --git a/基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.bat b/基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.bat new file mode 100644 index 0000000..aa5fbe1 --- /dev/null +++ b/基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.bat @@ -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. diff --git a/基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.py b/基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.py new file mode 100644 index 0000000..79ab5c9 --- /dev/null +++ b/基础智能/e8.多目相机实验取图实验/VisionCapAPIDemo.py @@ -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() \ No newline at end of file diff --git a/基础智能/e8.多目相机实验取图实验/名称-多目相机实验取图实验.txt b/基础智能/e8.多目相机实验取图实验/名称-多目相机实验取图实验.txt new file mode 100644 index 0000000..e69de29 diff --git a/基础智能/e9.雷达传感器数据获取实验/Config.json b/基础智能/e9.雷达传感器数据获取实验/Config.json new file mode 100644 index 0000000..44b144a --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/Config.json @@ -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] //其他参数 + } + ] +} diff --git a/基础智能/e9.雷达传感器数据获取实验/Python38Run.bat b/基础智能/e9.雷达传感器数据获取实验/Python38Run.bat new file mode 100644 index 0000000..ad265a4 --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/Python38Run.bat @@ -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%" diff --git a/基础智能/e9.雷达传感器数据获取实验/Readme.pdf b/基础智能/e9.雷达传感器数据获取实验/Readme.pdf new file mode 100644 index 0000000..7b6b357 Binary files /dev/null and b/基础智能/e9.雷达传感器数据获取实验/Readme.pdf differ diff --git a/基础智能/e9.雷达传感器数据获取实验/WinWSL.bat b/基础智能/e9.雷达传感器数据获取实验/WinWSL.bat new file mode 100644 index 0000000..3cd2942 --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/WinWSL.bat @@ -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 + diff --git a/基础智能/e9.雷达传感器数据获取实验/WslGUI.bat b/基础智能/e9.雷达传感器数据获取实验/WslGUI.bat new file mode 100644 index 0000000..8437ae7 --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/WslGUI.bat @@ -0,0 +1,21 @@ +@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" +wsl --shutdown diff --git a/基础智能/e9.雷达传感器数据获取实验/client_ue4_SITL.bat b/基础智能/e9.雷达传感器数据获取实验/client_ue4_SITL.bat new file mode 100644 index 0000000..32e5d09 --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/client_ue4_SITL.bat @@ -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. diff --git a/基础智能/e9.雷达传感器数据获取实验/err.log b/基础智能/e9.雷达传感器数据获取实验/err.log new file mode 100644 index 0000000..92991e0 --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/err.log @@ -0,0 +1,106 @@ +gpg-agent[345]: WARNING: "--write-env-file" is an obsolete option - it has no effect +gpg-agent[346]: gpg-agent (GnuPG) 2.2.19 started + +(xfce4-session:296): xfce4-session-WARNING **: 14:45:54.182: gpg-agent returned no PID in the variables + +(xfce4-session:296): xfce4-session-WARNING **: 14:45:54.185: xfsm_manager_load_session: Something wrong with /root/.cache/sessions/xfce4-session-localhost:0, Does it exist? Permissions issue? + +(xfwm4:347): xfwm4-WARNING **: 14:45:56.494: Unsupported GL renderer (llvmpipe (LLVM 12.0.0, 256 bits)). + +(xfsettingsd:385): libupower-glib-WARNING **: 14:45:56.733: Couldn't connect to proxy: Could not connect: No such file or directory + +(xfsettingsd:385): libupower-glib-CRITICAL **: 14:45:56.733: up_client_get_lid_is_closed: assertion 'UP_IS_CLIENT (client)' failed + +(xfsettingsd:385): GLib-GObject-WARNING **: 14:45:56.733: invalid (NULL) pointer instance + +(xfsettingsd:385): GLib-GObject-CRITICAL **: 14:45:56.733: g_signal_connect_data: assertion 'G_TYPE_CHECK_INSTANCE (instance)' failed + +(wrapper-2.0:406): GLib-GIO-CRITICAL **: 14:45:57.491: g_file_new_for_path: assertion 'path != NULL' failed + +(wrapper-2.0:406): GLib-GIO-CRITICAL **: 14:45:57.491: g_file_monitor_file: assertion 'G_IS_FILE (file)' failed + +(wrapper-2.0:406): GLib-GObject-WARNING **: 14:45:57.491: invalid (NULL) pointer instance + +(wrapper-2.0:406): GLib-GObject-CRITICAL **: 14:45:57.491: g_signal_connect_data: assertion 'G_TYPE_CHECK_INSTANCE (instance)' failed + +(wrapper-2.0:406): Gtk-WARNING **: 14:45:57.491: Attempting to add a widget with type GtkToggleButton to a container of type XfcePanelPlugin, but the widget is already inside a container of type XfcePanelPlugin, please remove the widget from its existing container first. + +(xfce4-session:296): xfce4-session-WARNING **: 14:45:57.506: failed to run script: Failed to execute child process “/usr/bin/pm-is-supported” (No such file or directory) + +(xfce4-session:296): xfce4-session-WARNING **: 14:45:57.516: failed to run script: Failed to execute child process “/usr/bin/pm-is-supported” (No such file or directory) + +(xfce4-session:296): xfce4-session-WARNING **: 14:45:57.525: failed to run script: Failed to execute child process “/usr/bin/pm-is-supported” (No such file or directory) + +(wrapper-2.0:407): Gtk-WARNING **: 14:45:57.577: Negative content width -1 (allocation 1, extents 1x1) while allocating gadget (node button, owner XfceArrowButton) + +** (agent:423): CRITICAL **: 14:45:57.584: Failed to get connection to system bus: Could not connect: No such file or directory + +(wrapper-2.0:406): Gtk-WARNING **: 14:45:57.611: Negative content width -3 (allocation 1, extents 2x2) while allocating gadget (node button, owner GtkToggleButton) + +** (wrapper-2.0:405): WARNING **: 14:45:57.637: Binding 'XF86AudioMicMute' failed! + +(wrapper-2.0:405): pulseaudio-plugin-WARNING **: 14:45:57.638: Could not have grabbed volume control keys. Is another volume control application (xfce4-volumed) running? +shared memfd open() failed: Function not implemented + +(wrapper-2.0:405): Gtk-WARNING **: 14:45:57.817: Negative content width -3 (allocation 1, extents 2x2) while allocating gadget (node button, owner PulseaudioButton) + +(nm-applet:442): nm-applet-WARNING **: 14:45:57.944: Error connecting to system D-Bus: Could not connect: No such file or directory + +(nm-applet:442): nm-applet-WARNING **: 14:45:57.945: Could not connect: No such file or directory + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.946: ((libnm/nm-client.c:3905)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.950: ((libnm/nm-client.c:3859)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.950: ((libnm/nm-client.c:3937)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.950: ((libnm/nm-client.c:3986)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.950: ((libnm/nm-client.c:4026)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.950: ((libnm/nm-client.c:4042)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.950: ((libnm/nm-client.c:4080)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.950: ((libnm/nm-client.c:4598)): assertion '' failed + +(nm-applet:442): libnm-CRITICAL **: 14:45:57.951: ((libnm/nm-client.c:4598)): assertion '' failed +shared memfd open() failed: Function not implemented +Connection failure: Connection refused +pa_context_connect() failed: Connection refused + +** (xiccd:485): CRITICAL **: 14:45:58.141: Failed to connect to colord: Could not connect: No such file or directory + +(xfdesktop:400): GVFS-RemoteVolumeMonitor-WARNING **: 14:45:58.495: remote volume monitor with dbus name org.gtk.vfs.UDisks2VolumeMonitor is not supported + +** (xfdesktop:400): WARNING **: 14:45:58.676: Failed to get system bus: Could not connect: No such file or directory +system-config-printer-applet: failed to connect to system D-Bus + +(xfce4-panel:386): xfce4-panel-CRITICAL **: 14:46:07.956: panel-window.c:2245 (panel_window_active_window_geometry_changed): expression 'WNCK_IS_WINDOW (active_window)' failed. + +(xfce4-panel:386): xfce4-panel-CRITICAL **: 14:46:34.605: panel-window.c:2245 (panel_window_active_window_geometry_changed): expression 'WNCK_IS_WINDOW (active_window)' failed. +Gdk-Message: 14:58:12.289: xfce4-session: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + +Gdk-Message: 14:58:12.290: xfwm4: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + +xfsettingsd: Another instance took over. Leaving... + +(xfce4-panel:386): xfce4-panel-CRITICAL **: 14:58:12.291: Name org.xfce.Panel lost on the message dbus, exiting. + +(Thunar:394): thunar-WARNING **: 14:58:12.291: Name 'org.xfce.FileManager' lost on the message dbus. + +(Thunar:394): thunar-WARNING **: 14:58:12.292: Name 'org.freedesktop.FileManager1' lost on the message dbus. +Gdk-Message: 14:58:12.298: xfsettingsd: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + +Gdk-Message: 14:58:12.300: xfce4-panel: Fatal IO error 25 (Inappropriate ioctl for device) on X server :0.0. + +Gdk-Message: 14:58:12.300: xfdesktop: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + +Gdk-Message: 14:58:12.302: wrapper-2.0: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + +Gdk-Message: 14:58:12.301: wrapper-2.0: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + +Gdk-Message: 14:58:12.302: wrapper-2.0: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + +Gdk-Message: 14:58:12.302: wrapper-2.0: Fatal IO error 11 (Resource temporarily unavailable) on X server :0.0. + diff --git a/基础智能/e9.雷达传感器数据获取实验/lidar.rviz b/基础智能/e9.雷达传感器数据获取实验/lidar.rviz new file mode 100644 index 0000000..969cafb --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/lidar.rviz @@ -0,0 +1,185 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Axes1 + - /PointCloud21/Status1 + Splitter Ratio: 0.5 + Tree Height: 546 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.4648884534835815 + Min Value: -7.074190616607666 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: /lidar_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Axes + Enabled: true + Length: 2 + Name: Axes + Radius: 0.20000000298023224 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 12.140262603759766 + Min Value: -14.380321502685547 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /rflysim/vehicle_0/lidar_0 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: rflysim_vehicle0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 111.6949691772461 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 25.732120513916016 + Y: 8.539153099060059 + Z: 0.2620249390602112 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.2752034068107605 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.4104323387146 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 846 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002aefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e000002ae000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002ae000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026200fffffffb0000000800540069006d00650100000000000004500000000000000000000004b0000002ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 1063 + Y: 195 diff --git a/基础智能/e9.雷达传感器数据获取实验/out.log b/基础智能/e9.雷达传感器数据获取实验/out.log new file mode 100644 index 0000000..def6e80 --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/out.log @@ -0,0 +1 @@ +/usr/bin/startxfce4: X server already running on display :0.0 diff --git a/基础智能/e9.雷达传感器数据获取实验/server_ue4.py b/基础智能/e9.雷达传感器数据获取实验/server_ue4.py new file mode 100644 index 0000000..ad0e771 --- /dev/null +++ b/基础智能/e9.雷达传感器数据获取实验/server_ue4.py @@ -0,0 +1,106 @@ + +# import required libraries +# pip3 install pymavlink pyserial + +import cv2 +import numpy as np +import time +import VisionCaptureApi +import PX4MavCtrlV4 as PX4MavCtrl +import math +# import matplotlib.pyplot as plt +import ReqCopterSim +import Open3DShow + +show3d=Open3DShow.Open3DShow() + + +vis = VisionCaptureApi.VisionCaptureApi() + +# VisionCaptureApi 中的配置函数 +vis.jsonLoad() +isSuss = vis.sendReqToUE4() + +vis.startImgCap() # 开启取图循环，执行本语句之后，已经可以通过vis.Img[i]读取到图片了 +print('Start Image Reciver') + +# vis.sendImuReqCopterSim(1,'127.0.0.1') # 发送请求，从目标飞机CopterSim读取IMU数据,回传地址为127.0.0.1，默认频率为200Hz +# 执行本语句之后，会自动开启数据监听，已经可以通过vis.imu读取到IMU数据了。 + + +mav = PX4MavCtrl.PX4MavCtrler(1) + + +time.sleep(2) +# Start MAV loop with UDP mode: MAVLINK_FULL +mav.InitMavLoop() +mav.InitTrueDataLoop() + +# Enter Offboard mode to start vehicle control +time.sleep(2) +mav.initOffboard() + +# 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(2) +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 +mav.SendPosNED(0, 0, -8, 0) +mav.SendCopterSpeed(3) +#time.sleep(10) + + +#创建点云显示窗口 +show3d.CreatShow(0) + + +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 + + + # target position in UE4 map global frame + if t < 10: + # fly to 15 meters high in 0 to 10s + targetPosE = np.array([-0, 0, -5]) + elif t<20: # fly circle after 10s + targetPosE = np.array( + [15,5, -5]) + else: + targetPosE = np.array([15,-10, -5]) + + # target position in vehilce takeoff frame + targetPosE = targetPosE+Error2UE4Map + mav.SendPosNED(targetPosE[0], targetPosE[1], targetPosE[2], 0) + + if vis.hasData[0]: + + # 更新点云 + show3d.UpdateShow(vis.Img[0]) + + # 标记 + vis.hasData[0]=False + + #isShowCloud = False + if t > 200: # end simulation when 200s + break + + +mav.endOffboard() + +time.sleep(1) +mav.stopRun() diff --git a/基础智能/e9.雷达传感器数据获取实验/名称-UDP直传点云数据入门实验.txt b/基础智能/e9.雷达传感器数据获取实验/名称-UDP直传点云数据入门实验.txt new file mode 100644 index 0000000..e69de29