Validate the choice of the sampling rate for all sensors

[sbrodeur]

Perform an integrated test on the robot to see how fast can we sample the sensors (e.g. IMU, Create) without saturating the CPU on the Beaglebone Black.

[sbrodeur]

For instance, this is a 'top' output using current configuration during recording:

top - 15:29:58 up  1:56,  3 users,  load average: 6.39, 3.54, 2.32
Tasks: 108 total,   3 running, 105 sleeping,   0 stopped,   0 zombie
%Cpu(s): 45.0 us, 39.5 sy,  0.0 ni,  0.0 id,  0.0 wa,  0.0 hi, 15.5 si,  0.0 st
KiB Mem:    507508 total,   419752 used,    87756 free,    47348 buffers
KiB Swap:   262140 total,        0 used,   262140 free.   199464 cached Mem

  PID USER      PR  NI    VIRT    RES    SHR S %CPU %MEM     TIME+ COMMAND                                                                                    
 9114 root      20   0   80892   8644   7832 R 14.3  1.7   0:46.49 create_driver                                                                              
 9620 root      20   0  187308  24032   7588 S 14.3  4.7   0:09.74 record                                                                                     
 9179 root      20   0   78084  10736   9648 R 12.1  2.1   0:45.91 imu_filter_node                                                                            
 9135 root      20   0   72740   8856   8064 S 10.0  1.7   0:32.02 create_odometry                                                                            
 9158 root      20   0   71108   7364   6676 S  5.5  1.5   0:15.30 imu_capture_mag                                                                            
 9150 root      20   0   71108   7372   6684 S  5.2  1.5   0:19.14 imu_capture_acc                                                                            
 9116 root      20   0  209260  45232  37684 S  3.3  8.9   0:12.55 camera_capture                                                                             
  731 root      20   0    4780   2876   2564 S  3.0  0.6   1:19.06 hostapd                                                                                    
 6112 root      20   0       0      0      0 S  3.0  0.0   0:08.03 kworker/u2:2                                                                               
 9169 root      20   0   71124   7432   6736 D  3.0  1.5   0:05.97 imu_capture_bar                                                                            
   61 root     -51   0       0      0      0 S  2.7  0.0   0:49.02 irq/158-4819c00                                                                            
 1874 root      20   0    9268   3892   3352 S  2.7  0.8   0:04.22 sshd                                                                                       
 9097 root      20   0   62964   7452   6716 S  2.7  1.5   0:03.32 rosout                                                                                     
 9115 root      20   0  209296  45836  38316 S  2.7  9.0   0:11.30 camera_capture                                                                             
 8995 root      20   0       0      0      0 S  2.4  0.0   0:03.47 kworker/u2:1            

[sbrodeur]

We may think of creating custom batch messages (similar to audio) for storing more than one sample for the IMU sensor, given that they are sample frequently (e.g. 100 Hz) and sending a single message through a publisher is consuming on the Beaglebone Black. We can still publish the single-sample message once in a while (e.g. 10 Hz), so that the orientation can still be calculated. The madgwick filter also takes significant resources, so it could be applied offline on the rosbag. The batch messages could also be split into individual standard messages during that offline processing.

[sbrodeur]

See commit b56c685 for batched messages, which solves the high CPU load issues when recording on the robot. Now what remains to be done:

• Create a script to unpack the batched messages from a recorded rosbag and write the content (including all other messages) to a new rosbag.
• Create a script to apply the orientation (madgwick filter) and odometry estimation from that rosbag and write the content (including all other messages) to a new rosbag. This requires making the odometry and madgwick packages compatible with this off-line mode, which should not be difficult (see http://wiki.ros.org/rosbag/Cookbook).

[sgcarrier]

Added unbatching to convert_rosbag.py in commit a861aaabbfc2b0ac496ecc9425911be6c1e5a856

[sbrodeur]

See commit 445adc2 for offline rosbag processing with the IMU madgwick filter. More options (e.g. like publishing /tf) should be added soon.

Usage:

rosrun imu_filter_madgwick imu_filter_rosbag --input=in.bag --output=out.bag

[sbrodeur]

See commit 0474740 for offline rosbag processing with the odometry estimator. Also added tf publishing support for this script and for the IMU madgwick filter.

Usage:

rosrun create create_odometry_rosbag --input=in.bag --output=out.bag --publish-tf

I could not test offline odometry since I dont have rosbag recordings new enough to have the topic /irobot_create/joints exposing JointState messages.

[sbrodeur]

See commit 3e4a6ed for modification of the convert_rosbag.py script, to fix some issues in the unbatched messages.

[sgcarrier]

Here are the latest stats on my last test run. We see that Baro seems to be limited to around 48hz and has a drop rate of 0.20% (ive been seeing this same % for a while now). And Gyro and mag seem limited to 92hz in batch mode (note that they have a 0% dropped rate though) . Aside from that, everything looks consistent. testrun_stats.txt

[sgcarrier]

Here are the stats for when we initiate recording on the robot and control via laptop (with video streaming of one camera to the laptop). This way the cpu isnt saturated (~5% idle). /Imu/Temp seems to have a high drop rate at first, but since it has very few messages, packets losses when recording starts has a high impact. robotRecord_remoteControl.txt

[sbrodeur]

This is the final configuration:

Sensors	Sampling rate (Hz)
iRobot Create	50 Hz
Camera	30 Hz
IMU baro	50 Hz
IMU temp	1 Hz
IMU gyro	90 Hz (frame size = 16)
IMU acc	90 Hz (frame size = 16)
IMU mag	90 Hz (frame size = 16)
Audio	16000 Hz (frame size = 1024)
Joystick	50 Hz

laptop: [nothing] roomba : fresh reboot, no joystick node %Cpu(s): 20.6 us, 12.6 sy, 0.0 ni, 66.8 id, 0.0 wa, 0.0 hi, 0.0 si, 0.0 st

laptop: stream+joysticknode roomba : normal %Cpu(s): 15.6 us, 16.0 sy, 0.0 ni, 60.1 id, 0.0 wa, 0.0 hi, 8.2 si, 0.0 st

laptop: stream+record+controls %Cpu(s): 22.8 us, 43.9 sy, 0.0 ni, 9.8 id, 0.0 wa, 0.0 hi, 23.5 si, 0.0 st

laptop: stream+controls roomba: record %Cpu(s): 51.6 us, 38.5 sy, 0.0 ni, 0.0 id, 0.6 wa, 0.0 hi, 9.3 si, 0.0 st

Note: An odd behaviour ive noticed is that sometimes si% will go very high while us% and sy% go low. The id% tends to be the most reliable to use. Although if we wanted to be more rigorous, we would sample usage over periods of time.

[sbrodeur]

The table and statistics above reported by @sgcarrier confirm that everything runs smoothly, and that we squeeze out the the performance that the Beaglebone Black can offer us.