Merge pull request #4 from SwarmUS/fix/SWARINFO-697/odom_velocities

Fix velocities calculated in the odom topic

src/diffdrive_roscore.cpp

@@ -184,12 +184,12 @@ namespace roboclaw {
             delta_2 = tmp;
         }
 
-        double u_w = ((delta_1 + delta_2) / steps_per_meter) / 2.0;
-        double u_p = ((delta_2 - delta_1) / steps_per_meter);
+        double translation_displacement = ((delta_1 + delta_2) / steps_per_meter) / 2.0;
+        double angular_displacement = ((delta_2 - delta_1) / steps_per_meter);
 
-        double delta_x = u_w * cos(last_theta);
-        double delta_y = u_w * sin(last_theta);
-        double delta_theta = u_p / base_width;
+        double delta_x = translation_displacement * cos(last_theta);
+        double delta_y = translation_displacement * sin(last_theta);
+        double delta_theta = angular_displacement / base_width;
 
         double cur_x = last_x + delta_x;
         double cur_y = last_y + delta_y;
@@ -211,14 +211,14 @@ namespace roboclaw {
 
         last_time = current_time;
 
-        // Position
+        // Position in the odom frame
         odom.pose.pose.position.x = cur_x;
         odom.pose.pose.position.y = cur_y;
 
-        // Velocity
-        odom.twist.twist.linear.x = (cur_x - last_x)/dt;
-        odom.twist.twist.linear.y = (cur_y - last_y)/dt;
-        odom.twist.twist.angular.z = (cur_theta - last_theta)/dt;
+        // Velocity in the base_footprint frame
+        odom.twist.twist.linear.x = translation_displacement/dt;
+        odom.twist.twist.linear.y = 0;
+        odom.twist.twist.angular.z = delta_theta/dt;
 
         tf::Quaternion quaternion = tf::createQuaternionFromRPY(0.0, 0.0, cur_theta);
         odom.pose.pose.orientation.w = quaternion.w();