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)

include/create/create.h

@@ -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/packet.h

@@ -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

@@ -67,8 +67,7 @@ namespace create {
       bool isReading;
       bool firstRead;
 
-      // These are just for diagnostics, maybe not necessary
-      // TODO: Investigate
+      // These are for possible diagnostics
       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/util.h

@@ -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

@@ -419,12 +419,12 @@ namespace create {
     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;
   }

src/data.cpp

@@ -81,7 +81,7 @@ namespace create {
     if (isValidPacketID(id)) {
       return packets[id];
     }
-    return boost::shared_ptr<Packet>(); //NULL;
+    return boost::shared_ptr<Packet>();
   }
 
   void Data::validateAll() {

src/packet.cpp

@@ -11,11 +11,12 @@ namespace create {
   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);
   }
 

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