Code cleanup

CMakeLists.txt

@@ -22,6 +22,7 @@ target_link_libraries(create
 
 ## Declare example executables
 add_executable(create_demo examples/create_demo.cpp)
+add_executable(odom_test examples/odom_test.cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(create_demo
@@ -29,6 +30,11 @@ target_link_libraries(create_demo
   create
 )
 
+target_link_libraries(odom_test
+  ${Boost_LIBRARIES}
+  create
+)
+
 ## Install
 ## I'm not familiar with install rules, do these make sense?
 install(TARGETS create DESTINATION lib)

LICENSE

@@ -14,9 +14,9 @@ modification, are permitted provided that the following conditions are met:
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

examples/create_demo.cpp

@@ -20,7 +20,7 @@ int main(int argc, char** argv) {
 
   std::cout << "battery level: " <<
     robot->getBatteryCharge() / (float) robot->getBatteryCapacity() * 100.0 << "%" << std::endl;
-  
+
   bool drive = false;
 
   // Make a song
@@ -29,8 +29,8 @@ int main(int argc, char** argv) {
   //                      67, 67, 66, 66, 65, 65, 66, 66 };
   //float durations[songLength];
   //for (int i = 0; i < songLength; i++) {
-  //  durations[i] = 0.25;   
+  //  durations[i] = 0.25;
-  //} 
+  //}
   //robot->createSong(0, songLength, notes, durations);
   //usleep(1000000);
   //robot->playSong(0);
@@ -38,9 +38,9 @@ int main(int argc, char** argv) {
   // Quit when center "Clean" button pressed
   while (!robot->isCleanButtonPressed()) {
     // Check for button presses
-    if (robot->isDayButtonPressed()) 
+    if (robot->isDayButtonPressed())
       std::cout << "day button press" << std::endl;
-    if (robot->isMinButtonPressed()) 
+    if (robot->isMinButtonPressed())
       std::cout << "min button press" << std::endl;
     if (robot->isDockButtonPressed()) {
       std::cout << "dock button press" << std::endl;
@@ -64,7 +64,7 @@ int main(int argc, char** argv) {
     // If everything is ok, drive forward using IR's to avoid obstacles
     if (drive) {
       robot->setPowerLED(0); // green
-      if (robot->isIRDetectLeft() || 
+      if (robot->isIRDetectLeft() ||
           robot->isIRDetectFrontLeft() ||
           robot->isIRDetectCenterLeft()) {
         // turn right
@@ -97,7 +97,7 @@ int main(int argc, char** argv) {
     else {
       robot->enableDebrisLED(false);
     }
-    
+
     // Check bumpers
     if (robot->isLeftBumper()) {
       std::cout << "left bump detected!" << std::endl;
@@ -110,13 +110,13 @@ int main(int argc, char** argv) {
   }
 
   std::cout << "Stopping Create." << std::endl;
- 
+
-  // Turn off lights 
+  // Turn off lights
   robot->setPowerLED(0, 0);
   robot->enableDebrisLED(false);
   robot->enableCheckRobotLED(false);
   robot->setDigitsASCII(' ', ' ', ' ', ' ');
-  
+
   // Make sure to disconnect to clean up
   robot->disconnect();
   delete robot;

include/create/create.h

@@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met:
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
@@ -65,7 +65,7 @@ namespace create {
 
       create::Pose pose;
       create::Vel vel;
-      
+
       uint32_t prevTicksLeft;
       uint32_t prevTicksRight;
       float prevLeftVel;
@@ -108,8 +108,10 @@ namespace create {
       /* Resets as if you have removed the battery.
        * Changes mode to MODE_PASSIVE.
        */
+      // TODO
       //void reset();
 
+      // TODO
       //void setBaud(int baudcode);
 
       /* Change Create mode.
@@ -145,6 +147,7 @@ namespace create {
 
       /* Set the PWM for each wheel.
        */
+      // TODO
       //void drivePWM(const int16_t& leftWheel, const int16_t& rightWheel) const;
 
       /* Set the forward and angular velocity of Create.
@@ -206,6 +209,7 @@ namespace create {
       //void setDigits(uint8_t digit1, uint8_t digit2,
       //               uint8_t digit3, uint8_t digit4);
 
+      // TODO
       // pushButton(...);
 
       /* Set the four 7-segment display digits from left to right with ASCII codes.
@@ -236,6 +240,7 @@ namespace create {
        */
       bool playSong(const uint8_t& songNumber) const;
 
+      // TODO
       //void registerCallback(...);
 
       /* True if a left or right wheeldrop is detected.

include/create/data.h

@@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met:
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

include/create/packet.h

@@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met:
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
@@ -39,20 +39,19 @@ namespace create {
       uint16_t data;
       uint16_t tmpData;
       mutable boost::mutex dataMutex;
+      mutable boost::mutex tmpDataMutex;
 
     public:
-      // TODO: Do they really need to be const? then they better be static
-      // I am actually not sure if const member vars are valid
       const uint8_t nbytes;
       const std::string info;
 
       Packet(const uint8_t& nbytes, const std::string& info);
       ~Packet();
+
+      // All of the following are thread safe
       void setTempData(const uint16_t& td);
       void validate();
-      // thread safe
       void setData(const uint16_t& d);
-      // thread safe
       uint16_t getData() const;
   };
 

include/create/serial.h

@@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met:
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
@@ -67,8 +67,7 @@ namespace create {
       bool isReading;
       bool firstRead;
 
-      // These are just for diagnostics, maybe not necessary
+      // These are for possible diagnostics
-      // TODO: Investigate
       uint64_t corruptPackets;
       uint64_t totalPackets;
       // State machine variables
@@ -83,8 +82,6 @@ namespace create {
       uint8_t expectedNumDataBytes;
 
       // Callback executed when data arrives from Create
-      // TODO: Should size be const?
-      // Not sure, this was from example
       void onData(const boost::system::error_code& e, const std::size_t& size);
       // Callback to execute once data arrives
       boost::function<void()> callback;

include/create/types.h

@@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met:
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

include/create/util.h

@@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met:
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
@@ -49,7 +49,7 @@ namespace create {
     static const float PI = 3.14159;
     static const float TWO_PI = 6.28318;
     static const float EPS = 0.0001;
-  
+
     inline float normalizeAngle(const float& angle) {
       float a = angle;
       while (a < -PI) a += TWO_PI;
@@ -57,9 +57,10 @@ namespace create {
       return a;
     };
 
+    typedef unsigned long long timestamp_t;
+
     /** Get a timestamp for the current time in micro-seconds.
      */
-    typedef unsigned long long timestamp_t;
     static timestamp_t getTimestamp() {
       struct timeval now;
       gettimeofday(&now, NULL);

src/create.cpp

@@ -1,7 +1,7 @@
 #include <boost/bind.hpp>
 #include <boost/make_shared.hpp>
 #include <iostream>
-#include <cmath> 
+#include <cmath>
 #include <ctime>
 #include <assert.h>
 
@@ -45,11 +45,11 @@ namespace create {
     init();
     serial->connect(dev, baud);
   }
-  
+
   Create::~Create() {
     disconnect();
   }
-  
+
   void Create::onData() {
     if (firstOnData) {
       // Initialize tick counts
@@ -88,7 +88,7 @@ namespace create {
 
     float wheelDistDiff = rightWheelDist - leftWheelDist;
     float deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
-    
+
     // Moving straight
     float deltaX, deltaY;
     if (fabs(wheelDistDiff) < util::EPS) {
@@ -98,7 +98,7 @@ namespace create {
     }
     else {
       float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
-      float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH; 
+      float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH;
       deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
       deltaY = turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
       pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
@@ -109,7 +109,7 @@ namespace create {
         vel.yaw = 0;
       }
     }
-  
+
     if (fabs(dt) > util::EPS) {
       vel.x = deltaX / dt;
       vel.y = deltaY / dt;
@@ -121,12 +121,12 @@ namespace create {
 
     pose.x += deltaDist * cos(pose.yaw);
     pose.y += deltaDist * sin(pose.yaw);
-   
+
     prevOnDataTime = curTime;
     // Make user registered callbacks, if any
     // TODO
   }
-  
+
   bool Create::connect(const std::string& port, const int& baud) {
     bool timeout = false;
     time_t start, now;
@@ -144,33 +144,33 @@ namespace create {
         COUT("[create::Create] ", "retrying to establish serial connection...");
       }
     }
-  
+
     return !timeout;
   }
-  
+
   void Create::disconnect() {
     serial->disconnect();
     firstOnData = true;
   }
-  
+
   //void Create::reset() {
   //  serial->sendOpcode(OC_RESET);
   //  serial->reset(); // better
     // TODO : Should we request reading packets again?
   //}
-  
+
   bool Create::setMode(const CreateMode& mode) {
     return serial->sendOpcode((Opcode) mode);
   }
-  
+
   bool Create::clean(const CleanMode& mode) {
     return serial->sendOpcode((Opcode) mode);
   }
-  
+
   bool Create::dock() const {
     return serial->sendOpcode(OC_DOCK);
   }
-  
+
   bool Create::setDate(const DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const {
     if (day < 0 || day > 6 ||
         hour < 0 || hour > 23 ||
@@ -180,7 +180,7 @@ namespace create {
     uint8_t cmd[4] = { OC_DATE, day, hour, min };
     return serial->send(cmd, 4);
   }
-  
+
   /*void Create::driveRadius(const float& vel, const float& radius) const {
     // Expects each parameter as two bytes each and in millimeters
     int16_t vel_mm = roundf(vel * 1000);
@@ -199,7 +199,7 @@ namespace create {
                        radius_mm >> 8,
                        radius_mm & 0xff
                      };
-  
+
     serial->send(cmd, 5);
   }
   */
@@ -218,7 +218,7 @@ namespace create {
                      };
     return serial->send(cmd, 5);
   }
-  
+
   /*void Create::drivePWM(const int16_t& leftPWM, const int16_t& rightPWM) const {
     uint8_t cmd[5] = { OC_DRIVE_PWM,
                        rightPWM >> 8,
@@ -229,45 +229,45 @@ namespace create {
     serial->send(cmd, 5);
   }
   */
-  
+
   bool Create::drive(const float& xVel, const float& angularVel) const {
     // Compute left and right wheel velocities
     float leftVel = xVel - ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
     float rightVel = xVel + ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
     return driveWheels(leftVel, rightVel);
   }
-  
+
   bool Create::setAllMotors(const float& main, const float& side, const float& vacuum) {
     if (main < -1.0 || main > 1.0 ||
         side < -1.0 || side > 1.0 ||
         vacuum < -1.0 || vacuum > 1.0)
-      return false;    
+      return false;
 
     mainMotorPower = roundf(main * 127);
     sideMotorPower = roundf(side * 127);
     vacuumMotorPower = roundf(vacuum * 127);
-  
+
     uint8_t cmd[4] = { OC_MOTORS_PWM,
                         mainMotorPower,
                         sideMotorPower,
                         vacuumMotorPower
                       };
-  
+
     return serial->send(cmd, 4);
   }
-  
+
   bool Create::setMainMotor(const float& main) {
     return setAllMotors(main, sideMotorPower, vacuumMotorPower);
   }
-  
+
   bool Create::setSideMotor(const float& side) {
     return setAllMotors(mainMotorPower, side, vacuumMotorPower);
   }
-  
+
   bool Create::setVacuumMotor(const float& vacuum) {
     return setAllMotors(mainMotorPower, sideMotorPower, vacuum);
   }
-  
+
   bool Create::updateLEDs() {
     uint8_t LEDByte = debrisLED + spotLED + dockLED + checkLED;
     uint8_t cmd[4] = { OC_LEDS,
@@ -275,10 +275,10 @@ namespace create {
                         powerLED,
                         powerLEDIntensity
                       };
-  
+
     return serial->send(cmd, 4);
   }
-  
+
   bool Create::enableDebrisLED(const bool& enable) {
     if (enable)
       debrisLED = LED_DEBRIS;
@@ -286,7 +286,7 @@ namespace create {
       debrisLED = 0;
     return updateLEDs();
   }
-  
+
   bool Create::enableSpotLED(const bool& enable) {
     if (enable)
       spotLED = LED_SPOT;
@@ -294,7 +294,7 @@ namespace create {
       spotLED = 0;
     return updateLEDs();
   }
-  
+
   bool Create::enableDockLED(const bool& enable) {
     if (enable)
       dockLED = LED_DOCK;
@@ -302,7 +302,7 @@ namespace create {
       dockLED = 0;
     return updateLEDs();
   }
-  
+
   bool Create::enableCheckRobotLED(const bool& enable) {
     if (enable)
       checkLED = LED_CHECK;
@@ -310,22 +310,22 @@ namespace create {
       checkLED = 0;
     return updateLEDs();
   }
-  
+
   bool Create::setPowerLED(const uint8_t& power, const uint8_t& intensity) {
     powerLED = power;
     powerLEDIntensity = intensity;
     return updateLEDs();
   }
-  
+
   //void Create::setDigits(uint8_t digit1, uint8_t digit2,
   //                       uint8_t digit3, uint8_t digit4) {
   //}
-  
+
   bool Create::setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
                               const uint8_t& digit3, const uint8_t& digit4) const {
     if (digit1 < 32 || digit1 > 126 ||
-        digit2 < 32 || digit2 > 126 ||  
+        digit2 < 32 || digit2 > 126 ||
-        digit3 < 32 || digit3 > 126 ||  
+        digit3 < 32 || digit3 > 126 ||
         digit4 < 32 || digit4 > 126)
       return false;
 
@@ -335,10 +335,10 @@ namespace create {
                         digit3,
                         digit4
                       };
-  
+
     return serial->send(cmd, 5);
   }
-  
+
   bool Create::defineSong(const uint8_t& songNumber,
                           const uint8_t& songLength,
                           const uint8_t* notes,
@@ -358,44 +358,44 @@ namespace create {
       cmd[i + 1] = duration;
       j++;
     }
-  
+
     return serial->send(cmd, 2 * songLength + 3);
   }
-   
+
   bool Create::playSong(const uint8_t& songNumber) const {
     if (songNumber < 0 || songNumber > 4)
       return false;
     uint8_t cmd[2] = { OC_PLAY, songNumber };
     return serial->send(cmd, 2);
   }
-  
+
   bool Create::isWheeldrop() const {
     return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;
   }
-  
+
   bool Create::isLeftBumper() const {
     return (GET_DATA(ID_BUMP_WHEELDROP) & 0x02) != 0;
   }
-  
+
   bool Create::isRightBumper() const {
     return (GET_DATA(ID_BUMP_WHEELDROP) & 0x01) != 0;
   }
-  
+
   bool Create::isWall() const {
     return GET_DATA(ID_WALL) == 1;
   }
-  
+
   bool Create::isCliff() const {
-    return GET_DATA(ID_CLIFF_LEFT) == 1 || 
+    return GET_DATA(ID_CLIFF_LEFT) == 1 ||
            GET_DATA(ID_CLIFF_FRONT_LEFT) == 1 ||
            GET_DATA(ID_CLIFF_FRONT_RIGHT) == 1 ||
            GET_DATA(ID_CLIFF_RIGHT) == 1;
   }
-  
+
   uint8_t Create::getDirtDetect() const {
     return GET_DATA(ID_DIRT_DETECT);
   }
-  
+
   uint8_t Create::getIROmni() const {
     return GET_DATA(ID_IR_OMNI);
   }
@@ -407,7 +407,7 @@ namespace create {
   uint8_t Create::getIRRight() const {
     return GET_DATA(ID_IR_RIGHT);
   }
-  
+
   ChargingState Create::getChargingState() const {
     uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
     assert(chargeState >= 0);
@@ -418,13 +418,13 @@ namespace create {
   bool Create::isCleanButtonPressed() const {
     return (GET_DATA(ID_BUTTONS) & 0x01) != 0;
   }
-  
+
-  // Not working. TODO Fix/report
+  // Not working
   bool Create::isClockButtonPressed() const {
     return (GET_DATA(ID_BUTTONS) & 0x80) != 0;
   }
 
-  // Not working. TODO Fix/report
+  // Not working
   bool Create::isScheduleButtonPressed() const {
     return (GET_DATA(ID_BUTTONS) & 0x40) != 0;
   }
@@ -448,55 +448,55 @@ namespace create {
   bool Create::isSpotButtonPressed() const {
     return (GET_DATA(ID_BUTTONS) & 0x02) != 0;
   }
-  
+
   uint16_t Create::getVoltage() const {
     return GET_DATA(ID_VOLTAGE);
   }
-  
+
   uint16_t Create::getCurrent() const {
     return GET_DATA(ID_CURRENT);
   }
-  
+
   uint8_t Create::getTemperature() const {
     return GET_DATA(ID_TEMP);
   }
-  
+
   uint16_t Create::getBatteryCharge() const {
     return GET_DATA(ID_CHARGE);
   }
-  
+
   uint16_t Create::getBatteryCapacity() const {
     return GET_DATA(ID_CAPACITY);
   }
-  
+
   bool Create::isIRDetectLeft() const {
     return (GET_DATA(ID_LIGHT) & 0x01) != 0;
   }
- 
+
-  bool Create::isIRDetectFrontLeft() const { 
+  bool Create::isIRDetectFrontLeft() const {
     return (GET_DATA(ID_LIGHT) & 0x02) != 0;
   }
 
-  bool Create::isIRDetectCenterLeft() const { 
+  bool Create::isIRDetectCenterLeft() const {
     return (GET_DATA(ID_LIGHT) & 0x04) != 0;
   }
 
-  bool Create::isIRDetectCenterRight() const { 
+  bool Create::isIRDetectCenterRight() const {
     return (GET_DATA(ID_LIGHT) & 0x08) != 0;
   }
 
-  bool Create::isIRDetectFrontRight() const { 
+  bool Create::isIRDetectFrontRight() const {
     return (GET_DATA(ID_LIGHT) & 0x10) != 0;
   }
 
-  bool Create::isIRDetectRight() const { 
+  bool Create::isIRDetectRight() const {
     return (GET_DATA(ID_LIGHT) & 0x20) != 0;
   }
 
   bool Create::isMovingForward() const {
     return GET_DATA(ID_STASIS) == 1;
   }
-  
+
   const Pose& Create::getPose() const {
     return pose;
   }

src/data.cpp

@@ -3,10 +3,10 @@
 #define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared<Packet>(nbytes,info))
 
 namespace create {
-  
+
   Data::Data() {
     // Populate data map
-    ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops"); 
+    ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops");
     ADD_PACKET(ID_WALL, 1, "wall");
     ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left");
     ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left");
@@ -58,7 +58,7 @@ namespace create {
     ADD_PACKET(ID_MAIN_BRUSH_CURRENT, 2, "main_brush_current");
     ADD_PACKET(ID_SIDE_BRUSH_CURRENT, 2, "side_brush_current");
     ADD_PACKET(ID_STASIS, 1, "stasis");
-    
+
     totalDataBytes = 0;
     for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
          it != packets.end();
@@ -67,23 +67,23 @@ namespace create {
       totalDataBytes += it->second->nbytes;
     }
   }
-  
+
   Data::~Data() { }
-  
+
   bool Data::isValidPacketID(uint8_t id) const {
     if (packets.count(id)) {
       return true;
     }
     return false;
   }
-  
+
   boost::shared_ptr<Packet> Data::getPacket(uint8_t id) {
     if (isValidPacketID(id)) {
       return packets[id];
     }
-    return boost::shared_ptr<Packet>(); //NULL;
+    return boost::shared_ptr<Packet>();
   }
-  
+
   void Data::validateAll() {
     for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
          it != packets.end();
@@ -91,15 +91,15 @@ namespace create {
       it->second->validate();
     }
   }
-  
+
   unsigned int Data::getTotalDataBytes() const {
     return totalDataBytes;
   }
-  
+
   uint8_t Data::getNumPackets() const {
     return packets.size();
   }
-  
+
   std::vector<uint8_t> Data::getPacketIDs() {
     return ids;
   }

src/packet.cpp

@@ -2,28 +2,29 @@
 
 namespace create {
 
-  Packet::Packet(const uint8_t& numBytes, const std::string& comment) : 
+  Packet::Packet(const uint8_t& numBytes, const std::string& comment) :
     nbytes(numBytes),
     info(comment),
     data(0),
     tmpData(0) { }
-  
+
   Packet::~Packet() { }
-  
+
   void Packet::setTempData(const uint16_t& tmp) {
-    // mutex for tmpData ?
+    boost::mutex::scoped_lock lock(tmpDataMutex);
     tmpData = tmp;
   }
-  
+
   void Packet::validate() {
+    boost::mutex::scoped_lock lock(tmpDataMutex);
     setData(tmpData);
   }
-  
+
   void Packet::setData(const uint16_t& d) {
     boost::mutex::scoped_lock lock(dataMutex);
     data = d;
   }
-  
+
   uint16_t Packet::getData() const {
     boost::mutex::scoped_lock lock(dataMutex);
     return data;

src/serial.cpp

@@ -14,16 +14,13 @@ namespace create {
     dataReady(false),
     corruptPackets(0),
     totalPackets(0) {
-    //std::cout << "# Serial Created" << std::endl;
   }
 
   Serial::~Serial() {
     disconnect();
-    //std::cout << "# Serial Destroyed" << std::endl;
   }
 
   bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
-    //std::cout << "## Serial connect start" << std::endl;
     using namespace boost::asio;
     port.open(portName);
     port.set_option(serial_port::baud_rate(baud));
@@ -32,12 +29,11 @@ namespace create {
     if (port.is_open()) {
       callback = cb;
       bool startReadSuccess = startReading();
-      if (!startReadSuccess)
+      if (!startReadSuccess) {
         port.close();
-      //std::cout << "## Serial connect done" << std::endl;
+      }
       return startReadSuccess;
     }
-    //std::cout << "## Serial connect failed" << std::endl;
     return false;
   }
 
@@ -47,13 +43,11 @@ namespace create {
     }
 
     if (connected()) {
-      //std::cout << "## Serial disconnect start" << std::endl;
       // Ensure not in Safe/Full modes
       sendOpcode(OC_START);
       // Stop OI
       sendOpcode(OC_STOP);
       port.close();
-      //std::cout << "## Serial disconnect done" << std::endl;
     }
   }
 
@@ -68,7 +62,6 @@ namespace create {
     // Only allow once
     if (isReading) return true;
 
-    //std::cout << "### Serial start reading" << std::endl;
     // Request from Create that we want a stream containing all packets
     uint8_t numPackets = data->getNumPackets();
     std::vector<uint8_t> packetIDs = data->getPacketIDs();
@@ -102,7 +95,7 @@ namespace create {
 
     // Wait for first complete read to finish
     boost::unique_lock<boost::mutex> lock(dataReadyMut);
-    //std::cout << "#### Waiting for dataReady" << std::endl;
+
     int attempts = 1;
     int maxAttempts = 10;
     while (!dataReady) {
@@ -114,21 +107,19 @@ namespace create {
           return false;
         }
         attempts++;
-        //std::cout << "Requesting data from Create. Attempt " << attempts << std::endl;
+
         // Request data again
         sendOpcode(OC_START);
         send(streamReq, 2 + numPackets);
       }
     }
-    //std::cout << "#### Data is ready." << std::endl;
+
     isReading = true;
-    //std::cout << "### Serial start reading DONE" << std::endl;
     return true;
   }
 
   void Serial::stopReading() {
     if (isReading) {
-      //std::cout << "### Start stopReading" << std::endl;
       io.stop();
       ioThread.join();
       isReading = false;
@@ -136,12 +127,10 @@ namespace create {
         boost::lock_guard<boost::mutex> lock(dataReadyMut);
         dataReady = false;
       }
-      //std::cout << "### End stopReading" << std::endl;
     }
   }
 
   void Serial::onData(const boost::system::error_code& e, const std::size_t& size) {
-    //std::cout << "#### onData" << std::endl;
     if (e) {
       CERR("[create::Serial] ", "serial error - " << e.message());
       return;
@@ -185,11 +174,11 @@ namespace create {
         case READ_PACKET_BYTES:
           numDataBytesRead++;
           if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
-            // high byte first
+            // High byte first
             packetBytes = (byteRead << 8) & 0xff00;
           }
           else {
-            // low byte
+            // Low byte
             packetBytes += byteRead;
           }
           if (numDataBytesRead >= expectedNumDataBytes) {
@@ -209,11 +198,9 @@ namespace create {
             // Notify first data packets ready
             {
               boost::lock_guard<boost::mutex> lock(dataReadyMut);
-             // std::cout << "locking." << std::endl;
               if (!dataReady) {
                 dataReady = true;
                 dataReadyCond.notify_one();
-                //std::cout << "##### Notified." << std::endl;
               }
             }
             // Callback to notify data is ready