second run causes significant slow down

[tfoote]

Sometimes the simulation seems to run much slower. I've been able to isolate it that if you launch a drone a 2nd time, the ardupilot does not speed up after "Ready to Fly" When in the first run it goes from 50% blocking to ~10% blocking. And in the 2nd run there's no noticable speedup, which slows the simulation down greatly across many vehicles.

tfoote@snowman:~ Last: [0] (40s Seconds)
$ spawn_blue 26 --acs enp0s25 --no-payload
SpawnModel: Successfully spawned model (delta_wing_26)
ROS_MASTER_URI=http://localhost:11337 roslaunch /tmp/uctf_kK8dDw.launch
... logging to /home/tfoote/.ros/log/76215d22-c400-11e6-a0c7-3c970ee1fca6/roslaunch-snowman-6471.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://snowman:39644/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.6

NODES
  /delta_wing_26/
    ardupilot_delta_wing_26 (uctf/arduplane.sh)
    ardupilot_delta_wing_26_mavproxy (uctf/mavproxy.sh)

auto-starting new master
process[master]: started with pid [6482]
ROS_MASTER_URI=http://localhost:11337

setting /run_id to 76215d22-c400-11e6-a0c7-3c970ee1fca6
process[rosout-1]: started with pid [6495]
started core service [/rosout]
process[delta_wing_26/ardupilot_delta_wing_26-2]: started with pid [6499]
temporary rootfs: /tmp/tmp.x9m8T0JlvR
Running [[arduplane -S --base-port 14260 --rc-in-port 14261 --gazebo-address 127.0.0.1 --gazebo-port-in 14264 --gazebo-port-out 14265 --home 35.72068,-120.771775,282.0,353.0 --model gazebo-zephyr --defaults /tmp/delta_wing_26_oP74NY -F 127.0.0.1]]
Starting SITL Gazebo
Started model gazebo-zephyr at 35.72068,-120.771775,282.0,353.0 at speed 1.0
Starting sketch 'ArduPlane'
Starting SITL input
bind port 14260 for 0
Serial port 0 on TCP port 14260
Waiting for connection ....
process[delta_wing_26/ardupilot_delta_wing_26_mavproxy-3]: started with pid [6515]
mavproxy.py --mav10 --master tcp:127.0.0.1:14260 --sitl 127.0.0.1:14261 --out udpbcast:127.0.0.255:14000 --out 127.0.0.1:14263 --aircraft delta_wing_26 __name:=ardupilot_delta_wing_26_mavproxy __log:=/home/tfoote/.ros/log/76215d22-c400-11e6-a0c7-3c970ee1fca6/delta_wing_26-ardupilot_delta_wing_26_mavproxy-3.log
Connect tcp:127.0.0.1:14260 source_system=255
Loaded defaults from /tmp/delta_wing_26_oP74NY
bind port 14262 for 2
Serial port 2 on TCP port 14262
bind port 14263 for 3
Serial port 3 on TCP port 14263
Failed to load module: No module named adsb. Use 'set moddebug 3' in the MAVProxy console to enable traceback
no script delta_wing_26/mavinit.scr
Log Directory: delta_wing_26/logs/2016-12-16/flight62
Telemetry log: delta_wing_26/logs/2016-12-16/flight62/flight.tlog
Waiting for heartbeat from tcp:127.0.0.1:14260MAV> 
setup:252: bad header in setup - erasing

Init ArduPlane V3.8.0beta1 (eb9d83d0)

