Velocity Measurements for Navigation

References

• Navigation Tuning Guide
• ROS Navigation Tuning Guide by Kaiyu Zheng student at University of Washington.

Expected Max Velocity

Max velocity should be 283 mm/rev * 80 rev/min / 60 sec/min / 1000 mm/m => 0.38 m/sec.

See Calibrate_Phidgets_Odometry#Max_Velocity

Linear Measured Velocity

Run Setup

In separate terminals

roscore
roslaunch floor_hugger odometry.launch
roslaunch floor_hugger motor_control_hack.launch
rostopic echo odom > velocity.log
. motor_test.sh        in directory .../floor_hugger/tests

velocity log is in directory .../floor_hugger/nav

Reading Odom Message

 header: 
   seq: 5541
   stamp: 
     secs: 1479327325
     nsecs:  52354329
   frame_id: odom
 child_frame_id: base_link
 pose: 
   pose: 
     position: 
       x: 0.371328110516
       y: -0.0627744986145
       z: 0.0
     orientation: 
       x: 0.0
       y: 0.0
       z: -0.0798948866666
       w: 0.996803294078
   covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 twist: 
   twist: 
     linear: 
       x: 0.339330900467
       y: -0.0545742572285
       z: 0.0
     angular: 
       x: 0.0
       y: 0.0
       z: -0.0219280328565
   covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

• Message sequence: 5541
• Time stamp is 1479327325.524 seconds
• Current x position (pose/pose/position/x) is 0.371 meters
• Velocity in x direction is (twist/twist/linear/x) is 0.339 meters / second

The instanious velocity is reasonably close to the expected

• Message sequence: 5583 - this is 42 measurements later
• Time stamp is ...326.962 seconds
• Current x position is 1.092 meters
• Velocity in x direction is 0.347 meters / second

Computer Average Velocity

The average speed computation

• Distance travelled is 1.092 meters - 0.371 meters equals 0.721 meters
• Time is ...326.962 seconds - 325.005 seconds equals 1.957 seconds
• Velocity is 0.721 meters / 1.957 seconds equals 0.369 meters / second

The average velocity of 0.37 m/s is close to expected of 0.38.

Instantaneous Velocity Plot

 rqt_plot /odom/twist/twist/linear/x

Linear Measured Acceleration

 x = (1/2) at^2
                   or 
 a = (2x) / (t^2)

The velocity measurement in sequence 5535 is 0. The next sequence has a positive velocity. Velocity stabilizes in sequence 5541 (see its values above).

• Message sequence: 5535
• Time stamp is ...324.780
• Current x position is 0.324 meters
• Velocity is x direction is 0.0 meters / second

Delta between sequence 5535 and 5541

• Distance is 0.371 - 0.324 is 0.047 meters
• Time is ...325.005 - 324.780 is 0.225 seconds
• Acceleration is (2 * 0.047) / (2 * 0.225 ^ 2) is 1.85 m/(s^2)

Additional computations are need to confirm.

Angular Measured Velocity

Run Setup

Remote logged in from iMac by running xQuartz.

ssh -X eepp@10.0.0.111

In separate terminals created by xterm&

roscore
roslaunch floor_hugger odometry.launch
roslaunch floor_hugger motor_control_hack.launch
rostopic echo odom > velocity02.log
rostopic pub -r 10  cmd_vel geometry_msgs/Twist '[0,0,0]' '[0,0,5]' 

Angular Velocity from Odom Message

Odom sequence 6460 had a angular stability. See twist/twist/angular/z for instantaneous angular velocity.

 header: 
   seq: 6460
   stamp: 
     secs: 1497641277
     nsecs: 956016111
   frame_id: odom
 child_frame_id: base_link
 pose: 
   pose: 
     position: 
       x: 0.0326110256215
       y: -0.0909931564523
       z: 0.0
     orientation: 
       x: -0.0
       y: 0.0
       z: 0.56163360369
       w: -0.827386061767
   covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 twist: 
   twist: 
     linear: 
       x: 0.00107144974025
       y: -0.00315760619871
       z: 0.0
     angular: 
       x: 0.0
       y: 0.0
       z: 2.24336518075
   covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Averaged readings from seq 6460 through 6469 (2.24, 2.69, 2.72, 2.25, 2.68, 2.73, 2.25, 2.72, 2.69, 2.25) is 2.52 radians/second.

Sanity Check

Timed 5 rotations with a stop watch.

• Results of 3 measurements is 12.68, 12.11, 12.28. Average is 12.36 seconds.
• Each measurement is 10 pi radians or 31.42.
• 31.41 radians / 12.36 seconds is 2.54 radians / second. This is within error of this measurement.

Angular Measured Acceleration

Using test data from Angular Measured Velocity.

• Sequence 6442
  • Time stamp is 1497641277 seconds, 137788131 nano seconds
  • Angular velocity 0.01268 radians / second
• Sequence 6448
  • Time stamp is 1497641277 seconds, 410549947 nano seconds
  • Angular velocity is 2.27775 radians / second
• Difference
  • 0.279 seconds
  • 2.27 radians / second

From Linear Measured Acceleration above

 a = (2x) / (t^2)
 a = (2 * 2.27) / 0.279^2 = 58.3 radians / second^2