Mode report workaround

README.md

@@ -8,7 +8,7 @@ C++ library for interfacing with iRobot's [Create 1 and 2](http://www.irobot.com
   - [`V_2`](https://drive.google.com/file/d/0B9O4b91VYXMdMmFPMVNDUEZ6d0U) (Create 1, Roomba 500 series)
   - [`V_3`](https://drive.google.com/file/d/0B9O4b91VYXMdSVk4amw1N09mQ3c) (Create 2, Roomba 600-800 series)
 * Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca))
-* Contributors: [Mani Monajjemi](http:mani.im), [Ben Wolsieffer](https://github.com/lopsided98)
+* Contributors: [Mani Monajjemi](http:mani.im), [Ben Wolsieffer](https://github.com/lopsided98), [Josh Gadeken](https://github.com/process1183)
 
 ## Build Status ##
 
@@ -72,3 +72,5 @@ To run unit tests, execute the following in the build directory:
 
 * _Clock_ and _Schedule_ buttons are not functional. This is a known bug related to the firmware.
 * Inaccurate odometry angle for Create 1 ([#22](https://github.com/AutonomyLab/libcreate/issues/22))
+* Some 600 series models incorrectly report the OI Mode in their sensor stream ([create_robot #64](https://github.com/AutonomyLab/create_robot/issues/64))
+  - To enable or disable the OI Mode reporting workaround, pass `true` or `false` to `setModeReportWorkaround()`

include/create/create.h

@@ -98,6 +98,10 @@ namespace create {
       void onData();
       bool updateLEDs();
 
+      // Flag to enable/disable the workaround for some 6xx incorrectly reporting OI mode
+      // https://github.com/AutonomyLab/create_robot/issues/64
+      bool modeReportWorkaround;
+
     protected:
       std::shared_ptr<create::Data> data;
       std::shared_ptr<create::Serial> serial;
@@ -673,6 +677,20 @@ namespace create {
        * \return total number of serial packets.
        */
       uint64_t getTotalPackets() const;
+
+      /**
+       * \brief Enable or disable the mode reporting workaround.
+       * Some Roomba 6xx robots incorrectly report the OI mode in their sensor streams. Enabling the workaround
+       * will cause libcreate to decrement the reported OI mode in the sensor stream by 1.
+       * See https://github.com/AutonomyLab/create_robot/issues/64
+       */
+      void setModeReportWorkaround(const bool& enable);
+
+      /**
+       * \return true if the mode reporting workaround is enabled, false otherwise.
+       */
+      bool getModeReportWorkaround() const;
+
   };  // end Create class
 
 }  // namespace create

src/create.cpp

@@ -42,6 +42,7 @@ namespace create {
     requestedLeftVel = 0;
     requestedRightVel = 0;
     dtHistoryLength = 100;
+    modeReportWorkaround = false;
     data = std::shared_ptr<Data>(new Data(model.getVersion()));
     if (model.getVersion() == V_1) {
       serial = std::make_shared<SerialQuery>(data, install_signal_handler);
@@ -1117,10 +1118,23 @@ namespace create {
     return requestedRightVel;
   }
 
+  void Create::setModeReportWorkaround(const bool& enable) {
+    modeReportWorkaround = enable;
+  }
+
+  bool Create::getModeReportWorkaround() const {
+    return modeReportWorkaround;
+  }
+
   create::CreateMode Create::getMode() {
     if (data->isValidPacketID(ID_OI_MODE)) {
-      mode = (create::CreateMode) GET_DATA(ID_OI_MODE);
+      if (modeReportWorkaround) {
+        mode = (create::CreateMode) (GET_DATA(ID_OI_MODE) - 1);
+      } else {
+        mode = (create::CreateMode) GET_DATA(ID_OI_MODE);
+      }
     }
+
     return mode;
   }