Merge pull request #12 from AutonomyLab/fix-odom

Fix odometry sign error

CMakeLists.txt

@@ -23,6 +23,7 @@ target_link_libraries(create
 ## Declare example executables
 add_executable(create_demo examples/create_demo.cpp)
 add_executable(bumper_example examples/bumper_example.cpp)
+add_executable(odom_example examples/odom_example.cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(create_demo
@@ -33,6 +34,11 @@ target_link_libraries(bumper_example
   ${Boost_LIBRARIES}
   create
 )
+target_link_libraries(odom_example
+  ${Boost_LIBRARIES}
+  create
+)
+
 ## Install
 install(TARGETS create DESTINATION lib)
 install(FILES

examples/odom_example.cpp

@@ -0,0 +1,50 @@
+#include "create/create.h"
+
+create::Create* robot;
+
+int main(int argc, char** argv) {
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  create::RobotModel model = create::CREATE_2;
+
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::CREATE_1;
+    baud = 57600;
+    std::cout << "1st generation Create selected" << std::endl;
+  }
+
+  robot = new create::Create(model);
+
+  // Attempt to connect to Create
+  if (robot->connect(port, baud))
+    std::cout << "Successfully connected to Create" << std::endl;
+  else {
+    std::cout << "Failed to connect to Create on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  robot->setMode(create::MODE_FULL);
+
+  usleep(1000000);
+
+  // drive in a circle
+  robot->drive(0.1, 0.5);
+
+  // Quit when center "Clean" button pressed
+  while (!robot->isCleanButtonPressed()) {
+    create::Pose pose = robot->getPose();
+
+    std::cout << "x: " << pose.x
+              << "\ty: " << pose.y
+              << "\tyaw: " << pose.yaw * 180.0/create::util::PI << std::endl;
+  
+    usleep(1000 * 100); //10hz
+  }
+
+  std::cout << "Stopping Create." << std::endl;
+
+  robot->disconnect();
+  delete robot;
+
+  return 0;
+}

src/create.cpp

@@ -66,10 +66,15 @@ namespace create {
     float dt = (curTime - prevOnDataTime) / 1000000.0;
     float deltaDist, deltaX, deltaY, deltaYaw;
     if (model == CREATE_1) {
+      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+       * Angle returned is NOT correct if your robot is using older firmware:      *
+       * http://answers.ros.org/question/31935/createroomba-odometry/              *
+       * TODO: Consider using velocity command as substitute for pose estimation.  *
+       * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
       deltaDist = ((int16_t) GET_DATA(ID_DISTANCE)) / 1000.0; //mm -> m
       deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R
       deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) );
-      deltaY = deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
+      deltaY = -deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
     }
     else if (model == CREATE_2) {
       // Get cumulative ticks (wraps around at 65535)
@@ -103,13 +108,12 @@ namespace create {
         deltaX = deltaDist * cos(pose.yaw);
         deltaY = deltaDist * sin(pose.yaw);
         deltaYaw = 0.0;
-        //vel.yaw = 0;
       }
       else {
         float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
-        deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH;
+        deltaYaw = wheelDistDiff / util::CREATE_2_AXLE_LENGTH;
         deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
-        deltaY = turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
+        deltaY = -turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
       }
     } // if CREATE_2