Free RAM: 131072
Firmware change: erasing EEPROM...
done.
load_all took 0us
0 0 0 DataFlash_File: buffer size=16384
online system 26
26:INITIALISING> Mode INITIALISING
Freq: (0.000000) Average[max] wait 0.001869[0.002088], acting: 14819416.951955[1481941694.991898] blocking: 1.000000 %
APM: <startup_ground> Ground start
APM: Calibrating barometer
Freq: (253.706054) Average[max] wait 0.001900[0.002096], acting: 0.002042[0.100177] blocking: 0.518078 %
APM: ArduPlane V3.8.0beta1 (eb9d83d0)
Freq: (203.621619) Average[max] wait 0.001866[0.002136], acting: 0.003045[0.100183] blocking: 0.620044 %
Freq: (254.017811) Average[max] wait 0.001893[0.002102], acting: 0.002044[0.100150] blocking: 0.519178 %
Received 762 parameters
Saved 762 parameters to delta_wing_26/logs/2016-12-16/flight62/mav.parm
Freq: (204.017600) Average[max] wait 0.001860[0.002118], acting: 0.003042[0.100167] blocking: 0.620537 %
APM: Initialising APM
Freq: (254.052122) Average[max] wait 0.001891[0.002108], acting: 0.002045[0.100174] blocking: 0.519471 %
Freq: (203.876880) Average[max] wait 0.001863[0.002094], acting: 0.003042[0.100185] blocking: 0.620109 %
Freq: (253.998583) Average[max] wait 0.001896[0.002085], acting: 0.002041[0.100163] blocking: 0.518490 %
Freq: (203.851117) Average[max] wait 0.001859[0.002101], acting: 0.003046[0.100199] blocking: 0.620983 %
Freq: (253.787876) Average[max] wait 0.001895[0.002200], acting: 0.002045[0.100180] blocking: 0.518974 %
Freq: (203.885997) Average[max] wait 0.001859[0.002128], acting: 0.003046[0.100169] blocking: 0.620942 %
Freq: (254.068281) Average[max] wait 0.001892[0.002101], acting: 0.002044[0.100188] blocking: 0.519357 %
Freq: (203.873213) Average[max] wait 0.001860[0.002104], acting: 0.003045[0.100182] blocking: 0.620780 %
Freq: (254.219347) Average[max] wait 0.001891[0.002121], acting: 0.002043[0.100156] blocking: 0.519384 %
Freq: (204.013830) Average[max] wait 0.001861[0.002129], acting: 0.003040[0.100165] blocking: 0.620312 %
Freq: (336.724202) Average[max] wait 0.001931[0.002112], acting: 0.001039[0.100159] blocking: 0.349800 %
Freq: (498.221073) Average[max] wait 0.001967[0.002149], acting: 0.000040[0.000136] blocking: 0.019909 %
APM: Barometer calibration complete
APM: No airspeed
APM: Ready to fly
Freq: (498.584119) Average[max] wait 0.001960[0.002097], acting: 0.000046[0.000433] blocking: 0.022858 %
Freq: (498.221073) Average[max] wait 0.001964[0.002082], acting: 0.000043[0.000112] blocking: 0.021517 %
Freq: (499.660365) Average[max] wait 0.001957[0.002092], acting: 0.000045[0.000129] blocking: 0.022364 %
Freq: (498.986871) Average[max] wait 0.001956[0.002100], acting: 0.000048[0.000111] blocking: 0.024068 %
Freq: (500.783716) Average[max] wait 0.001951[0.002425], acting: 0.000045[0.000094] blocking: 0.022722 %
26:MANUAL> Mode MANUAL
Freq: (333.231163) Average[max] wait 0.001925[0.002099], acting: 0.001076[0.103596] blocking: 0.358611 %
Freq: (497.842010) Average[max] wait 0.001962[0.002106], acting: 0.000046[0.000111] blocking: 0.023135 %
APM: GPS 0: detected as u-blox at 38400 baud
Freq: (498.469759) Average[max] wait 0.001962[0.002083], acting: 0.000044[0.000103] blocking: 0.022090 %
GPS lock at 0 meters
Freq: (457.362191) Average[max] wait 0.001922[0.002186], acting: 0.000265[0.022149] blocking: 0.120974 %
Freq: (457.777007) Average[max] wait 0.001923[0.002084], acting: 0.000261[0.021704] blocking: 0.119485 %
Freq: (461.259232) Average[max] wait 0.001919[0.002099], acting: 0.000249[0.020072] blocking: 0.114734 %
Freq: (459.050096) Average[max] wait 0.001924[0.002225], acting: 0.000254[0.020948] blocking: 0.116763 %
APM: Init HOME
APM: GPS alt: 33242
Freq: (461.454610) Average[max] wait 0.001923[0.002116], acting: 0.000244[0.019866] blocking: 0.112611 %
Freq: (457.226578) Average[max] wait 0.001921[0.002157], acting: 0.000266[0.022171] blocking: 0.121607 %
Freq: (455.413040) Average[max] wait 0.001925[0.002228], acting: 0.000270[0.022470] blocking: 0.123142 %
Freq: (457.584231) Average[max] wait 0.001918[0.002096], acting: 0.000267[0.021977] blocking: 0.122256 %
Freq: (455.919458) Average[max] wait 0.001920[0.002160], acting: 0.000273[0.022702] blocking: 0.124639 %
Freq: (461.889093) Average[max] wait 0.001922[0.002104], acting: 0.000243[0.019906] blocking: 0.112194 %
Freq: (461.491166) Average[max] wait 0.001923[0.002088], acting: 0.000244[0.019931] blocking: 0.112400 %
Freq: (461.384559) Average[max] wait 0.001920[0.002077], acting: 0.000247[0.019893] blocking: 0.113951 %
Freq: (425.868683) Average[max] wait 0.001888[0.002101], acting: 0.000460[0.022378] blocking: 0.196048 %
Freq: (457.961444) Average[max] wait 0.001928[0.002125], acting: 0.000255[0.022093] blocking: 0.116692 %
Freq: (458.740818) Average[max] wait 0.001926[0.002111], acting: 0.000254[0.020545] blocking: 0.116425 %
Freq: (458.051968) Average[max] wait 0.001925[0.002096], acting: 0.000258[0.020920] blocking: 0.118069 %
Freq: (463.823915) Average[max] wait 0.001917[0.002293], acting: 0.000239[0.019662] blocking: 0.110848 %
Freq: (457.705571) Average[max] wait 0.001928[0.002245], acting: 0.000257[0.021156] blocking: 0.117538 %
Freq: (455.697050) Average[max] wait 0.001925[0.002270], acting: 0.000269[0.022481] blocking: 0.122825 %
Freq: (456.654600) Average[max] wait 0.001921[0.002109], acting: 0.000269[0.022102] blocking: 0.122906 %
Freq: (456.817236) Average[max] wait 0.001922[0.002111], acting: 0.000268[0.021856] blocking: 0.122244 %
APM: EKF2 IMU0 initial yaw alignment complete
APM: EKF2 IMU1 initial yaw alignment complete
Freq: (457.000403) Average[max] wait 0.001921[0.002174], acting: 0.000267[0.021925] blocking: 0.121957 %
Freq: (460.844667) Average[max] wait 0.001924[0.002093], acting: 0.000246[0.020144] blocking: 0.113409 %
Freq: (461.629321) Average[max] wait 0.001921[0.002197], acting: 0.000245[0.019829] blocking: 0.113240 %
Freq: (460.349990) Average[max] wait 0.001920[0.002090], acting: 0.000252[0.019845] blocking: 0.116161 %
Freq: (459.939074) Average[max] wait 0.001918[0.002103], acting: 0.000256[0.021112] blocking: 0.117954 %
Freq: (419.031063) Average[max] wait 0.001887[0.002101], acting: 0.000499[0.022973] blocking: 0.209163 %
Freq: (458.040963) Average[max] wait 0.001922[0.002139], acting: 0.000261[0.021761] blocking: 0.119530 %
Freq: (458.465030) Average[max] wait 0.001924[0.002087], acting: 0.000257[0.020838] blocking: 0.117689 %
Freq: (462.357522) Average[max] wait 0.001922[0.002097], acting: 0.000241[0.019681] blocking: 0.111223 %
Freq: (461.105584) Average[max] wait 0.001922[0.002088], acting: 0.000247[0.020142] blocking: 0.113891 %
Freq: (461.721301) Average[max] wait 0.001925[0.002102], acting: 0.000241[0.019724] blocking: 0.111356 %
Freq: (454.687802) Average[max] wait 0.001918[0.002109], acting: 0.000281[0.023519] blocking: 0.127766 %
Freq: (456.077604) Average[max] wait 0.001926[0.002112], acting: 0.000266[0.022021] blocking: 0.121470 %
Freq: (458.017955) Average[max] wait 0.001924[0.002200], acting: 0.000260[0.021643] blocking: 0.118985 %
Freq: (459.637161) Average[max] wait 0.001920[0.002106], acting: 0.000255[0.020821] blocking: 0.117239 %
[delta_wing_26/ardupilot_delta_wing_26_mavproxy-3] killing on exit
[delta_wing_26/ardupilot_delta_wing_26-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
DeleteModel: successfully deleted model (delta_wing_26)
tfoote@snowman:~ Last: [0] (20s Seconds)
$ 
tfoote@snowman:~ Last: [0] (0s Seconds)
$ 
tfoote@snowman:~ Last: [0] (0s Seconds)
$ spawn_blue 26 --acs enp0s25 --no-payload
SpawnModel: Successfully spawned model (delta_wing_26)
ROS_MASTER_URI=http://localhost:11337 roslaunch /tmp/uctf_oCqShZ.launch
... logging to /home/tfoote/.ros/log/82b6a2d6-c400-11e6-a0c7-3c970ee1fca6/roslaunch-snowman-6542.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://snowman:44445/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.6

NODES
  /delta_wing_26/
    ardupilot_delta_wing_26 (uctf/arduplane.sh)
    ardupilot_delta_wing_26_mavproxy (uctf/mavproxy.sh)

auto-starting new master
process[master]: started with pid [6553]
ROS_MASTER_URI=http://localhost:11337

setting /run_id to 82b6a2d6-c400-11e6-a0c7-3c970ee1fca6
process[rosout-1]: started with pid [6566]
started core service [/rosout]
process[delta_wing_26/ardupilot_delta_wing_26-2]: started with pid [6582]
temporary rootfs: /tmp/tmp.wVPFoVbw95
Running [[arduplane -S --base-port 14260 --rc-in-port 14261 --gazebo-address 127.0.0.1 --gazebo-port-in 14264 --gazebo-port-out 14265 --home 35.72068,-120.771775,282.0,353.0 --model gazebo-zephyr --defaults /tmp/delta_wing_26_us4X1A -F 127.0.0.1]]
Starting SITL Gazebo
Started model gazebo-zephyr at 35.72068,-120.771775,282.0,353.0 at speed 1.0
Starting sketch 'ArduPlane'
Starting SITL input
bind port 14260 for 0
Serial port 0 on TCP port 14260
Waiting for connection ....
process[delta_wing_26/ardupilot_delta_wing_26_mavproxy-3]: started with pid [6597]
mavproxy.py --mav10 --master tcp:127.0.0.1:14260 --sitl 127.0.0.1:14261 --out udpbcast:127.0.0.255:14000 --out 127.0.0.1:14263 --aircraft delta_wing_26 __name:=ardupilot_delta_wing_26_mavproxy __log:=/home/tfoote/.ros/log/82b6a2d6-c400-11e6-a0c7-3c970ee1fca6/delta_wing_26-ardupilot_delta_wing_26_mavproxy-3.log
Connect tcp:127.0.0.1:14260 source_system=255
Loaded defaults from /tmp/delta_wing_26_us4X1A
bind port 14262 for 2
Serial port 2 on TCP port 14262
bind port 14263 for 3
Serial port 3 on TCP port 14263
Failed to load module: No module named adsb. Use 'set moddebug 3' in the MAVProxy console to enable traceback
no script delta_wing_26/mavinit.scr
Log Directory: delta_wing_26/logs/2016-12-16/flight63
Telemetry log: delta_wing_26/logs/2016-12-16/flight63/flight.tlog
MAV> Waiting for heartbeat from tcp:127.0.0.1:14260
setup:252: bad header in setup - erasing

Init ArduPlane V3.8.0beta1 (eb9d83d0)

Free RAM: 131072
Firmware change: erasing EEPROM...
done.
load_all took 0us
0 0 0 DataFlash_File: buffer size=16384
online system 26
26:INITIALISING> Mode INITIALISING
Freq: (0.000000) Average[max] wait 0.001859[0.002096], acting: 14819417.163154[1481941716.111298] blocking: 1.000000 %
APM: <startup_ground> Ground start
APM: Calibrating barometer
Freq: (175.935201) Average[max] wait 0.001871[0.002119], acting: 0.003812[0.177100] blocking: 0.670769 %
APM: ArduPlane V3.8.0beta1 (eb9d83d0)
Freq: (204.006685) Average[max] wait 0.001859[0.002107], acting: 0.003043[0.100187] blocking: 0.620713 %
Freq: (254.074129) Average[max] wait 0.001890[0.002186], acting: 0.002046[0.100156] blocking: 0.519841 %
Received 762 parameters
Saved 762 parameters to delta_wing_26/logs/2016-12-16/flight63/mav.parm
Freq: (203.831997) Average[max] wait 0.001860[0.002112], acting: 0.003047[0.100177] blocking: 0.620962 %
Freq: (253.974744) Average[max] wait 0.001892[0.002251], acting: 0.002045[0.100139] blocking: 0.519456 %
Freq: (203.935961) Average[max] wait 0.001859[0.002096], acting: 0.003045[0.100160] blocking: 0.620889 %
Freq: (253.751180) Average[max] wait 0.001899[0.002088], acting: 0.002041[0.100178] blocking: 0.518017 %
Freq: (204.010853) Average[max] wait 0.001857[0.002115], acting: 0.003044[0.100185] blocking: 0.621116 %
Freq: (253.939839) Average[max] wait 0.001892[0.002109], acting: 0.002046[0.100178] blocking: 0.519437 %
Freq: (203.837445) Average[max] wait 0.001861[0.002136], acting: 0.003045[0.100176] blocking: 0.620755 %
Freq: (253.981818) Average[max] wait 0.001893[0.002095], acting: 0.002044[0.100171] blocking: 0.519224 %
Freq: (203.930904) Average[max] wait 0.001860[0.002110], acting: 0.003044[0.100178] blocking: 0.620766 %
Freq: (254.041813) Average[max] wait 0.001893[0.002105], acting: 0.002043[0.100183] blocking: 0.518979 %
Freq: (203.947265) Average[max] wait 0.001859[0.002089], acting: 0.003044[0.100167] blocking: 0.620848 %
Freq: (254.247086) Average[max] wait 0.001895[0.002114], acting: 0.002038[0.100162] blocking: 0.518200 %
Freq: (204.073784) Average[max] wait 0.001856[0.002084], acting: 0.003045[0.100153] blocking: 0.621303 %
APM: Barometer calibration complete
APM: No airspeed
APM: Ready to fly
Freq: (253.633180) Average[max] wait 0.001893[0.002102], acting: 0.002049[0.100157] blocking: 0.519826 %
Freq: (203.767729) Average[max] wait 0.001860[0.002111], acting: 0.003048[0.100180] blocking: 0.621074 %
Freq: (254.135399) Average[max] wait 0.001890[0.002095], acting: 0.002045[0.100145] blocking: 0.519786 %
Freq: (203.874402) Average[max] wait 0.001856[0.002087], acting: 0.003048[0.100173] blocking: 0.621513 %
Freq: (254.055354) Average[max] wait 0.001889[0.002168], acting: 0.002047[0.100177] blocking: 0.520183 %
26:MANUAL> Mode MANUAL
Freq: (203.984262) Average[max] wait 0.001853[0.002084], acting: 0.003050[0.100180] blocking: 0.622068 %
Freq: (183.820176) Average[max] wait 0.003391[0.150374], acting: 0.002049[0.100206] blocking: 0.376726 %
APM: GPS 0: detected as u-blox at 38400 baud
Freq: (203.882727) Average[max] wait 0.001858[0.002089], acting: 0.003047[0.100178] blocking: 0.621153 %
GPS lock at 0 meters
Freq: (243.869629) Average[max] wait 0.001853[0.002090], acting: 0.002247[0.100177] blocking: 0.547972 %
Freq: (184.516842) Average[max] wait 0.001751[0.002095], acting: 0.003669[0.100185] blocking: 0.677009 %
Freq: (232.524067) Average[max] wait 0.001820[0.002104], acting: 0.002481[0.100164] blocking: 0.576863 %
Freq: (190.139398) Average[max] wait 0.001787[0.002108], acting: 0.003473[0.100178] blocking: 0.660259 %
APM: Init HOME
APM: GPS alt: 33242
Freq: (224.778748) Average[max] wait 0.001786[0.002084], acting: 0.002662[0.100178] blocking: 0.598435 %
Freq: (189.610770) Average[max] wait 0.001788[0.002108], acting: 0.003486[0.100184] blocking: 0.661006 %
[delta_wing_26/ardupilot_delta_wing_26_mavproxy-3] killing on exit
[delta_wing_26/ardupilot_delta_wing_26-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
DeleteModel: successfully deleted model (delta_wing_26)

[tfoote]

Leaving it running in the good state it even accelerates to 2% of the time blocking (note the markup is wrong it's a raw fraction not a percent.) But we're now turning over at ~500hz which is where gazebo is throttling.

Freq: (499.533017) Average[max] wait 0.001957[0.002113], acting: 0.000045[0.000137] blocking: 0.022438 %
Freq: (499.086621) Average[max] wait 0.001959[0.002107], acting: 0.000045[0.000138] blocking: 0.022306 %
Freq: (499.485427) Average[max] wait 0.001957[0.002091], acting: 0.000044[0.000143] blocking: 0.022220 %
Freq: (499.154332) Average[max] wait 0.001960[0.002104], acting: 0.000044[0.000136] blocking: 0.021923 %
Freq: (499.790161) Average[max] wait 0.001955[0.002090], acting: 0.000046[0.000207] blocking: 0.022842 %
Freq: (500.412685) Average[max] wait 0.001954[0.002058], acting: 0.000044[0.000130] blocking: 0.022104 %
Freq: (498.079670) Average[max] wait 0.001957[0.002170], acting: 0.000050[0.000138] blocking: 0.025108 %
Freq: (499.430709) Average[max] wait 0.001956[0.002105], acting: 0.000046[0.000128] blocking: 0.022845 %
Freq: (501.932552) Average[max] wait 0.001950[0.002101], acting: 0.000042[0.000106] blocking: 0.021314 %
Freq: (498.355451) Average[max] wait 0.001962[0.002106], acting: 0.000045[0.000149] blocking: 0.022254 %
Freq: (498.095049) Average[max] wait 0.001961[0.002109], acting: 0.000047[0.000141] blocking: 0.023166 %
Freq: (499.412869) Average[max] wait 0.001955[0.002161], acting: 0.000047[0.000144] blocking: 0.023525 %
Freq: (499.373626) Average[max] wait 0.001955[0.002080], acting: 0.000048[0.000134] blocking: 0.024070 %
Freq: (499.483048) Average[max] wait 0.001959[0.002095], acting: 0.000043[0.000147] blocking: 0.021529 %

[tfoote]

A potentially related issue is that after 15 mintues of running slowly (cpu time) gazebo got noteably faster and went from 26% CPU to 100% CPU. (trial with 50 drones) And at that point everything got much more responsive.

[tfoote]

Here's some timing: starting the first set of drones slowed it down for ~100 seconds the 2nd set of drones ~ 250 seconds: https://gist.github.com/tfoote/5fd7190d22f80f9af663a43fd7a175ac

[tfoote]

With more debugging I've found that ardupilot is updating about 40 out of 41 times in well under 1 ms. However in that remaining cycle it blocks for 100ms which blocks gazebo for 100ms too.

Here's a full gist: https://gist.github.com/tfoote/8fc39ed2cb07b582ca75a98f3de26141 and the branch with instrumentation: https://github.com/tfoote/ardupilot/tree/instrumenting_sim_gazebo

[tfoote]

We found the slowdown of 100ms every 40 frames: https://github.com/tfoote/ardupilot/commit/0b38166bc7c48c920e572726346bfa882d4ba8ce

After running that we're still seeing generally slow behavior but the large gaps are gone. I'm going to try larger tests.

[tfoote]

There were two fixes. One was to remove the long sleep in ardupilot. tfoote/ardupilot@0b38166 And the second is to enable sim time to keep things synchronized #104

Loading and unloading plugins still slows down the simulation but that's expected.