Update Autonomy Lab link

(cherry picked from commit 116be443e7970de1574b5dc5f91e414828854c08)

.dockerignore

@@ -0,0 +1,2 @@
+.github
+ci/Dockerfile

.forgejo/workflows/package.yaml

@@ -0,0 +1,76 @@
+---
+name: Package
+
+on:
+  push:
+    tags:
+      - "*"
+
+jobs:
+  package:
+    strategy:
+      matrix:
+        platform:
+          - dpkg: arm64
+            runs-on: debian-12-arm64
+
+    runs-on: debian-12-arm64
+
+    steps:
+      - uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        with:
+          fetch-depth: 0
+
+      - name: Install dependencies
+        run: |
+          apt update
+          apt install -y \
+            build-essential cmake git libboost-system-dev libboost-thread-dev file libgtest-dev googletest
+
+      - uses: actions/checkout@v4
+
+      - name: configure
+        run: cmake -B build -DCMAKE_BUILD_TYPE=Release -DCPACK_DEBIAN_PACKAGE_RELEASE=compair1
+
+      - name: build
+        run: cmake --build build --config Release -j2
+
+      - name: test
+        working-directory: build
+        run: ctest -C Release
+
+      - name: build deb
+        working-directory: build
+        run: cpack --build . -G DEB
+
+      - name: get tag
+        run: |
+          echo "TAG=$(git describe --abbrev=0 --tags)-compair1" >>$GITHUB_ENV
+
+      # https://forgejo.org/docs/latest/user/packages/debian/#publish-a-package
+      - name: push deb to apt repository
+        env:
+          REPO: ${{ github.repository }}
+        run: |
+          url="${GITHUB_SERVER_URL}/api/packages/${{ github.repository_owner }}/debian/pool/bookworm/compair/upload"
+          urlBotball="${GITHUB_SERVER_URL}/api/packages/Botball/debian/pool/bookworm/wombatos/upload"
+
+          echo "api url: $url"
+          debfile="_packages/libcreate_${TAG}_${{ matrix.platform.dpkg }}.deb"
+
+          echo "uploading file: $debfile"
+          curl --fail-with-body \
+            -X PUT \
+            --user "oauth2:${{ secrets.PACKAGE_REGISTRY_TOKEN }}" \
+            --upload-file "$debfile" \
+            "$url"
+
+          echo "uploading file for WombatOs: $debfile"
+          curl --fail-with-body \
+            -X PUT \
+            --user "oauth2:${{ secrets.PACKAGE_REGISTRY_TOKEN }}" \
+            --upload-file "$debfile" \
+            "$urlBotball"
+
+          echo "final url: ${GITHUB_SERVER_URL}/${{ github.repository_owner }}/-/packages/debian/libcreate/${TAG}"
+          echo "final url for Botball: ${GITHUB_SERVER_URL}/Botball/-/packages/debian/libcreate/${TAG}"

.forgejo/workflows/test.yaml

@@ -0,0 +1,34 @@
+---
+name: Build and test
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+
+env:
+  BUILD_TYPE: Release
+
+jobs:
+  test:
+    runs-on: debian-12
+
+    steps:
+      - name: Install dependencies
+        run: |
+          apt update
+          apt install -y \
+            build-essential cmake git libboost-system-dev libboost-thread-dev file libgtest-dev googletest
+
+      - uses: actions/checkout@v4
+
+      - name: Configure CMake
+        run: cmake -B ${{ github.workspace }}/build -DCMAKE_BUILD_TYPE=${{ env.BUILD_TYPE }}
+
+      - name: Build
+        run: cmake --build ${{ github.workspace }}/build --config ${{ env.BUILD_TYPE }}
+
+      - name: Test
+        working-directory: ${{ github.workspace }}/build
+        run: ctest -C ${{ env.BUILD_TYPE }}

.gitignore

@@ -0,0 +1,3 @@
+# CMake/CPack
+build/
+_packages/

.travis.yml

@@ -1,12 +0,0 @@
-language: cpp
-
-compiler:
-  - gcc
-
-before_install:
-  - sudo apt-get update -qq
-  - sudo apt-get install libboost-system-dev libboost-thread-dev
-
-script:
-  - cmake .
-  - make

CHANGELOG.rst

@@ -0,0 +1,170 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package libcreate
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+3.1.0 (2023-05-09)
+------------------
+* Address warnings and errors
+* Catch boost exceptions in Serial.h
+* Contributors: Swapnil Patel
+
+3.0.0 (2022-04-06)
+------------------
+* Add option to workaround bug where firmware reports unexpected OI mode (`#67 <https://github.com/AutonomyLab/libcreate/issues/67>`_)
+* Update links to serial protocol documentation
+* Add option to disable signal handlers (`#65 <https://github.com/AutonomyLab/libcreate/issues/65>`_)
+* Fix 'maybe-uninitialized' warnings
+* Remove travis.yml
+* Add GitHub workflow for CI
+* Fix motor setting (`#62 <https://github.com/AutonomyLab/libcreate/issues/62>`_)
+* Use average dt values for velocity calculation (`#60 <https://github.com/AutonomyLab/libcreate/issues/60>`_)
+* Use steady clock for computing velocity (`#59 <https://github.com/AutonomyLab/libcreate/issues/59>`_)
+* Replace boost features with C++11 equivalents (`#58 <https://github.com/AutonomyLab/libcreate/issues/58>`_)
+* Implement methods for getting overcurrent status (`#57 <https://github.com/AutonomyLab/libcreate/issues/57>`_)
+* Use OC_MOTORS instead of OC_MOTORS_PWM on V_1 models (`#55 <https://github.com/AutonomyLab/libcreate/issues/55>`_)
+* Contributors: Daniel Smith, Jacob Perron, Josh Gadeken, Stefan Krupop, tim-fan
+
+2.0.0 (2019-09-02)
+------------------
+* Cleanup examples
+* Use std::chrono instead of custom timestamp function
+* Remove Trusty CI job
+  * Since it is EOL.
+* Default to C++11
+* Add compiler flags '-Wall -Wextra -Wpedantic'
+  * Fix warnings as a result.
+* Disconnect from serial cleanly on SIGINT
+  * Send the STOP opcode before exiting the program to ensure the robot is not left in a state that could potentially drain the battery.
+* Initialize variable for compiler compatibility
+* Add other serial communication options
+  Otherwise, it's possible that Operation "7" (0x07) is confused for "135" (0x87)
+* Add cliff sensor example
+* Add API for getting left and right cliff detections
+* Update wheeldrop example
+* Add API for getting left and right wheeldrop
+* Use shared pointer when binding callback for serial read (`#38 <https://github.com/autonomylab/libcreate/issues/38>`_)
+  * Resolves an issue with ROS Melodic on 18.04.
+* Fix for compatibility with Boost 1.66
+  * Compatibility with at least as early as Boost 1.58 still persists
+* Update wheel diameter for Create 2
+  * Now matches the spec from iRobot.
+* Add Bionic CI job
+* Add static cast to fix compiler warnings
+* Use package.xml format 3
+  * Make catkin dependency conditional on ROS 1.
+* Add Xenial build to CI
+* Remove std::cout statement
+* Contributors: Anton Gerasimov, Jacob Perron, Ryota Suzuki, Yutaka Kondo
+
+1.6.1 (2018-04-21)
+------------------
+* Build and install gtest as part of CI
+* Update README with instructions for building and running unit tests
+* Remove external cmake project for gtest
+  Now only build tests if a gtest installation already exists on the system. This should expedite time to build for users that do not care about building/running unit tests and also eliminates the need for internet access when building.
+* Add test depend to gtest in package.xml
+* Contributors: Jacob Perron
+
+1.6.0 (2018-04-07)
+------------------
+* Add unit tests (gtests)
+* Refactor Packet API
+    * Declare setData member as protected
+    * Rename 'setTempData' to 'setDataToValidate'
+* Remove redundant packets from Data constructor
+* Updated setDigits function API comments
+    * added HTML to adjust for spacing in diagram, showing the proper ordering of segments.
+* Update examples
+    * More concise and focusing on individual features:
+        * Battery level
+        * Bumpers
+        * Drive circle
+        * LEDs
+        * Serial packets
+        * Play song
+        * Wheeldrop
+* Update README
+* Refactor cmake files
+* Contributors: Jacob Perron, K.Moriarty
+
+1.5.0 (2017-12-17)
+------------------
+* Add APIs for getting the measured velocities of the wheels
+* Add ability to drive the wheels with direct pwm duty
+* Update documentation
+* Add mainpage.dox
+* Use package.xml format 2
+* Add doxygen as doc dependency
+* Contributors: Erik Schembor, Jacob Perron
+
+1.4.0 (2016-10-16)
+------------------
+* Switch to trusty for CI
+* Set mimumum cmake version to 2.8.12
+* Update CMakeLists.txt configuration and install rules
+* Add package.xml
+* Add config.cmake.in
+* Contributors: Jacob Perron
+
+1.3.0 (2016-08-23)
+------------------
+* Add support for early model Roomba 400s and other robots using the original SCI protocol.
+* Expose individual wheel distances and requested velocities. Fix wheel distance calculation for the Create 1.
+* Manually link to thread library. This allows libcreate to build on ARM.
+* Fix odometry inversion for Create 1.
+* Contributors: Ben Wolsieffer, Jacob Perron
+
+1.2.1 (2016-04-30)
+------------------
+* Make velocity relative to base frame, not odometry frame
+* Contributors: Jacob Perron
+
+1.2.0 (2016-04-15)
+------------------
+* Add covariance info to Pose and Vel
+* Fix getMode bug
+* Contributors: Jacob Perron
+
+1.1.1 (2016-04-07)
+------------------
+* Fix odometry sign error
+* Add warning in code regarding Create 1 odometry issue
+* Add odom_example.cpp
+* Contributors: Jacob Perron
+
+1.1.0 (2016-04-02)
+------------------
+* Add API to get light sensor signals
+* Contributors: Jacob Perron
+
+1.0.0 (2016-04-01)
+------------------
+* Fix odometry for Create 1
+* Fix odom angle sign error
+* Convert units to base units
+* Implement 'getMode'
+* Rename 'isIRDetect*' functions to 'isLightBumper*'
+* Documentation / code cleanup
+* Add function 'driveRadius'
+* Add function 'isVirtualWall'
+* Fix sign error on returned 'current' and 'temperature'
+* Contributors: Jacob Perron
+
+0.1.1 (2016-03-25)
+------------------
+* Fix odometry bug
+* Contributors: Jacob Perron
+
+0.1.0 (2016-03-24)
+------------------
+* Add enum of special IR characters
+* Fix bug: convert distance measurement to meters
+* Add support for first generation Create (Roomba 400 series)
+* Fix bug: Too many packets requested corrupting serial buffer
+* Expose functions for getting number of corrupt packets and total packets in Create class
+* Add getters for number of corrupt and total packets received over serial
+* Update README.md
+* Added build badge
+* Added CI (travis)
+* Instantaneous velocity now available
+* Contributors: Jacob Perron

CMakeLists.txt

@@ -1,45 +1,227 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(libcreate)
+#########
+# Setup #
+#########
 
-find_package(Boost REQUIRED system thread)
+cmake_minimum_required(VERSION 3.25)
 
-## Specify additional locations of header files
+set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
+
+include(Versioning)
+
+########
+# Main #
+########
+
+# After installation this project can be found by 'find_package' command:
+#
+#    find_package(libcreate REQUIRED)
+#    include_directores(${libcreate_INCLUDE_DIRS})
+#    target_link_libraries(... ${libcreate_LIBRARIES})
+#
+
+project(
+  libcreate 
+  VERSION ${TAG_VERSION_MAJOR}.${TAG_VERSION_MINOR}.${TAG_VERSION_PATCH}
+)
+
+add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+
+set(PACKAGE_VERSION ${TAG_VERSION_MAJOR}.${TAG_VERSION_MINOR}.${TAG_VERSION_PATCH})
+
+option(LIBCREATE_BUILD_TESTS "Enable the build of tests." ON)
+
+find_package(Boost REQUIRED COMPONENTS system thread)
+find_package(Threads REQUIRED)
+
+# Default to C++11
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 11)
+endif()
+
+#########
+# Build #
+#########
+
+set(LIBRARY_NAME create)
+
+# Specify locations of header files
 include_directories(
   include
 )
 
-## Declare cpp library
-add_library(create
+# Declare cpp library
+add_library(${LIBRARY_NAME} SHARED
   src/create.cpp
   src/serial.cpp
+  src/serial_stream.cpp
+  src/serial_query.cpp
   src/data.cpp
   src/packet.cpp
+  src/types.cpp
 )
 
-target_link_libraries(create
+# Manually link to thread library for build on ARM
+if(THREADS_HAVE_PTHREAD_ARG)
+  set_property(TARGET ${LIBRARY_NAME} PROPERTY COMPILE_OPTIONS "-pthread")
+  set_property(TARGET ${LIBRARY_NAME} PROPERTY INTERFACE_COMPILE_OPTIONS "-pthread")
+endif()
+
+if(CMAKE_THREAD_LIBS_INIT)
+  target_link_libraries(${LIBRARY_NAME} "${CMAKE_THREAD_LIBS_INIT}")
+endif()
+
+# Link to Boost
+target_link_libraries(${LIBRARY_NAME}
   ${Boost_LIBRARIES}
 )
 
-## Declare example executables
-add_executable(create_demo examples/create_demo.cpp)
-add_executable(bumper_example examples/bumper_example.cpp)
+# Declare example executables
+set(EXAMPLES
+  battery_level
+  bumpers
+  cliffs
+  drive_circle
+  leds
+  packets
+  play_song
+  wheeldrop
+)
 
-## Specify libraries to link a library or executable target against
-target_link_libraries(create_demo
+foreach(EXAMPLE ${EXAMPLES})
+  add_executable(${EXAMPLE} examples/${EXAMPLE}.cpp)
+
+  target_link_libraries(${EXAMPLE}
     ${Boost_LIBRARIES}
-  create
+    ${LIBRARY_NAME}
+  )
+endforeach()
+
+#################
+# Configuration #
+#################
+
+# set(CMAKE_INSTALL_PREFIX "/usr/") // complib needs  this, riplib doesn't
+
+# Install directories layout:
+#   * <prefix>/lib/
+#   * <prefix>/bin/
+#   * <prefix>/include/
+#   * <prefix>/lib/cmake/<PROJECT-NAME>
+#   * <prefix>/share/<PROJECT_NAME>
+set(LIB_INSTALL_DIR "lib")
+set(BIN_INSTALL_DIR "bin")
+set(INCLUDE_INSTALL_DIR "include")
+set(CONFIG_INSTALL_DIR "${LIB_INSTALL_DIR}/cmake/${PROJECT_NAME}")
+set(SHARE_INSTALL_DIR "share/${PROJECT_NAME}")
+
+set(GENERATED_DIR "${CMAKE_CURRENT_BINARY_DIR}/generated")
+set(VERSION_CONFIG "${GENERATED_DIR}/${PROJECT_NAME}-config-version.cmake")
+set(PROJECT_CONFIG "${GENERATED_DIR}/${PROJECT_NAME}-config.cmake")
+set(TARGETS_EXPORT_NAME "${PROJECT_NAME}-targets")
+
+include(CMakePackageConfigHelpers)
+
+# Configure '<PROJECT-NAME>-config-version.cmake'
+write_basic_package_version_file(
+  "${VERSION_CONFIG}"
+  VERSION "${PACKAGE_VERSION}"
+  COMPATIBILITY SameMajorVersion
 )
-target_link_libraries(bumper_example
-  ${Boost_LIBRARIES}
-  create
+
+# Configure '<PROJECT-NAME>-config.cmake'
+configure_package_config_file(
+  "config.cmake.in"
+  "${PROJECT_CONFIG}"
+  INSTALL_DESTINATION "${CONFIG_INSTALL_DIR}"
+  PATH_VARS
+    INCLUDE_INSTALL_DIR
+    LIBRARY_NAME
 )
-## Install
-install(TARGETS create DESTINATION lib)
-install(FILES
-        include/create/create.h
-        include/create/serial.h
-        include/create/types.h
-        include/create/data.h
-        include/create/packet.h
-        include/create/util.h
-        DESTINATION include/create)
+
+###########
+# Install #
+###########
+
+# Install targets
+install(
+  TARGETS ${LIBRARY_NAME}
+  EXPORT "${TARGETS_EXPORT_NAME}"
+  LIBRARY DESTINATION "${LIB_INSTALL_DIR}"
+  ARCHIVE DESTINATION "${LIB_INSTALL_DIR}"
+  RUNTIME DESTINATION "${BIN_INSTALL_DIR}"
+  INCLUDES DESTINATION "${INCLUDE_INSTALL_DIR}"
+)
+
+# Install headers
+install(
+  DIRECTORY include/
+  DESTINATION ${INCLUDE_INSTALL_DIR}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+# Install config
+install(
+  FILES "${PROJECT_CONFIG}" "${VERSION_CONFIG}"
+  DESTINATION "${CONFIG_INSTALL_DIR}"
+)
+
+# Install targets
+install(
+  EXPORT "${TARGETS_EXPORT_NAME}"
+  DESTINATION "${CONFIG_INSTALL_DIR}"
+)
+
+# Install package.xml (for catkin)
+install(
+  FILES package.xml
+  DESTINATION ${SHARE_INSTALL_DIR}
+)
+
+###########
+# Testing #
+###########
+
+if(LIBCREATE_BUILD_TESTS)
+  find_package(GTest)
+  include_directories(${GTEST_INCLUDE_DIRS})
+endif()
+
+if(LIBCREATE_BUILD_TESTS AND ${GTEST_FOUND})
+  message("GTest installation found. Building tests.")
+
+  enable_testing()
+
+  # Add tests
+  set(LIBCREATE_TESTS
+    test_create
+    test_data
+    test_packet
+    test_robot_model
+    test_serial_stream
+    test_serial_query
+  )
+
+  foreach(LIBCREATE_TEST ${LIBCREATE_TESTS})
+    add_executable(${LIBCREATE_TEST} tests/${LIBCREATE_TEST}.cpp)
+
+    target_link_libraries(${LIBCREATE_TEST}
+      ${LIBRARY_NAME}
+      ${GTEST_LIBRARIES}
+      gtest_main
+    )
+
+    add_test(
+      NAME ${LIBCREATE_TEST}
+      COMMAND ${LIBCREATE_TEST}
+    )
+  endforeach()
+else()
+  message("No GTest installation found. Skipping tests.")
+endif()
+
+#############
+# Packaging #
+#############
+
+include(Packing)

README.md

@@ -1,33 +1,76 @@
-# libcreate
+# libcreate #
 
-C++ library for interfacing with iRobot's [Create 1 and 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx). This library forms the basis of the ROS driver in [create_autonomy](https://github.com/autonomylab/create_autonomy).
+C++ library for interfacing with iRobot's Create 1 and 2 as well as most models of Roomba. [create_robot](http://wiki.ros.org/create_robot) is a [ROS](http://www.ros.org/) wrapper for this library.
 
-* Documentation: TODO
-* Code API: TODO
-* Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca))
-* Contributors: [Mani Monajjemi](http:mani.im)
+* [Code API](http://docs.ros.org/noetic/api/libcreate/html/index.html)
+* Protocol documentation:
+  - [`V_1`](https://drive.google.com/file/d/0B9O4b91VYXMdUHlqNklDU09NU0k/view?usp=sharing&resourcekey=0-KxMpRPBMsGAj7eSYC_9ewA) (Roomba 400 series )
+  - [`V_2`](https://drive.google.com/file/d/0B9O4b91VYXMdMmFPMVNDUEZ6d0U/view?usp=sharing&resourcekey=0-bqKH8xhtWdYtTik_LLWo9Q) (Create 1, Roomba 500 series)
+  - [`V_3`](https://drive.google.com/file/d/0B9O4b91VYXMdSVk4amw1N09mQ3c/view?usp=sharing&resourcekey=0-rKvug2IzC7nj4zV31EJtww) (Create 2, Roomba 600-800 series)
+* Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](https://autonomy.cs.sfu.ca), [Simon Fraser University](http://www.sfu.ca))
+* Contributors: [Mani Monajjemi](http:mani.im), [Ben Wolsieffer](https://github.com/lopsided98), [Josh Gadeken](https://github.com/process1183)
 
-## Dependencies
+## Build Status ##
+
+![Build Status](https://github.com/AutonomyLab/libcreate/workflows/Build%20and%20test/badge.svg)
+
+## Dependencies ##
 
 * [Boost System Library](http://www.boost.org/doc/libs/1_59_0/libs/system/doc/index.html)
 * [Boost Thread Library](http://www.boost.org/doc/libs/1_59_0/doc/html/thread.html)
+* [Optional] [googletest](https://github.com/google/googletest)
 
-## Install
+### Install ###
 
-* `cmake CMakeLists.txt`
-* `make`
-* `sudo make install`
+        sudo apt-get install build-essential cmake libboost-system-dev libboost-thread-dev
 
-## Example
+        # Optionally, install gtest for building unit tests
+        sudo apt-get install libgtest-dev
+        cd /usr/src/gtest
+        sudo cmake CMakeLists.txt
+        sudo make
+        sudo cp *.a /usr/lib
 
-See source for examples.
+#### Serial Permissions ####
 
-Example compile line: `g++ create_demo.cpp -lcreate -lboost_system -lboost_thread`
+User permission is requried to connect to Create over serial. You can add your user to the dialout group to get permission:
 
-## Bugs
+        sudo usermod -a -G dialout $USER
 
-* _Clock_ and _Schedule_ button presses are not detected. This is a known problem to the developers at iRobot.
+Logout and login again for this to take effect.
 
-## Build Status
+## Build ##
 
-![Build Status](https://api.travis-ci.org/AutonomyLab/libcreate.svg?branch=master)
+Note, the examples found in the "examples" directory are built with the library.
+
+#### cmake ####
+
+        git clone https://github.com/AutonomyLab/libcreate.git
+        cd libcreate
+        mkdir build && cd build
+        cmake ..
+        make -j
+
+#### catkin ####
+
+Requires [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/).
+
+        mkdir -p create_ws/src
+        cd create_ws
+        catkin init
+        cd src
+        git clone https://github.com/AutonomyLab/libcreate.git
+        catkin build
+
+## Running Tests ##
+
+To run unit tests, execute the following in the build directory:
+
+        make test
+
+## Known Issues ##
+
+* _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()`

cmake/Packing.cmake

@@ -0,0 +1,41 @@
+# these are cache variables, so they could be overwritten with -D,
+set(CPACK_PACKAGE_NAME ${PROJECT_NAME}
+    CACHE STRING "The resulting package name"
+)
+# which is useful in case of packing only selected components instead of the whole thing
+set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "C++ library for interfacing with iRobot's Create 1 and 2"
+    CACHE STRING "Package description for the package metadata"
+)
+set(CPACK_PACKAGE_VENDOR "Verein zur Förderung von Jugendlichen durch Robotikwettbewerbe")
+
+set(CPACK_VERBATIM_VARIABLES YES)
+
+set(CPACK_PACKAGE_INSTALL_DIRECTORY ${CPACK_PACKAGE_NAME})
+SET(CPACK_OUTPUT_FILE_PREFIX "${CMAKE_SOURCE_DIR}/_packages")
+
+set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR})
+set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR})
+set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH})
+
+set(CPACK_PACKAGE_CONTACT "kontakt@comp-air.at")
+set(CPACK_DEBIAN_PACKAGE_MAINTAINER "comp-air dev team")
+
+set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
+set(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
+
+# Discover and set dependencies correcly
+set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS YES)
+
+# The installation path directory should have 0755 permissions
+set(
+    CPACK_INSTALL_DEFAULT_DIRECTORY_PERMISSIONS
+    OWNER_READ OWNER_WRITE OWNER_EXECUTE
+    GROUP_READ GROUP_EXECUTE
+    WORLD_READ WORLD_EXECUTE
+)
+
+# package name for deb. If set, then instead of some-application-0.9.2-Linux.deb
+# you'll get some-application_0.9.2_amd64.deb (note the underscores too)
+set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT)
+
+include(CPack)

cmake/Versioning.cmake

@@ -0,0 +1,31 @@
+find_package(Git)
+
+if(GIT_EXECUTABLE)
+    execute_process(
+        COMMAND ${GIT_EXECUTABLE} describe --tags
+        OUTPUT_VARIABLE TAG_VERSION
+        RESULT_VARIABLE ERROR_CODE
+        OUTPUT_STRIP_TRAILING_WHITESPACE
+    )
+
+    if(DEFINED ENV{GITHUB_REF} AND ENV{GITHUB_REF_TYPE} EQUAL "tag")
+        set(TAG_VERSION $ENV{GITHUB_REF})
+        message(STATUS "Extracted version from GITHUB_REF")
+    endif()
+
+    if(TAG_VERSION STREQUAL "")
+        set(TAG_VERSION 0.0.0)
+        message(WARNING "Failed to determine version from Git tags. Using default version \"${TAG_VERSION}\".")
+    endif()
+
+    message(STATUS "Project version: ${TAG_VERSION}")
+
+    # Split into major, minor, patch
+    string(
+        REGEX MATCH "([0-9]+)\\.([0-9]+)\\.([0-9]+)"
+        TAG_VERSION_MATCH ${TAG_VERSION}
+    )
+    set(TAG_VERSION_MAJOR ${CMAKE_MATCH_1})
+    set(TAG_VERSION_MINOR ${CMAKE_MATCH_2})
+    set(TAG_VERSION_PATCH ${CMAKE_MATCH_3})
+endif()

config.cmake.in

@@ -0,0 +1,10 @@
+@PACKAGE_INIT@
+find_package(Boost REQUIRED COMPONENTS system thread)
+find_package(Threads REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@TARGETS_EXPORT_NAME@.cmake")
+set_and_check(libcreate_INCLUDE_DIRS "@PACKAGE_INCLUDE_INSTALL_DIR@")
+list(APPEND libcreate_INCLUDE_DIRS "${Boost_INCLUDE_DIRS}")
+set(libcreate_LIBRARIES "@LIBRARY_NAME@")
+list(APPEND libcreate_LIBRARIES "${Boost_LIBRARIES}")
+check_required_components("@PROJECT_NAME@")

examples/battery_level.cpp

@@ -0,0 +1,78 @@
+/**
+Software License Agreement (BSD)
+
+\file      battery_level.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud))
+    std::cout << "Connected to robot" << std::endl;
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  // Switch to Full mode
+  robot.setMode(create::MODE_FULL);
+
+  // Get battery capacity (max charge)
+  const float battery_capacity = robot.getBatteryCapacity();
+
+  float battery_charge = 0.0f;
+  while (true) {
+    // Get battery charge
+    battery_charge = robot.getBatteryCharge();
+
+    // Print battery percentage
+    std::cout << "\rBattery level: " << (battery_charge / battery_capacity) * 100.0 << "%";
+
+    usleep(100000);  // 10 Hz
+  }
+
+  return 0;
+}

examples/bumper_example.cpp

@@ -1,73 +0,0 @@
-#include "create/create.h"
-
-int main(int argc, char** argv) {
-  std::string port = "/dev/ttyUSB0";
-  int baud = 115200;
-  create::RobotModel model = create::CREATE_2;
-
-  create::Create* 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);
-
-  uint16_t signals[6];
-  bool contact_bumpers[2];
-  bool light_bumpers[6];
-
-  // Stop program when clean button is pressed
-  while (!robot->isCleanButtonPressed()) {
-    signals[0] = robot->getLightSignalLeft();
-    signals[1] = robot->getLightSignalFrontLeft();
-    signals[2] = robot->getLightSignalCenterLeft();
-    signals[3] = robot->getLightSignalCenterRight();
-    signals[4] = robot->getLightSignalFrontRight();
-    signals[5] = robot->getLightSignalRight();
-
-    contact_bumpers[0] = robot->isLeftBumper();
-    contact_bumpers[1] = robot->isRightBumper();
-
-    light_bumpers[0] = robot->isLightBumperLeft();
-    light_bumpers[1] = robot->isLightBumperFrontLeft();
-    light_bumpers[2] = robot->isLightBumperCenterLeft();
-    light_bumpers[3] = robot->isLightBumperCenterRight();
-    light_bumpers[4] = robot->isLightBumperFrontRight();
-    light_bumpers[5] = robot->isLightBumperRight();
-
-    // print signals from left to right
-    std::cout << "[ " << signals[0] << " , "
-                      << signals[1] << " , "
-                      << signals[2] << " , "
-                      << signals[3] << " , "
-                      << signals[4] << " , "
-                      << signals[5]
-              << " ]" << std::endl;
-    std::cout << "[ " << light_bumpers[0] << " , "
-                      << light_bumpers[1] << " , "
-                      << light_bumpers[2] << " , "
-                      << light_bumpers[3] << " , "
-                      << light_bumpers[4] << " , "
-                      << light_bumpers[5]
-              << " ]" << std::endl;
-    std::cout << "[ " << contact_bumpers[0] << " , "
-                      << contact_bumpers[1]
-              << " ]" << std::endl;
-    std::cout << std::endl;
-
-    usleep(1000 * 100); //10hz
-  }
-
-  std::cout << "Stopping Create" << std::endl;
-
-  // Disconnect from robot
-  robot->disconnect();
-  delete robot;
-
-  return 0;
-}

examples/bumpers.cpp

@@ -0,0 +1,115 @@
+/**
+Software License Agreement (BSD)
+
+\file      bumpers.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud))
+    std::cout << "Connected to robot" << std::endl;
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  // Switch to Full mode
+  robot.setMode(create::MODE_FULL);
+
+  uint16_t light_signals[6] = {0, 0, 0, 0, 0, 0};
+  bool light_bumpers[6] = {false, false, false, false, false, false};
+  bool contact_bumpers[2] = {false, false};
+
+  while (true) {
+    // Get light sensor data (only available for Create 2 or later robots)
+    if (model == create::RobotModel::CREATE_2) {
+      // Get raw light signal values
+      light_signals[0] = robot.getLightSignalLeft();
+      light_signals[1] = robot.getLightSignalFrontLeft();
+      light_signals[2] = robot.getLightSignalCenterLeft();
+      light_signals[3] = robot.getLightSignalCenterRight();
+      light_signals[4] = robot.getLightSignalFrontRight();
+      light_signals[5] = robot.getLightSignalRight();
+
+      // Get obstacle data from light sensors (true/false)
+      light_bumpers[0] = robot.isLightBumperLeft();
+      light_bumpers[1] = robot.isLightBumperFrontLeft();
+      light_bumpers[2] = robot.isLightBumperCenterLeft();
+      light_bumpers[3] = robot.isLightBumperCenterRight();
+      light_bumpers[4] = robot.isLightBumperFrontRight();
+      light_bumpers[5] = robot.isLightBumperRight();
+    }
+
+    // Get state of bumpers
+    contact_bumpers[0] = robot.isLeftBumper();
+    contact_bumpers[1] = robot.isRightBumper();
+
+    // Print signals from left to right
+    std::cout << "[ " << light_signals[0] << " , "
+                      << light_signals[1] << " , "
+                      << light_signals[2] << " , "
+                      << light_signals[3] << " , "
+                      << light_signals[4] << " , "
+                      << light_signals[5]
+              << " ]" << std::endl;
+    std::cout << "[ " << light_bumpers[0] << " , "
+                      << light_bumpers[1] << " , "
+                      << light_bumpers[2] << " , "
+                      << light_bumpers[3] << " , "
+                      << light_bumpers[4] << " , "
+                      << light_bumpers[5]
+              << " ]" << std::endl;
+    std::cout << "[ " << contact_bumpers[0] << " , "
+                      << contact_bumpers[1]
+              << " ]" << std::endl;
+    std::cout << std::endl;
+
+    usleep(100000);  // 10 Hz
+  }
+
+  return 0;
+}

examples/cliffs.cpp

@@ -0,0 +1,86 @@
+/**
+Software License Agreement (BSD)
+
+\file      cliffs.cpp
+\authors   Jacob Perron <jacobmperron@gmail.com>
+\copyright Copyright (c) 2019, Jacob Perron, All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of the copyright holder nor the names of its contributors
+   may be used to endorse or promote products derived from this software
+   without specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud)) {
+    std::cout << "Connected to robot" << std::endl;
+  }
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  // Switch to Full mode
+  robot.setMode(create::MODE_FULL);
+
+  while (true) {
+    // Get cliff status
+    const bool cliff_left = robot.isCliffLeft();
+    const bool cliff_front_left = robot.isCliffFrontLeft();
+    const bool cliff_front_right = robot.isCliffFrontRight();
+    const bool cliff_right = robot.isCliffRight();
+
+    // Print status
+    std::cout << "\rCliffs (left to right): [ " <<
+      cliff_left <<
+      ", " <<
+      cliff_front_left <<
+      ", " <<
+      cliff_front_right <<
+      ", " <<
+      cliff_right <<
+      " ]";
+
+    usleep(10000);  // 10 Hz
+  }
+
+  return 0;
+}

examples/create_demo.cpp

@@ -1,133 +0,0 @@
-#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;
-  }
-
-  // Note: Some functionality does not work as expected in Safe mode
-  robot->setMode(create::MODE_FULL);
-
-  std::cout << "battery level: " <<
-    robot->getBatteryCharge() / (float) robot->getBatteryCapacity() * 100.0 << "%" << std::endl;
-
-  bool drive = false;
-
-  // Make a song
-  //uint8_t songLength = 16;
-  //uint8_t notes[16] = { 67, 67, 66, 66, 65, 65, 66, 66,
-  //                      67, 67, 66, 66, 65, 65, 66, 66 };
-  //float durations[songLength];
-  //for (int i = 0; i < songLength; i++) {
-  //  durations[i] = 0.25;
-  //}
-  //robot->defineSong(0, songLength, notes, durations);
-  //usleep(1000000);
-  //robot->playSong(0);
-
-  // Quit when center "Clean" button pressed
-  while (!robot->isCleanButtonPressed()) {
-    // Check for button presses
-    if (robot->isDayButtonPressed())
-      std::cout << "day button press" << std::endl;
-    if (robot->isMinButtonPressed())
-      std::cout << "min button press" << std::endl;
-    if (robot->isDockButtonPressed()) {
-      std::cout << "dock button press" << std::endl;
-      robot->enableCheckRobotLED(false);
-    }
-    if (robot->isSpotButtonPressed()) {
-      std::cout << "spot button press" << std::endl;
-      robot->enableCheckRobotLED(true);
-    }
-    if (robot->isHourButtonPressed()) {
-      std::cout << "hour button press" << std::endl;
-      drive = !drive;
-    }
-
-    // Check for wheeldrop or cliffs
-    if (robot->isWheeldrop() || robot->isCliff()) {
-      drive = false;
-      robot->setPowerLED(255);
-    }
-
-    // If everything is ok, drive forward using IR's to avoid obstacles
-    if (drive) {
-      robot->setPowerLED(0); // green
-      if (robot->isLightBumperLeft() ||
-          robot->isLightBumperFrontLeft() ||
-          robot->isLightBumperCenterLeft()) {
-        // turn right
-        robot->drive(0.1, -1.0);
-        robot->setDigitsASCII('-','-','-',']');
-      }
-      else if (robot->isLightBumperRight() ||
-               robot->isLightBumperFrontRight() ||
-               robot->isLightBumperCenterRight()) {
-        // turn left
-        robot->drive(0.1, 1.0);
-        robot->setDigitsASCII('[','-','-','-');
-      }
-      else {
-        // drive straight
-        robot->drive(0.1, 0.0);
-        robot->setDigitsASCII(' ','^','^',' ');
-      }
-    }
-    else { // drive == false
-      // stop moving
-      robot->drive(0, 0);
-      robot->setDigitsASCII('S','T','O','P');
-    }
-
-    // Turn on blue 'debris' light if moving forward
-    if (robot->isMovingForward()) {
-      robot->enableDebrisLED(true);
-    }
-    else {
-      robot->enableDebrisLED(false);
-    }
-
-    // Check bumpers
-    if (robot->isLeftBumper()) {
-      std::cout << "left bump detected!" << std::endl;
-    }
-    if (robot->isRightBumper()) {
-      std::cout << "right bump detected!" << std::endl;
-    }
-
-    usleep(1000 * 100); //10hz
-  }
-
-  std::cout << "Stopping Create." << std::endl;
-
-  // 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;
-
-  return 0;
-}

examples/drive_circle.cpp

@@ -0,0 +1,81 @@
+/**
+Software License Agreement (BSD)
+
+\file      drive_circle.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iomanip>
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud))
+    std::cout << "Connected to robot" << std::endl;
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  // Switch to Full mode
+  robot.setMode(create::MODE_FULL);
+
+  // There's a delay between switching modes and when the robot will accept drive commands
+  usleep(100000);
+
+  // Command robot to drive a radius of 0.15 metres at 0.2 m/s
+  robot.driveRadius(0.2, 0.15);
+
+  while (true) {
+    // Get robot odometry and print
+    const create::Pose pose = robot.getPose();
+
+    std::cout << std::fixed << std::setprecision(2) << "\rOdometry (x, y, yaw): ("
+              << pose.x << ", " << pose.y << ", " << pose.yaw << ")      ";
+
+    usleep(10000);  // 10 Hz
+  }
+
+  return 0;
+}

examples/leds.cpp

@@ -0,0 +1,94 @@
+/**
+Software License Agreement (BSD)
+
+\file      leds.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud))
+    std::cout << "Connected to robot" << std::endl;
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  // Switch to Full mode
+  robot.setMode(create::MODE_FULL);
+
+  bool enable_check_robot_led = true;
+  bool enable_debris_led = false;
+  bool enable_dock_led = true;
+  bool enable_spot_led = false;
+  uint8_t power_led = 0;
+  char digit_buffer[5];
+
+  while (true) {
+    // Set LEDs
+    robot.enableCheckRobotLED(enable_check_robot_led);
+    robot.enableDebrisLED(enable_debris_led);
+    robot.enableDockLED(enable_dock_led);
+    robot.enableSpotLED(enable_spot_led);
+    robot.setPowerLED(power_led);
+
+    // Set 7-segment displays
+    const int len = sprintf(digit_buffer, "%d", power_led);
+    for (int i = len; i < 4; i++) digit_buffer[i] = ' ';
+    robot.setDigitsASCII(digit_buffer[0], digit_buffer[1], digit_buffer[2], digit_buffer[3]);
+
+    // Update LED values
+    enable_check_robot_led = !enable_check_robot_led;
+    enable_debris_led = !enable_debris_led;
+    enable_dock_led = !enable_dock_led;
+    enable_spot_led = !enable_spot_led;
+    power_led++;
+
+    usleep(250000);  // 5 Hz
+  }
+
+  return 0;
+}

examples/packets.cpp

@@ -0,0 +1,72 @@
+/**
+Software License Agreement (BSD)
+
+\file      packets.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud))
+    std::cout << "Connected to robot" << std::endl;
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  while (true) {
+    // Get serial packet statistics
+    const uint64_t total_packets = robot.getTotalPackets();
+    const uint64_t num_corrupt_packets = robot.getNumCorruptPackets();
+
+    // Print packet stats 
+    std::cout << "\rTotal packets: " << total_packets << "  Corrupt packets: " << num_corrupt_packets;
+
+    usleep(100000);  // 10 Hz
+  }
+
+  return 0;
+}

examples/play_song.cpp

@@ -0,0 +1,93 @@
+/**
+Software License Agreement (BSD)
+
+\file      play_song.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud))
+    std::cout << "Connected to robot" << std::endl;
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  // Switch to Full mode
+  robot.setMode(create::MODE_FULL);
+
+  // Useful note defintions
+  const uint8_t G = 55;
+  const uint8_t AS = 58;
+  const uint8_t DS = 51;
+  const float half = 1.0f;
+  const float quarter = 0.5f;
+  const float dotted_eigth = 0.375f;
+  const float sixteenth = 0.125f;
+
+  // Define a song
+  const uint8_t song_len = 9;
+  const uint8_t notes[song_len] = { G, G, G, DS, AS, G, DS, AS, G };
+  const float durations[song_len] = { quarter, quarter, quarter, dotted_eigth, sixteenth, quarter, dotted_eigth, sixteenth, half };
+  robot.defineSong(0, song_len, notes, durations);
+
+  // Sleep to provide time for song to register
+  usleep(1000000);
+
+  std::cout << "Playing a song!" << std::endl;
+
+  // Request to play the song we just defined
+  robot.playSong(0);
+
+  // Expect the song to take about four seconds
+  usleep(4500000);
+
+  // Call disconnect to avoid leaving robot in Full mode
+  robot.disconnect();
+
+  return 0;
+}

examples/wheeldrop.cpp

@@ -0,0 +1,85 @@
+/**
+Software License Agreement (BSD)
+
+\file      wheeldrop.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+
+#include <iostream>
+
+int main(int argc, char** argv) {
+  // Select robot. Assume Create 2 unless argument says otherwise
+  create::RobotModel model = create::RobotModel::CREATE_2;
+  std::string port = "/dev/ttyUSB0";
+  int baud = 115200;
+  if (argc > 1 && std::string(argv[1]) == "create1") {
+    model = create::RobotModel::CREATE_1;
+    baud = 57600;
+    std::cout << "Running driver for Create 1" << std::endl;
+  }
+  else {
+    std::cout << "Running driver for Create 2" << std::endl;
+  }
+
+  // Construct robot object
+  create::Create robot(model);
+
+  // Connect to robot
+  if (robot.connect(port, baud))
+    std::cout << "Connected to robot" << std::endl;
+  else {
+    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
+    return 1;
+  }
+
+  // Switch to Full mode
+  robot.setMode(create::MODE_FULL);
+
+  while (true) {
+    // Get wheeldrop status
+    const bool wheeldrop_left = robot.isLeftWheeldrop();
+    const bool wheeldrop_right = robot.isRightWheeldrop();
+
+    // Print status
+    std::cout << "\rWheeldrop status (left and right): [ " <<
+      wheeldrop_left <<
+      ", " <<
+      wheeldrop_right <<
+      " ]";
+
+    // If dropped, then make light red
+    if (wheeldrop_left || wheeldrop_right)
+      robot.setPowerLED(255);  // Red
+    else
+      robot.setPowerLED(0);  // Green
+
+    usleep(10000);  // 10 Hz
+  }
+
+  return 0;
+}

include/create/create.h

@@ -32,11 +32,15 @@ POSSIBILITY OF SUCH DAMAGE.
 #ifndef CREATE_H
 #define CREATE_H
 
-#include <boost/shared_ptr.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+#include <chrono>
+#include <memory>
 #include <string>
 #include <unistd.h>
+#include <deque>
 
-#include "create/serial.h"
+#include "create/serial_stream.h"
+#include "create/serial_query.h"
 #include "create/data.h"
 #include "create/types.h"
 #include "create/util.h"
@@ -44,6 +48,8 @@ POSSIBILITY OF SUCH DAMAGE.
 namespace create {
   class Create {
     private:
+      typedef boost::numeric::ublas::matrix<float> Matrix;
+
       enum CreateLED {
         LED_DEBRIS = 1,
         LED_SPOT = 2,
@@ -65,71 +71,119 @@ namespace create {
       uint8_t powerLED;
       uint8_t powerLEDIntensity;
 
+      CreateMode mode;
+
       create::Pose pose;
       create::Vel vel;
 
       uint32_t prevTicksLeft;
       uint32_t prevTicksRight;
-      float prevLeftVel;
-      float prevRightVel;
+      float totalLeftDist;
+      float totalRightDist;
       bool firstOnData;
-      util::timestamp_t prevOnDataTime;
+      std::chrono::time_point<std::chrono::steady_clock> prevOnDataTime;
+      std::deque<float> dtHistory;
+      uint8_t dtHistoryLength;
 
-      void init();
-      bool updateLEDs();
+      Matrix poseCovar;
+
+      float measuredLeftVel;
+      float measuredRightVel;
+      float requestedLeftVel;
+      float requestedRightVel;
+
+      void init(bool install_signal_handler);
+      // Add two matrices and handle overflow case
+      Matrix addMatrices(const Matrix &A, const Matrix &B) const;
       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:
-      boost::shared_ptr<create::Data> data;
-      boost::shared_ptr<create::Serial> serial;
+      std::shared_ptr<create::Data> data;
+      std::shared_ptr<create::Serial> serial;
 
     public:
-      /* Default constructor.
-       * Does not attempt to establish serial connection to Create.
+      /**
+       * \brief Default constructor.
+       *
+       * Calling this constructor Does not attempt to establish a serial connection to the robot.
+       *
+       * \param model the type of the robot. See RobotModel to determine the value for your robot.
+       * \param install_signal_handler if true, then register a signal handler to disconnect from
+       *   the robot on SIGINT or SIGTERM.
        */
-      Create(RobotModel = CREATE_2);
+      Create(RobotModel model = RobotModel::CREATE_2, bool install_signal_handler = true);
 
-      /* Attempts to establish serial connection to Create.
+      /**
+       * \brief Attempts to establish serial connection to Create.
+       *
        * \param port of your computer that is connected to Create.
        * \param baud rate to communicate with Create. Typically,
        *        115200 for Create 2 and 57600 for Create 1.
-       * \param model type of robot.
+       * \param model type of robot. See RobotModel to determine the value for your robot.
+       * \param install_signal_handler if true, then register a signal handler to disconnect from
+       *   the robot on SIGINT or SIGTERM.
        */
-      Create(const std::string& port, const int& baud, RobotModel model = CREATE_2);
+      Create(const std::string& port, const int& baud, RobotModel model = RobotModel::CREATE_2, bool install_signal_handler = true);
 
+      /**
+       * \brief Attempts to disconnect from serial.
+       */
       ~Create();
 
-      /* Make a serial connection to Create.
+      /**
+       * \brief Resets the create as if the battery was removed and reinserted.
+       */
+      void reset();
+
+      /**
+       * \brief Make a serial connection to Create.
+       *
        * This is the first thing that should be done after instantiated this class.
+       *
        * \return true if a successful connection is established, false otherwise.
        */
       bool connect(const std::string& port, const int& baud);
 
+      /**
+       * \brief Check if serial connection is active.
+       *
+       * \return true if successfully connected, false otherwise.
+       */
       inline bool connected() const { return serial->connected(); };
 
-      /* Disconnect from serial.
+      /**
+       * \brief Disconnect from serial.
        */
       void disconnect();
 
-      /* Change Create mode.
-       * \param mode to put Create in.
+      /**
+       * \brief Change Create mode.
+       * \param mode to change Create to.
        * \return true if successful, false otherwise
        */
       bool setMode(const create::CreateMode& mode);
 
-      /* Starts a cleaning mode.
+      /**
+       * \brief Starts a cleaning mode.
        * Changes mode to MODE_PASSIVE.
        * \return true if successful, false otherwise
        */
       bool clean(const create::CleanMode& mode = CLEAN_DEFAULT);
 
-      /* Starts the docking behaviour.
+      /**
+       * \brief Starts the docking behaviour.
        * Changes mode to MODE_PASSIVE.
        * \return true if successful, false otherwise
        */
       bool dock() const;
 
-      /* Sets the internal clock of Create.
+      /**
+       * \brief Sets the internal clock of Create.
        * \param day in range [0, 6]
        * \param hour in range [0, 23]
        * \param min in range [0, 59]
@@ -137,7 +191,8 @@ namespace create {
        */
       bool setDate(const create::DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const;
 
-      /* Set the average wheel velocity and turning radius of Create.
+      /**
+       * \brief Set the average wheel velocity and turning radius of Create.
        * \param velocity is in m/s bounded between [-0.5, 0.5]
        * \param radius in meters.
        *        Special cases: drive straight = CREATE_2_STRAIGHT_RADIUS,
@@ -145,41 +200,55 @@ namespace create {
        *                       turn in place clockwise = -CREATE_2_IN_PLACE_RADIUS
        * \return true if successful, false otherwise
        */
-      bool driveRadius(const float& velocity, const float& radius) const;
+      bool driveRadius(const float& velocity, const float& radius);
 
-      /* Set the velocities for the left and right wheels.
+      /**
+       * \brief Set the velocities for the left and right wheels.
        * \param leftWheel velocity in m/s.
        * \param rightWheel veloctiy in m/s.
        * \return true if successful, false otherwise
        */
-      bool driveWheels(const float& leftWheel, const float& rightWheel) const;
+      bool driveWheels(const float& leftWheel, const float& rightWheel);
 
-      /* Set the forward and angular velocity of Create.
+      /**
+       * \brief Set the direct  for the left and right wheels.
+       * \param leftWheel pwm in the range [-1, 1]
+       * \param rightWheel pwm in the range [-1, 1]
+       * \return true if successful, false otherwise
+       */
+      bool driveWheelsPwm(const float& leftWheel, const float& rightWheel);
+
+      /**
+       * \brief Set the forward and angular velocity of Create.
        * \param xVel in m/s
        * \param angularVel in rads/s
        * \return true if successful, false otherwise
        */
-      bool drive(const float& xVel, const float& angularVel) const;
+      bool drive(const float& xVel, const float& angularVel);
 
-      /* Set the power to the side brush motor.
+      /**
+       * \brief Set the power to the side brush motor.
        * \param power is in the range [-1, 1]
        * \return true if successful, false otherwise
        */
       bool setSideMotor(const float& power);
 
-      /* Set the power to the main brush motor.
+      /**
+       * \brief Set the power to the main brush motor.
        * \param power is in the range [-1, 1]
        * \return true if successful, false otherwise
        */
       bool setMainMotor(const float& power);
 
-      /* Set the power to the vacuum motor.
+      /**
+       * \brief Set the power to the vacuum motor.
        * \param power is in the range [0, 1]
        * \return true if successful, false otherwise
        */
       bool setVacuumMotor(const float& power);
 
-      /* Set the power of all motors.
+      /**
+       * \brief Set the power of all motors.
        * \param mainPower in the range [-1, 1]
        * \param sidePower in the range [-1, 1]
        * \param vacuumPower in the range [0, 1]
@@ -187,55 +256,65 @@ namespace create {
        */
       bool setAllMotors(const float& mainPower, const float& sidePower, const float& vacuumPower);
 
-      /* Set the blue "debris" LED on/off.
+      /**
+       * \brief Set the blue "debris" LED on/off.
        * \param enable
        * \return true if successful, false otherwise
        */
       bool enableDebrisLED(const bool& enable);
 
-      /* Set the green "spot" LED on/off.
+      /**
+       * \brief Set the green "spot" LED on/off.
        * \param enable
        * \return true if successful, false otherwise
        */
       bool enableSpotLED(const bool& enable);
 
-      /* Set the green "dock" LED on/off.
+      /**
+       * \brief Set the green "dock" LED on/off.
        * \param enable
        * \return true if successful, false otherwise
        */
       bool enableDockLED(const bool& enable);
 
-      /* Set the orange "check Create" LED on/off.
+      /**
+       * \brief Set the orange "check Create" LED on/off.
        * \param enable
        * \return true if successful, false otherwise
        */
       bool enableCheckRobotLED(const bool& enable);
 
-      /* Set the center power LED.
+      /**
+       * \brief Set the center power LED.
        * \param power in range [0, 255] where 0 = green and 255 = red
        * \param intensity in range [0, 255]
        * \return true if successful, false otherwise
        */
       bool setPowerLED(const uint8_t& power, const uint8_t& intensity = 255);
 
-      /* Set the four 7-segment display digits from left to right.
+      /**
+       * \brief Set the four 7-segment display digits from left to right.
+       *
+       * \todo This function is not yet implemented refer to https://github.com/AutonomyLab/libcreate/issues/7
        * \param segments to enable (true) or disable (false).
        *        The size of segments should be less than 29.
        *        The ordering of segments is left to right, top to bottom for each digit:
        *
-       *            0           7             14            21
-       *          |‾‾‾|       |‾‾‾|         |‾‾‾|         |‾‾‾|
-       *        1 |___| 2   8 |___| 9    15 |___| 16   22 |___| 23
-       *          | 3 |       | 10|         | 17|         | 24|
-       *        4 |___| 5   11|___| 12   18 |___| 19   25 |___| 26
-       *            6           13            20            27
+       *     <pre>
+                 0           7             14            21
+               |‾‾‾|       |‾‾‾|         |‾‾‾|         |‾‾‾|
+             1 |___| 2   8 |___| 9    15 |___| 16   22 |___| 23
+               | 3 |       | 10|         | 17|         | 24|
+             4 |___| 5   11|___| 12   18 |___| 19   25 |___| 26
+                 6           13            20            27
+            </pre>
        *
        * \return true if successful, false otherwise
        */
-      //TODO (https://github.com/AutonomyLab/libcreate/issues/7)
-      //bool setDigits(const std::vector<bool>& segments) const;
+      bool setDigits(const std::vector<bool>& segments) const;
 
-      /* Set the four 7-segment display digits from left to right with ASCII codes.
+      /**
+       * \brief Set the four 7-segment display digits from left to right with ASCII codes.
        * Any code out side the accepted ascii ranges results in blank display.
        * \param digit1 is left most digit with ascii range [32, 126]
        * \param digit2 is second to left digit with ascii range [32, 126]
@@ -246,7 +325,8 @@ namespace create {
       bool setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
                           const uint8_t& digit3, const uint8_t& digit4) const;
 
-      /* Defines a song from the provided notes and labels it with a song number.
+      /**
+       * \brief Defines a song from the provided notes and labels it with a song number.
        * \param songNumber can be one of four possible song slots, [0, 4]
        * \param songLength is the number of notes, maximum 16.
        *        length(notes) = length(durations) = songLength should be true.
@@ -260,208 +340,383 @@ namespace create {
                       const uint8_t* notes,
                       const float* durations) const;
 
-      /* Play a previously created song.
+      /**
+       * \brief Play a previously created song.
        * This command will not work if a song was not already defined with the specified song number.
        * \param songNumber is one of four stored songs in the range [0, 4]
        * \return true if successful, false otherwise
        */
       bool playSong(const uint8_t& songNumber) const;
 
-      /* True if a left or right wheeldrop is detected.
+      /**
+       * \brief Set dtHistoryLength parameter.
+       * Used to configure the size of the buffer for calculating average time delta (dt).
+       * between onData calls, which in turn is used for velocity calculation.
+       * \param dtHistoryLength number of historical samples to use for calculating average dt.
+       */
+      void setDtHistoryLength(const uint8_t& dtHistoryLength);
+
+      /**
+       * \return true if a left or right wheeldrop is detected, false otherwise.
        */
       bool isWheeldrop() const;
 
-      /* Returns true if left bumper is pressed, false otherwise.
+      /**
+       * \return true if a left wheeldrop is detected, false otherwise.
+       */
+      bool isLeftWheeldrop() const;
+
+      /**
+       * \return true if a right wheeldrop is detected, false otherwise.
+       */
+      bool isRightWheeldrop() const;
+
+      /**
+       * \return true if left bumper is pressed, false otherwise.
        */
       bool isLeftBumper() const;
 
-      /* Returns true if right bumper is pressed, false otherwise.
+      /**
+       * \return true if right bumper is pressed, false otherwise.
        */
       bool isRightBumper() const;
 
-      /* True if wall is seen to right of Create, false otherwise.
+      /**
+       * \return true if wall is seen to right of Create, false otherwise.
        */
       bool isWall() const;
 
-      /* True if there are any cliff detections, false otherwise.
+      /**
+       * \return true if there are any cliff detections, false otherwise.
        */
       bool isCliff() const;
 
-      /* True if there is a virtual wall signal is being received.
+      /**
+       * \return true if the left sensor detects a cliff, false otherwise.
+       */
+      bool isCliffLeft() const;
+
+      /**
+       * \return true if the front left sensor detects a cliff, false otherwise.
+       */
+      bool isCliffFrontLeft() const;
+
+      /**
+       * \return true if the right sensor detects a cliff, false otherwise.
+       */
+      bool isCliffRight() const;
+
+      /**
+       * \return true if the front right sensor detects a cliff, false otherwise.
+       */
+      bool isCliffFrontRight() const;
+
+      /**
+       * \return the IR value of the left cliff sensor.
+       */
+      uint16_t getCliffSignalLeft() const;
+
+      /**
+       * \return the IR value of the front left cliff sensor.
+       */
+      uint16_t getCliffSignalFrontLeft() const;
+
+      /**
+       * \return the IR value of the right cliff sensor.
+       */
+      uint16_t getCliffSignalRight() const;
+
+      /**
+       * \return the IR value of the front right cliff sensor.
+       */
+      uint16_t getCliffSignalFrontRight() const;
+
+      /**
+       * \return true if there is a virtual wall signal is being received.
        */
       bool isVirtualWall() const;
 
-      //TODO (https://github.com/AutonomyLab/libcreate/issues/8)
-      //bool isWheelOvercurrent() const;
+      /**
+       * \todo Not implemented yet (https://github.com/AutonomyLab/libcreate/issues/8)
+       * \return true if drive motors are overcurrent.
+       */
+      bool isWheelOvercurrent() const;
 
-      //TODO (https://github.com/AutonomyLab/libcreate/issues/8)
-      //bool isMainBrushOvercurrent() const;
+      /**
+       * \todo Not implemented yet (https://github.com/AutonomyLab/libcreate/issues/8)
+       * \return true if main brush motor is overcurrent.
+       */
+      bool isMainBrushOvercurrent() const;
 
-      //TODO (https://github.com/AutonomyLab/libcreate/issues/8)
-      //bool isSideBrushOvercurrent() const;
+      /**
+       * \todo Not implemented yet (https://github.com/AutonomyLab/libcreate/issues/8)
+       * \return true if side brush motor is overcurrent.
+       */
+      bool isSideBrushOvercurrent() const;
 
-      /* Get level of the dirt detect sensor.
+      /**
+       * \brief Get level of the dirt detect sensor.
        * \return value in range [0, 255]
        */
       uint8_t getDirtDetect() const;
 
-      /* Get value of 8-bit IR character currently being received by omnidirectional sensor.
+      /**
+       * \brief Get value of 8-bit IR character currently being received by omnidirectional sensor.
        * \return value in range [0, 255]
        */
       uint8_t getIROmni() const;
 
-      /* Get value of 8-bit IR character currently being received by left sensor.
+      /**
+       * \brief Get value of 8-bit IR character currently being received by left sensor.
        * \return value in range [0, 255]
        */
 
       uint8_t getIRLeft() const;
 
-      /* Get value of 8-bit IR character currently being received by right sensor.
+      /**
+       * \brief Get value of 8-bit IR character currently being received by right sensor.
        * \return value in range [0, 255]
        */
       uint8_t getIRRight() const;
 
-      /* Get state of 'clean' button ('play' button on Create 1).
+      /**
+       * \brief Get state of 'clean' button ('play' button on Create 1).
+       * \return true if button is pressed, false otherwise.
        */
       bool isCleanButtonPressed() const;
 
-      /* Not supported by any firmware!
+      /**
+       * \brief Not supported by any firmware!
        */
       bool isClockButtonPressed() const;
 
-      /* Not supported by any firmware!
+      /**
+       * \brief Not supported by any firmware!
        */
       bool isScheduleButtonPressed() const;
 
-      /* Get state of 'day' button.
+      /**
+       * \brief Get state of 'day' button.
+       * \return true if button is pressed, false otherwise.
        */
       bool isDayButtonPressed() const;
 
-      /* Get state of 'hour' button.
+      /**
+       * \brief Get state of 'hour' button.
+       * \return true if button is pressed, false otherwise.
        */
       bool isHourButtonPressed() const;
 
-      /* Get state of 'min' button.
+      /**
+       * \brief Get state of 'min' button.
+       * \return true if button is pressed, false otherwise.
        */
       bool isMinButtonPressed() const;
 
-      /* Get state of 'dock' button ('advance' button on Create 1).
+      /**
+       * \brief Get state of 'dock' button ('advance' button on Create 1).
+       * \return true if button is pressed, false otherwise.
        */
       bool isDockButtonPressed() const;
 
-      /* Get state of 'spot' button.
+      /**
+       * \brief Get state of 'spot' button.
+       * \return true if button is pressed, false otherwise.
        */
       bool isSpotButtonPressed() const;
 
-      /* Get battery voltage.
+      /**
+       * \brief Get battery voltage.
        * \return value in volts
        */
       float getVoltage() const;
 
-      /* Get current flowing in/out of battery.
+      /**
+       * \brief Get current flowing in/out of battery.
        * A positive current implies Create is charging.
        * \return value in amps
        */
       float getCurrent() const;
 
-      /* Get the temperature of battery.
+      /**
+       * \brief Get the temperature of battery.
        * \return value in Celsius
        */
       int8_t getTemperature() const;
 
-      /* Get battery's remaining charge.
+      /**
+       * \brief Get battery's remaining charge.
        * \return value in amp-hours
        */
       float getBatteryCharge() const;
 
-      /* Get estimated battery charge capacity.
+      /**
+       * \brief Get estimated battery charge capacity.
        * \return in amp-hours
        */
       float getBatteryCapacity() const;
 
-      /* Return true if farthest left light sensor detects an obstacle, false otherwise.
+      /**
+       * \return true if farthest left light sensor detects an obstacle, false otherwise.
        */
       bool isLightBumperLeft() const;
 
-      /* Return true if front left light sensor detects an obstacle, false otherwise.
+      /**
+       * \return true if front left light sensor detects an obstacle, false otherwise.
        */
       bool isLightBumperFrontLeft() const;
 
-      /* Return true if center left light sensor detects an obstacle, false otherwise.
+      /**
+       * \return true if center left light sensor detects an obstacle, false otherwise.
        */
       bool isLightBumperCenterLeft() const;
 
-      /* Return true if farthest right light sensor detects an obstacle, false otherwise.
+      /**
+       * \return true if farthest right light sensor detects an obstacle, false otherwise.
        */
       bool isLightBumperRight() const;
 
-      /* Return true if front right light sensor detects an obstacle, false otherwise.
+      /**
+       * \return true if front right light sensor detects an obstacle, false otherwise.
        */
       bool isLightBumperFrontRight() const;
 
-      /* Return true if center right light sensor detects an obstacle, false otherwise.
+      /**
+       * \return true if center right light sensor detects an obstacle, false otherwise.
        */
       bool isLightBumperCenterRight() const;
 
-      /* Return the signal strength from the left light sensor.
+      /**
+       * \brief Get the signal strength from the left light sensor.
        * \return value in range [0, 4095]
        */
       uint16_t getLightSignalLeft() const;
 
-      /* Return the signal strength from the front-left light sensor.
+      /**
+       * \brief Get the signal strength from the front-left light sensor.
        * \return value in range [0, 4095]
        */
       uint16_t getLightSignalFrontLeft() const;
 
-      /* Return the signal strength from the center-left light sensor.
+      /**
+       * \brief Get the signal strength from the center-left light sensor.
        * \return value in range [0, 4095]
        */
       uint16_t getLightSignalCenterLeft() const;
 
-      /* Return the signal strength from the right light sensor.
+      /**
+       * \brief Get the signal strength from the right light sensor.
        * \return value in range [0, 4095]
        */
       uint16_t getLightSignalRight() const;
 
-      /* Return the signal strength from the front-right light sensor.
+      /**
+       * \brief Get the signal strength from the front-right light sensor.
        * \return value in range [0, 4095]
        */
       uint16_t getLightSignalFrontRight() const;
 
-      /* Return the signal strength from the center-right light sensor.
+      /**
+       * \brief Get the signal strength from the center-right light sensor.
        * \return value in range [0, 4095]
        */
       uint16_t getLightSignalCenterRight() const;
 
-      /* Return true if Create is moving forward, false otherwise.
+      /**
+       * \return true if Create is moving forward, false otherwise.
        */
       bool isMovingForward() const;
 
-      /* Get the current charging state.
+      /**
+       * \brief Get the total distance the left wheel has moved.
+       * \return distance in meters.
+       */
+      float getLeftWheelDistance() const;
+
+      /**
+       * \brief Get the total distance the right wheel has moved.
+       * \return distance in meters.
+       */
+      float getRightWheelDistance() const;
+
+      /**
+       * \brief Get the measured velocity of the left wheel.
+       * \return velocity in m/s
+       */
+      float getMeasuredLeftWheelVel() const;
+
+      /**
+       * \brief Get the measured velocity of the right wheel.
+       * \return velocity in m/s
+       */
+      float getMeasuredRightWheelVel() const;
+
+      /**
+       * \brief Get the requested velocity of the left wheel.
+       * This value is bounded at the maximum velocity of the robot model.
+       * \return requested velocity in m/s.
+       */
+      float getRequestedLeftWheelVel() const;
+
+      /**
+       * \brief Get the requested velocity of the right wheel.
+       * This value is bounded at the maximum velocity of the robot model.
+       * \return requested velocity in m/s.
+       */
+      float getRequestedRightWheelVel() const;
+
+      /**
+       * \brief Get the current charging state.
+       * \return charging state.
        */
       create::ChargingState getChargingState() const;
 
-      /* Get the current mode reported by Create.
+      /**
+       * \brief Get the current mode reported by Create.
+       * \return mode.
        */
-      create::CreateMode getMode() const;
+      create::CreateMode getMode();
 
-      /* Get the estimated position of Create based on wheel encoders.
+      /**
+       * \brief Get the estimated pose of Create based on wheel encoders.
+       * \return pose (x-y position in meters and yaw angle in Radians)
        */
-      const create::Pose& getPose() const;
+      create::Pose getPose() const;
 
-      /* Get the estimated velocity of Create based on wheel encoders.
+      /**
+       * \brief Get the estimated velocity of Create based on wheel encoders.
+       * \return velocity (x and y in m/s and angular velocity in Radians/s)
        */
-      const create::Vel& getVel() const;
+      create::Vel getVel() const;
 
-      /* Get the number of corrupt serial packets since first connecting to Create.
+      /**
+       * \brief Get the number of corrupt serial packets since first connecting to Create.
+       * This value is ideally zero. If the number is consistently increasing then
+       * chances are some sensor information is not being updated.
+       * \return number of corrupt packets.
        */
       uint64_t getNumCorruptPackets() const;
 
-      /* Get the total number of serial packets (including corrupt packets) since first connecting to Create.
+      /**
+       * \brief Get the total number of serial packets received (including corrupt packets) since first connecting to 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
-
 #endif  // CREATE_DRIVER_H

include/create/data.h

@@ -32,9 +32,8 @@ POSSIBILITY OF SUCH DAMAGE.
 #ifndef CREATE_DATA_H
 #define CREATE_DATA_H
 
-#include <boost/shared_ptr.hpp>
-#include <boost/make_shared.hpp>
 #include <map>
+#include <memory>
 #include <vector>
 
 #include "create/packet.h"
@@ -43,16 +42,16 @@ POSSIBILITY OF SUCH DAMAGE.
 namespace create {
   class Data {
     private:
-      std::map<uint8_t, boost::shared_ptr<Packet> > packets;
+      std::map<uint8_t, std::shared_ptr<Packet> > packets;
       uint32_t totalDataBytes;
       std::vector<uint8_t> ids;
 
     public:
-      Data(RobotModel = CREATE_2);
+      Data(ProtocolVersion version = V_3);
       ~Data();
 
       bool isValidPacketID(const uint8_t id) const;
-      boost::shared_ptr<Packet> getPacket(const uint8_t id);
+      std::shared_ptr<Packet> getPacket(const uint8_t id);
       void validateAll();
       uint32_t getTotalDataBytes() const;
       uint8_t getNumPackets() const;

include/create/packet.h

@@ -31,15 +31,20 @@ POSSIBILITY OF SUCH DAMAGE.
 #ifndef CREATE_PACKET_H
 #define CREATE_PACKET_H
 
-#include <boost/thread/mutex.hpp>
+#include <mutex>
+#include <string>
 
 namespace create {
   class Packet {
     private:
       uint16_t data;
       uint16_t tmpData;
-      mutable boost::mutex dataMutex;
-      mutable boost::mutex tmpDataMutex;
+      mutable std::mutex dataMutex;
+      mutable std::mutex tmpDataMutex;
+
+    protected:
+      // Thread safe
+      void setData(const uint16_t& d);
 
     public:
       const uint8_t nbytes;
@@ -48,10 +53,11 @@ namespace create {
       Packet(const uint8_t& nbytes, const std::string& info);
       ~Packet();
 
-      // All of the following are thread safe
-      void setTempData(const uint16_t& td);
+      // Thread safe
+      void setDataToValidate(const uint16_t& td);
+      // Thread safe
       void validate();
-      void setData(const uint16_t& d);
+      // Thread safe
       uint16_t getData() const;
   };
 

include/create/serial.h

@@ -35,66 +35,62 @@ POSSIBILITY OF SUCH DAMAGE.
 #ifndef CREATE_SERIAL_H
 #define CREATE_SERIAL_H
 
+#include <condition_variable>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <thread>
+
 #include <boost/asio.hpp>
-#include <boost/thread.hpp>
-#include <boost/thread/condition_variable.hpp>
-#include <boost/function.hpp>
-#include <boost/shared_ptr.hpp>
 
 #include "create/data.h"
 #include "create/types.h"
 #include "create/util.h"
 
 namespace create {
-  class Serial {
-    private:
-      enum ReadState {
-        READ_HEADER,
-        READ_NBYTES,
-        READ_PACKET_ID,
-        READ_PACKET_BYTES,
-        READ_CHECKSUM
-      };
+  class Serial : public std::enable_shared_from_this<Serial> {
 
-      boost::shared_ptr<Data> data;
+    protected:
       boost::asio::io_service io;
+      boost::asio::signal_set signals;
       boost::asio::serial_port port;
-      boost::thread ioThread;
-      boost::condition_variable dataReadyCond;
-      boost::mutex dataReadyMut;
-      ReadState readState;
+
+    private:
+      std::thread ioThread;
+      std::condition_variable dataReadyCond;
+      std::mutex dataReadyMut;
       bool dataReady;
       bool isReading;
       bool firstRead;
-
-      // These are for possible diagnostics
-      uint64_t corruptPackets;
-      uint64_t totalPackets;
-      // State machine variables
-      uint8_t headerByte;
-      uint8_t packetID;
-      uint8_t expectedNumBytes;
       uint8_t byteRead;
-      uint16_t packetBytes;
-      uint8_t numBytesRead;
-      uint32_t byteSum;
-      uint8_t numDataBytesRead;
-      uint8_t expectedNumDataBytes;
 
       // Callback executed when data arrives from Create
       void onData(const boost::system::error_code& e, const std::size_t& size);
       // Callback to execute once data arrives
-      boost::function<void()> callback;
+      std::function<void()> callback;
       // Start and stop reading data from Create
       bool startReading();
       void stopReading();
+      bool openPort(const std::string& portName, const int& baud);
+      bool closePort();
+
+    protected:
+      std::shared_ptr<Data> data;
+      // These are for possible diagnostics
+      uint64_t corruptPackets;
+      uint64_t totalPackets;
+
+      virtual bool startSensorStream() = 0;
+      virtual void processByte(uint8_t byteRead) = 0;
+
+      void signalHandler(const boost::system::error_code& error, int signal_number);
       // Notifies main thread that data is fresh and makes the user callback
       void notifyDataReady();
 
     public:
-      Serial(boost::shared_ptr<Data> data, const uint8_t& header = create::util::CREATE_2_HEADER);
+      Serial(std::shared_ptr<Data> data, bool install_signal_handler);
       ~Serial();
-      bool connect(const std::string& port, const int& baud = 115200, boost::function<void()> cb = 0);
+      bool connect(const std::string& port, const int& baud = 115200, std::function<void()> cb = 0);
       void disconnect();
       inline bool connected() const { return port.is_open(); };
       bool send(const uint8_t* bytes, const uint32_t numBytes);

include/create/serial_query.h

@@ -0,0 +1,75 @@
+/**
+Software License Agreement (BSD)
+
+\file      serial_query.h
+\authors   Jacob Perron <jperron@sfu.ca>
+\authors   Ben Wolsieffer <benwolsieffer@gmail.com>
+\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+
+// Based on example from:
+//   https://github.com/labust/labust-ros-pkg/wiki/Create-a-Serial-Port-application
+
+#ifndef CREATE_SERIAL_QUERY_H
+#define CREATE_SERIAL_QUERY_H
+
+#include <memory>
+
+#include <boost/asio.hpp>
+
+#include "create/data.h"
+#include "create/types.h"
+#include "create/util.h"
+#include "create/serial.h"
+
+namespace create {
+  class SerialQuery : public Serial {
+
+    private:
+      boost::asio::deadline_timer streamRecoveryTimer;
+      uint8_t packetID;
+      int8_t packetByte;
+      uint16_t packetData;
+      const uint8_t maxPacketID;
+
+      bool started;
+
+      void requestSensorData();
+      void restartSensorStream(const boost::system::error_code& err);
+
+      void flushInput();
+
+    protected:
+      bool startSensorStream();
+      void processByte(uint8_t byteRead);
+
+    public:
+      SerialQuery(std::shared_ptr<Data> data, bool install_signal_handler = true);
+      virtual ~SerialQuery() = default;
+  };
+}  // namespace create
+
+#endif // CREATE_SERIAL_H

include/create/serial_stream.h

@@ -0,0 +1,81 @@
+/**
+Software License Agreement (BSD)
+
+\file      serial_stream.h
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+
+// Based on example from:
+//   https://github.com/labust/labust-ros-pkg/wiki/Create-a-Serial-Port-application
+
+#ifndef CREATE_SERIAL_STREAM_H
+#define CREATE_SERIAL_STREAM_H
+
+#include <memory>
+
+#include "create/data.h"
+#include "create/types.h"
+#include "create/util.h"
+#include "create/serial.h"
+
+namespace create {
+  class SerialStream : public Serial {
+    private:
+      enum ReadState {
+        READ_HEADER,
+        READ_NBYTES,
+        READ_PACKET_ID,
+        READ_PACKET_BYTES,
+        READ_CHECKSUM
+      };
+
+      // State machine variables
+      ReadState readState;
+      uint8_t headerByte;
+      uint8_t packetID;
+      uint8_t expectedNumBytes;
+      uint16_t packetBytes;
+      uint8_t numBytesRead;
+      uint32_t byteSum;
+      uint8_t numDataBytesRead;
+      uint8_t expectedNumDataBytes;
+
+    protected:
+      bool startSensorStream();
+      void processByte(uint8_t byteRead);
+
+    public:
+      SerialStream(
+        std::shared_ptr<Data> data,
+        const uint8_t& header = create::util::STREAM_HEADER,
+        bool install_signal_handler = true);
+      virtual ~SerialStream() = default;
+
+  };
+}  // namespace create
+
+#endif // CREATE_SERIAL_H

include/create/types.h

@@ -32,10 +32,56 @@ POSSIBILITY OF SUCH DAMAGE.
 #ifndef CREATE_TYPES_H
 #define CREATE_TYPES_H
 
+#include <vector>
+#include <stdint.h>
+#include <string>
+#include <stdexcept>
+
 namespace create {
-  enum RobotModel {
-    CREATE_1 = 0, // Roomba 400 series
-    CREATE_2      // Roomba 600 series 
+  enum ProtocolVersion {
+    V_1 = 1,
+    V_2 = 2,
+    V_3 = 4,
+    V_ALL = 0xFFFFFFFF
+  };
+
+  class RobotModel {
+    public:
+      bool operator==(RobotModel& other) const;
+      operator uint32_t() const;
+
+      uint32_t getId() const;
+      ProtocolVersion getVersion() const;
+      float getAxleLength() const;
+      unsigned int getBaud() const;
+      float getMaxVelocity() const;
+      float getWheelDiameter() const;
+
+      /**
+       * \brief Compatible with Roomba 400 series and earlier.
+       */
+      static RobotModel ROOMBA_400;
+
+      /**
+       * \brief Compatible with Create 1 or Roomba 500 series.
+       */
+      static RobotModel CREATE_1;
+
+      /**
+       * \brief Compatible with Create 2 or Roomba 600 series and greater.
+       */
+      static RobotModel CREATE_2;
+
+    private:
+      uint32_t id;
+      ProtocolVersion version;
+      float axleLength;
+      unsigned int baud;
+      float maxVelocity;
+      float wheelDiameter;
+
+      RobotModel(const ProtocolVersion version, const float axleLength, const unsigned int baud, const float maxVelocity = 0.5, const float wheelDiameter = 0.078);
+      static uint32_t nextId;
   };
 
   enum SensorPacketID {
@@ -58,8 +104,8 @@ namespace create {
     ID_CLIFF_RIGHT = 12,
     ID_VIRTUAL_WALL = 13,
     ID_OVERCURRENTS = 14,
-    ID_DIRT_DETECT = 15,
-    ID_UNUSED_1 = 16,
+    ID_DIRT_DETECT_LEFT = 15,
+    ID_DIRT_DETECT_RIGHT = 16,
     ID_IR_OMNI = 17,
     ID_IR_LEFT = 52,
     ID_IR_RIGHT = 53,
@@ -77,8 +123,8 @@ namespace create {
     ID_CLIFF_FRONT_LEFT_SIGNAL = 29,
     ID_CLIFF_FRONT_RIGHT_SIGNAL = 30,
     ID_CLIFF_RIGHT_SIGNAL = 31,
-    ID_UNUSED_2 = 32,
-    ID_UNUSED_3 = 33,
+    ID_CARGO_BAY_DIGITAL_INPUTS = 32,
+    ID_CARGO_BAY_ANALOG_SIGNAL = 33,
     ID_CHARGE_SOURCE = 34,
     ID_OI_MODE = 35,
     ID_SONG_NUM = 36,
@@ -110,6 +156,7 @@ namespace create {
     OC_RESET = 7,
     OC_STOP = 173,
     OC_BAUD = 129,
+    OC_CONTROL = 130,
     OC_SAFE = 131,
     OC_FULL = 132,
     OC_CLEAN = 135,
@@ -134,7 +181,7 @@ namespace create {
     OC_SENSORS= 142,
     OC_QUERY_LIST=149,
     OC_STREAM = 148,
-    OC_TOGGLE_STREAM = 150,
+    OC_TOGGLE_STREAM = 150
   };
 
   enum BAUDCODE {
@@ -153,10 +200,10 @@ namespace create {
   };
 
   enum CreateMode {
-    MODE_OFF = OC_POWER,
-    MODE_PASSIVE = OC_START,
-    MODE_SAFE = OC_SAFE,
-    MODE_FULL = OC_FULL,
+    MODE_OFF = 0,
+    MODE_PASSIVE = 1,
+    MODE_SAFE = 2,
+    MODE_FULL = 3,
     MODE_UNAVAILABLE = -1
   };
 
@@ -222,10 +269,18 @@ namespace create {
     IR_CHAR_VIRTUAL_WALL = 162
   };
 
+  /**
+   * \brief Represents a robot pose.
+   */
   struct Pose {
     float x;
     float y;
     float yaw;
+
+    /**
+     * \brief 3x3 covariance matrix in row-major order.
+     */
+    std::vector<float> covariance;
   };
 
   typedef Pose Vel;

include/create/util.h

@@ -32,7 +32,7 @@ POSSIBILITY OF SUCH DAMAGE.
 #ifndef CREATE_UTIL_H
 #define CREATE_UTIL_H
 
-#include <sys/time.h>
+#include <cmath>
 
 #define COUT(prefix,msg) (std::cout<<prefix<<msg<<std::endl)
 #define CERR(prefix,msg) (std::cerr<<prefix<<msg<<std::endl)
@@ -40,15 +40,12 @@ POSSIBILITY OF SUCH DAMAGE.
 namespace create {
   namespace util {
 
-    static const uint8_t CREATE_2_HEADER = 19;
-    static const float CREATE_2_AXLE_LENGTH = 0.235;
-    static const float CREATE_2_TICKS_PER_REV = 508.8;
-    static const uint32_t CREATE_2_MAX_ENCODER_TICKS = 65535;
-    static const float CREATE_2_WHEEL_DIAMETER = 0.078;
-    static const float CREATE_2_MAX_VEL = 0.5;
-    static const float CREATE_2_MAX_RADIUS = 2.0;
-    static const float CREATE_2_STRAIGHT_RADIUS = 32.768;
-    static const float CREATE_2_IN_PLACE_RADUIS = 0.001;
+    static const uint8_t STREAM_HEADER = 19;
+    static const float V_3_TICKS_PER_REV = 508.8;
+    static const uint32_t V_3_MAX_ENCODER_TICKS = 65535;
+    static const float MAX_RADIUS = 2.0;
+    static const float STRAIGHT_RADIUS = 32.768;
+    static const float IN_PLACE_RADIUS = 0.001;
     static const float PI = 3.14159;
     static const float TWO_PI = 6.28318;
     static const float EPS = 0.0001;
@@ -58,16 +55,10 @@ namespace create {
       while (a < -PI) a += TWO_PI;
       while (a > PI) a -= TWO_PI;
       return a;
-    };
+    }
 
-    typedef unsigned long long timestamp_t;
-
-    /** Get a timestamp for the current time in micro-seconds.
-     */
-    static timestamp_t getTimestamp() {
-      struct timeval now;
-      gettimeofday(&now, NULL);
-      return now.tv_usec + (timestamp_t) now.tv_sec * 1000000;
+    inline bool willFloatOverflow(const float a, const float b) {
+      return ( (a < 0.0) == (b < 0.0) && std::abs(b) > std::numeric_limits<float>::max() - std::abs(a) );
     }
   }  // namespace util
 } // namespace create

mainpage.dox

@@ -0,0 +1,7 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b libcreate is a C++ library for interfacing with iRobot's Create mobile robot platforms and various Roomba models.
+
+*/

package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>libcreate</name>
+  <version>3.1.0</version>
+  <description>C++ library for interfacing with iRobot's Create 1 and Create 2</description>
+
+  <maintainer email="jacobmperron@gmail.com">Jacob Perron</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/libcreate</url>
+  <url type="repository">https://github.com/AutonomyLab/libcreate</url>
+  <url type="bugtracker">https://github.com/AutonomyLab/libcreate/issues</url>
+
+  <author email="jacobmperron@gmail.com">Jacob Perron</author>
+
+  <buildtool_depend>cmake</buildtool_depend>
+
+  <depend>boost</depend>
+
+  <exec_depend condition="$ROS_VERSION == 1">catkin</exec_depend>
+
+  <doc_depend>doxygen</doc_depend>
+
+  <test_depend>gtest</test_depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+</package>

src/create.cpp

@@ -1,20 +1,20 @@
-#include <boost/bind.hpp>
-#include <boost/make_shared.hpp>
 #include <iostream>
 #include <cmath>
 #include <ctime>
+#include <memory>
 #include <assert.h>
 
 #include "create/create.h"
 
 #define GET_DATA(id) (data->getPacket(id)->getData())
-#define BOUND(val,min,max) (val<min?val=min:(val>max?val=max:val=val))
+#define BOUND_CONST(val,min,max) (val<min?min:(val>max?max:val))
+#define BOUND(val,min,max) (val = BOUND_CONST(val,min,max))
 
 namespace create {
 
-  // TODO: Handle SIGINT to do clean disconnect
+  namespace ublas = boost::numeric::ublas;
 
-  void Create::init() {
+  void Create::init(bool install_signal_handler) {
     mainMotorPower = 0;
     sideMotorPower = 0;
     vacuumMotorPower = 0;
@@ -26,23 +26,40 @@ namespace create {
     powerLEDIntensity = 0;
     prevTicksLeft = 0;
     prevTicksRight = 0;
+    totalLeftDist = 0.0;
+    totalRightDist = 0.0;
     firstOnData = true;
+    mode = MODE_OFF;
     pose.x = 0;
     pose.y = 0;
     pose.yaw = 0;
+    pose.covariance = std::vector<float>(9, 0.0);
     vel.x = 0;
     vel.y = 0;
     vel.yaw = 0;
-    data = boost::shared_ptr<Data>(new Data(model));
-    serial = boost::make_shared<Serial>(data);
+    vel.covariance = std::vector<float>(9, 0.0);
+    poseCovar = Matrix(3, 3, 0.0);
+    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);
+    } else {
+      serial = std::make_shared<SerialStream>(
+        data, create::util::STREAM_HEADER, install_signal_handler);
+    }
   }
 
-  Create::Create(RobotModel m) : model(m) {
-    init();
+  Create::Create(RobotModel m, bool install_signal_handler) : model(m) {
+    init(install_signal_handler);
   }
 
-  Create::Create(const std::string& dev, const int& baud, RobotModel m) : model(m) {
-    init();
+  Create::Create(const std::string& dev, const int& baud, RobotModel m, bool install_signal_handler)
+    : model(m)
+  {
+    init(install_signal_handler);
     serial->connect(dev, baud);
   }
 
@@ -50,80 +67,235 @@ namespace create {
     disconnect();
   }
 
+  void Create::reset() {
+    if (!connected()) {
+      CERR("[create::Serial] ", "send failed, not connected.");
+      return;
+    }
+    serial->sendOpcode(OC_START);
+    serial->sendOpcode(OC_RESET);
+  }
+
+  Create::Matrix Create::addMatrices(const Matrix &A, const Matrix &B) const {
+    size_t rows = A.size1();
+    size_t cols = A.size2();
+
+    assert(rows == B.size1());
+    assert(cols == B.size2());
+
+    Matrix C(rows, cols);
+    for (size_t i = 0u; i < rows; i++) {
+      for (size_t j = 0u; j < cols; j++) {
+        const float a = A(i, j);
+        const float b = B(i, j);
+        if (util::willFloatOverflow(a, b)) {
+          // If overflow, set to float min or max depending on direction of overflow
+          C(i, j) = (a < 0.0) ? std::numeric_limits<float>::min() : std::numeric_limits<float>::max();
+        }
+        else {
+          C(i, j) = a + b;
+        }
+      }
+    }
+    return C;
+  }
+
   void Create::onData() {
     if (firstOnData) {
-      if (model == CREATE_2) {
+      if (model.getVersion() >= V_3) {
         // Initialize tick counts
         prevTicksLeft = GET_DATA(ID_LEFT_ENC);
         prevTicksRight = GET_DATA(ID_RIGHT_ENC);
       }
-      prevOnDataTime = util::getTimestamp();
+      prevOnDataTime = std::chrono::steady_clock::now();
       firstOnData = false;
     }
 
     // Get current time
-    util::timestamp_t curTime = util::getTimestamp();
-    float dt = (curTime - prevOnDataTime) / 1000000.0;
-    float deltaDist, deltaX, deltaY, deltaYaw;
-    if (model == CREATE_1) {
-      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) );
+    auto curTime = std::chrono::steady_clock::now();
+    float dt = static_cast<std::chrono::duration<float>>(curTime - prevOnDataTime).count();
+    float deltaDist = 0.0f;
+    float deltaX = 0.0f;
+    float deltaY = 0.0f;
+    float deltaYaw = 0.0f;
+    float leftWheelDist = 0.0f;
+    float rightWheelDist = 0.0f;
+    float wheelDistDiff = 0.0f;
+
+    // Protocol versions 1 and 2 use distance and angle fields for odometry
+    int16_t angleField = 0;
+    if (model.getVersion() <= V_2) {
+      // This is a standards compliant way of doing unsigned to signed conversion
+      uint16_t distanceRaw = GET_DATA(ID_DISTANCE);
+      int16_t distance;
+      std::memcpy(&distance, &distanceRaw, sizeof(distance));
+      deltaDist = distance / 1000.0; // mm -> m
+
+      // Angle is processed differently in versions 1 and 2
+      uint16_t angleRaw = GET_DATA(ID_ANGLE);
+      std::memcpy(&angleField, &angleRaw, sizeof(angleField));
     }
-    else if (model == CREATE_2) {
+
+    if (model.getVersion() == V_1) {
+      wheelDistDiff = 2.0 * angleField / 1000.0;
+      leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
+      rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
+      deltaYaw = wheelDistDiff / model.getAxleLength();
+    } else if (model.getVersion() == V_2) {
+      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+       * Certain older Creates have major problems with odometry                   *
+       * http://answers.ros.org/question/31935/createroomba-odometry/              *
+       *                                                                           *
+       * All Creates have an issue with rounding of the angle field, which causes  *
+       * major errors to accumulate in the odometry yaw.                           *
+       * http://wiki.tekkotsu.org/index.php/Create_Odometry_Bug                    *
+       * https://github.com/AutonomyLab/create_autonomy/issues/28                  *
+       *                                                                           *
+       * TODO: Consider using velocity command as substitute for pose estimation   *
+       * to mitigate both of these problems.                                       *
+       * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+      deltaYaw = angleField * (util::PI / 180.0); // D2R
+      wheelDistDiff = model.getAxleLength() * deltaYaw;
+      leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
+      rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
+    } else if (model.getVersion() >= V_3) {
       // Get cumulative ticks (wraps around at 65535)
       uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
       uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
       // Compute ticks since last update
       int ticksLeft = totalTicksLeft - prevTicksLeft;
       int ticksRight = totalTicksRight - prevTicksRight;
+
+    // Handle wrap around
+    if (ticksLeft > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksLeft -= util::V_3_MAX_ENCODER_TICKS;
+    } else if (ticksLeft < -0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksLeft += util::V_3_MAX_ENCODER_TICKS;
+    }
+
+    if (ticksRight > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksRight -= util::V_3_MAX_ENCODER_TICKS;
+    } else if (ticksRight < -0.9 * util::V_3_MAX_ENCODER_TICKS) {
+        ticksRight += util::V_3_MAX_ENCODER_TICKS;
+    }
+
       prevTicksLeft = totalTicksLeft;
       prevTicksRight = totalTicksRight;
 
-      // Handle wrap around
-      if (fabs(ticksLeft) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
-        ticksLeft = (ticksLeft % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
-      }
-      if (fabs(ticksRight) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
-        ticksRight = (ticksRight % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
-      }
-
       // Compute distance travelled by each wheel
-      float leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV)
-                             * util::CREATE_2_WHEEL_DIAMETER * util::PI;
-      float rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV)
-                              * util::CREATE_2_WHEEL_DIAMETER * util::PI;
-
-      float wheelDistDiff = rightWheelDist - leftWheelDist;
+      leftWheelDist = (ticksLeft / util::V_3_TICKS_PER_REV)
+          * model.getWheelDiameter() * util::PI;
+      rightWheelDist = (ticksRight / util::V_3_TICKS_PER_REV)
+          * model.getWheelDiameter() * util::PI;
       deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
 
+      wheelDistDiff = rightWheelDist - leftWheelDist;
+      deltaYaw = wheelDistDiff / model.getAxleLength();
+    }
+
+    // determine average dt over window
+    dtHistory.push_front(dt);
+
+    if (dtHistory.size() > dtHistoryLength){
+      dtHistory.pop_back();
+    }
+
+    float dtHistorySum = 0;
+    for (auto it = dtHistory.cbegin(); it != dtHistory.cend(); ++it)
+    {
+      dtHistorySum += *it;
+    }
+    auto dtAverage = dtHistorySum / dtHistory.size();
+
+    measuredLeftVel = leftWheelDist / dtAverage;
+    measuredRightVel = rightWheelDist / dtAverage;
+
     // Moving straight
     if (fabs(wheelDistDiff) < util::EPS) {
       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;
+    } else {
+      float turnRadius = (model.getAxleLength() / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
       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
 
-    if (fabs(dt) > util::EPS) {
-      vel.x = deltaX / dt;
-      vel.y = deltaY / dt;
-      vel.yaw = deltaYaw / dt;
-    }
-    else {
+    totalLeftDist += leftWheelDist;
+    totalRightDist += rightWheelDist;
+
+    if (fabs(dtAverage) > util::EPS) {
+      vel.x = deltaDist / dtAverage;
+      vel.y = 0.0;
+      vel.yaw = deltaYaw / dtAverage;
+    } else {
       vel.x = 0.0;
       vel.y = 0.0;
       vel.yaw = 0.0;
     }
 
+    // Update covariances
+    // Ref: "Introduction to Autonomous Mobile Robots" (Siegwart 2004, page 189)
+    float kr = 1.0; // TODO: Perform experiments to find these nondeterministic parameters
+    float kl = 1.0;
+    float cosYawAndHalfDelta = cos(pose.yaw + (deltaYaw / 2.0)); // deltaX?
+    float sinYawAndHalfDelta = sin(pose.yaw + (deltaYaw / 2.0)); // deltaY?
+    float distOverTwoWB = deltaDist / (model.getAxleLength() * 2.0);
+
+    Matrix invCovar(2, 2);
+    invCovar(0, 0) = kr * fabs(rightWheelDist);
+    invCovar(0, 1) = 0.0;
+    invCovar(1, 0) = 0.0;
+    invCovar(1, 1) = kl * fabs(leftWheelDist);
+
+    Matrix Finc(3, 2);
+    Finc(0, 0) = (cosYawAndHalfDelta / 2.0) - (distOverTwoWB * sinYawAndHalfDelta);
+    Finc(0, 1) = (cosYawAndHalfDelta / 2.0) + (distOverTwoWB * sinYawAndHalfDelta);
+    Finc(1, 0) = (sinYawAndHalfDelta / 2.0) + (distOverTwoWB * cosYawAndHalfDelta);
+    Finc(1, 1) = (sinYawAndHalfDelta / 2.0) - (distOverTwoWB * cosYawAndHalfDelta);
+    Finc(2, 0) = (1.0 / model.getAxleLength());
+    Finc(2, 1) = (-1.0 / model.getAxleLength());
+    Matrix FincT = boost::numeric::ublas::trans(Finc);
+
+    Matrix Fp(3, 3);
+    Fp(0, 0) = 1.0;
+    Fp(0, 1) = 0.0;
+    Fp(0, 2) = (-deltaDist) * sinYawAndHalfDelta;
+    Fp(1, 0) = 0.0;
+    Fp(1, 1) = 1.0;
+    Fp(1, 2) = deltaDist * cosYawAndHalfDelta;
+    Fp(2, 0) = 0.0;
+    Fp(2, 1) = 0.0;
+    Fp(2, 2) = 1.0;
+    Matrix FpT = boost::numeric::ublas::trans(Fp);
+
+    Matrix velCovar = ublas::prod(invCovar, FincT);
+    velCovar = ublas::prod(Finc, velCovar);
+
+    vel.covariance[0] = velCovar(0, 0);
+    vel.covariance[1] = velCovar(0, 1);
+    vel.covariance[2] = velCovar(0, 2);
+    vel.covariance[3] = velCovar(1, 0);
+    vel.covariance[4] = velCovar(1, 1);
+    vel.covariance[5] = velCovar(1, 2);
+    vel.covariance[6] = velCovar(2, 0);
+    vel.covariance[7] = velCovar(2, 1);
+    vel.covariance[8] = velCovar(2, 2);
+
+    Matrix poseCovarTmp = ublas::prod(poseCovar, FpT);
+    poseCovarTmp = ublas::prod(Fp, poseCovarTmp);
+    poseCovar = addMatrices(poseCovarTmp, velCovar);
+
+    pose.covariance[0] = poseCovar(0, 0);
+    pose.covariance[1] = poseCovar(0, 1);
+    pose.covariance[2] = poseCovar(0, 2);
+    pose.covariance[3] = poseCovar(1, 0);
+    pose.covariance[4] = poseCovar(1, 1);
+    pose.covariance[5] = poseCovar(1, 2);
+    pose.covariance[6] = poseCovar(2, 0);
+    pose.covariance[7] = poseCovar(2, 1);
+    pose.covariance[8] = poseCovar(2, 2);
+
+    // Update pose
     pose.x += deltaX;
     pose.y += deltaY;
     pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
@@ -140,7 +312,7 @@ namespace create {
     float maxWait = 30; // seconds
     float retryInterval = 5; //seconds
     time(&start);
-    while (!serial->connect(port, baud, boost::bind(&Create::onData, this)) && !timeout) {
+    while (!serial->connect(port, baud, std::bind(&Create::onData, this)) && !timeout) {
       time(&now);
       if (difftime(now, start) > maxWait) {
         timeout = true;
@@ -167,7 +339,39 @@ namespace create {
   //}
 
   bool Create::setMode(const CreateMode& mode) {
-    return serial->sendOpcode((Opcode) mode);
+    if (model.getVersion() == V_1){
+      // Switch to safe mode (required for compatibility with V_1)
+      if (!(serial->sendOpcode(OC_START) && serial->sendOpcode(OC_CONTROL))) return false;
+    }
+    bool ret = false;
+    switch (mode) {
+      case MODE_OFF:
+        if (model.getVersion() == V_2) {
+          CERR("[create::Create] ", "protocol version 2 does not support turning robot off");
+          ret = false;
+        } else {
+          ret = serial->sendOpcode(OC_POWER);
+        }
+        break;
+      case MODE_PASSIVE:
+        ret = serial->sendOpcode(OC_START);
+        break;
+      case MODE_SAFE:
+        if (model.getVersion() > V_1) {
+          ret = serial->sendOpcode(OC_SAFE);
+        }
+        break;
+      case MODE_FULL:
+        ret = serial->sendOpcode(OC_FULL);
+        break;
+      default:
+        CERR("[create::Create] ", "cannot set robot to mode '" << mode << "'");
+        ret = false;
+    }
+    if (ret) {
+      this->mode = mode;
+    }
+    return ret;
   }
 
   bool Create::clean(const CleanMode& mode) {
@@ -180,55 +384,111 @@ namespace create {
 
   bool Create::setDate(const DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const {
     if (day < 0 || day > 6 ||
-        hour < 0 || hour > 23 ||
-        min < 0 || min > 59)
+        hour > 23 ||
+        min > 59)
       return false;
 
-    uint8_t cmd[4] = { OC_DATE, day, hour, min };
+    uint8_t cmd[4] = { OC_DATE, static_cast<uint8_t>(day), hour, min };
     return serial->send(cmd, 4);
   }
 
-  bool Create::driveRadius(const float& vel, const float& radius) const {
+  bool Create::driveRadius(const float& vel, const float& radius) {
+    // Bound velocity
+    float boundedVel = BOUND_CONST(vel, -model.getMaxVelocity(), model.getMaxVelocity());
+
     // Expects each parameter as two bytes each and in millimeters
-    int16_t vel_mm = roundf(vel * 1000);
+    int16_t vel_mm = roundf(boundedVel * 1000);
     int16_t radius_mm = roundf(radius * 1000);
-    BOUND(vel_mm, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
 
     // Bound radius if not a special case
-    if (radius_mm != 32768 && radius_mm != 32767 &&
+    if (radius_mm != -32768 && radius_mm != 32767 &&
         radius_mm != -1 && radius_mm != 1) {
-      BOUND(radius_mm, -util::CREATE_2_MAX_RADIUS, util::CREATE_2_MAX_RADIUS);
+      BOUND(radius_mm, -util::MAX_RADIUS * 1000, util::MAX_RADIUS * 1000);
     }
 
     uint8_t cmd[5] = { OC_DRIVE,
-                       vel_mm >> 8,
-                       vel_mm & 0xff,
-                       radius_mm >> 8,
-                       radius_mm & 0xff
+                       static_cast<uint8_t>(vel_mm >> 8),
+                       static_cast<uint8_t>(vel_mm & 0xff),
+                       static_cast<uint8_t>(radius_mm >> 8),
+                       static_cast<uint8_t>(radius_mm & 0xff)
                      };
 
     return serial->send(cmd, 5);
   }
 
-  bool Create::driveWheels(const float& leftVel, const float& rightVel) const {
-    int16_t leftCmd = roundf(leftVel * 1000);
-    int16_t rightCmd = roundf(rightVel * 1000);
-    BOUND(leftCmd, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
-    BOUND(rightCmd, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
+  bool Create::driveWheels(const float& leftVel, const float& rightVel) {
+    const float boundedLeftVel = BOUND_CONST(leftVel, -model.getMaxVelocity(), model.getMaxVelocity());
+    const float boundedRightVel = BOUND_CONST(rightVel, -model.getMaxVelocity(), model.getMaxVelocity());
+    requestedLeftVel = boundedLeftVel;
+    requestedRightVel = boundedRightVel;
+    if (model.getVersion() > V_1) {
+      int16_t leftCmd = roundf(boundedLeftVel * 1000);
+      int16_t rightCmd = roundf(boundedRightVel * 1000);
 
       uint8_t cmd[5] = { OC_DRIVE_DIRECT,
-                       rightCmd >> 8,
-                       rightCmd & 0xff,
-                       leftCmd >> 8,
-                       leftCmd & 0xff
+                         static_cast<uint8_t>(rightCmd >> 8),
+                         static_cast<uint8_t>(rightCmd & 0xff),
+                         static_cast<uint8_t>(leftCmd >> 8),
+                         static_cast<uint8_t>(leftCmd & 0xff)
                        };
       return serial->send(cmd, 5);
+    } else {
+      float radius;
+      // Prevent divide by zero when driving straight
+      if (boundedLeftVel != boundedRightVel) {
+        radius = -((model.getAxleLength() / 2.0) * (boundedLeftVel + boundedRightVel)) /
+          (boundedLeftVel - boundedRightVel);
+      } else {
+        radius = util::STRAIGHT_RADIUS;
+      }
+
+      float vel;
+      // Fix signs for spin in place
+      if (boundedLeftVel == -boundedRightVel || std::abs(roundf(radius * 1000)) <= 1) {
+        radius = util::IN_PLACE_RADIUS;
+        vel = boundedRightVel;
+      } else {
+        vel = (std::abs(boundedLeftVel) + std::abs(boundedRightVel)) / 2.0 * ((boundedLeftVel + boundedRightVel) > 0 ? 1.0 : -1.0);
+      }
+
+      // Radius turns have a maximum radius of 2.0 meters
+      // When the radius is > 2 but <= 10, use a 2 meter radius
+      // When it is > 10, drive straight
+      // TODO: alternate between these 2 meter radius and straight to
+      // fake a larger radius
+      if (radius > 10.0) {
+        radius = util::STRAIGHT_RADIUS;
+      }
+
+      return driveRadius(vel, radius);
+    }
+  }
+
+  bool Create::driveWheelsPwm(const float& leftWheel, const float& rightWheel)
+  {
+    static const int16_t PWM_COUNTS = 255;
+
+    if (leftWheel < -1.0 || leftWheel > 1.0 ||
+        rightWheel < -1.0 || rightWheel > 1.0)
+      return false;
+
+    int16_t leftPwm = roundf(leftWheel * PWM_COUNTS);
+    int16_t rightPwm = roundf(rightWheel * PWM_COUNTS);
+
+    uint8_t cmd[5] = { OC_DRIVE_PWM,
+                       static_cast<uint8_t>(rightPwm >> 8),
+                       static_cast<uint8_t>(rightPwm & 0xff),
+                       static_cast<uint8_t>(leftPwm >> 8),
+                       static_cast<uint8_t>(leftPwm & 0xff)
+                     };
+
+    return serial->send(cmd, 5);
   }
 
-  bool Create::drive(const float& xVel, const float& angularVel) const {
+  bool Create::drive(const float& xVel, const float& angularVel) {
     // 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);
+    float leftVel = xVel - ((model.getAxleLength() / 2.0) * angularVel);
+    float rightVel = xVel + ((model.getAxleLength() / 2.0) * angularVel);
     return driveWheels(leftVel, rightVel);
   }
 
@@ -242,6 +502,15 @@ namespace create {
     sideMotorPower = roundf(side * 127);
     vacuumMotorPower = roundf(vacuum * 127);
 
+    if (model.getVersion() == V_1) {
+        uint8_t cmd[2] = { OC_MOTORS,
+                           static_cast<uint8_t>((side != 0.0 ? 1 : 0) |
+                                     (vacuum != 0.0 ? 2 : 0) |
+                                     (main != 0.0 ? 4 : 0))
+                         };
+        return serial->send(cmd, 2);
+    }
+
     uint8_t cmd[4] = { OC_MOTORS_PWM,
                        mainMotorPower,
                        sideMotorPower,
@@ -252,15 +521,15 @@ namespace create {
   }
 
   bool Create::setMainMotor(const float& main) {
-    return setAllMotors(main, sideMotorPower, vacuumMotorPower);
+    return setAllMotors(main, static_cast<float>(sideMotorPower) / 127.0, static_cast<float>(vacuumMotorPower) / 127.0);
   }
 
   bool Create::setSideMotor(const float& side) {
-    return setAllMotors(mainMotorPower, side, vacuumMotorPower);
+    return setAllMotors(static_cast<float>(mainMotorPower) / 127.0, side, static_cast<float>(vacuumMotorPower) / 127.0);
   }
 
   bool Create::setVacuumMotor(const float& vacuum) {
-    return setAllMotors(mainMotorPower, sideMotorPower, vacuum);
+    return setAllMotors(static_cast<float>(mainMotorPower) / 127.0, static_cast<float>(sideMotorPower) / 127.0, vacuum);
   }
 
   bool Create::updateLEDs() {
@@ -340,7 +609,7 @@ namespace create {
                           const float* durations) const {
     int i, j;
     uint8_t duration;
-    uint8_t cmd[2 * songLength + 3];
+    std::vector<uint8_t> cmd(2 * songLength + 3);
     cmd[0] = OC_SONG;
     cmd[1] = songNumber;
     cmd[2] = songLength;
@@ -354,16 +623,20 @@ namespace create {
       j++;
     }
 
-    return serial->send(cmd, 2 * songLength + 3);
+    return serial->send(cmd.data(), cmd.size());
   }
 
   bool Create::playSong(const uint8_t& songNumber) const {
-    if (songNumber < 0 || songNumber > 4)
+    if (songNumber > 4)
       return false;
     uint8_t cmd[2] = { OC_PLAY, songNumber };
     return serial->send(cmd, 2);
   }
 
+  void Create::setDtHistoryLength(const uint8_t& dtHistoryLength) {
+    this->dtHistoryLength = dtHistoryLength;
+  }
+
   bool Create::isWheeldrop() const {
     if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
       return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;
@@ -374,6 +647,26 @@ namespace create {
     }
   }
 
+  bool Create::isLeftWheeldrop() const {
+    if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
+      return (GET_DATA(ID_BUMP_WHEELDROP) & 0x08) != 0;
+    }
+    else {
+      CERR("[create::Create] ", "Wheeldrop sensor not supported!");
+      return false;
+    }
+  }
+
+  bool Create::isRightWheeldrop() const {
+    if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
+      return (GET_DATA(ID_BUMP_WHEELDROP) & 0x04) != 0;
+    }
+    else {
+      CERR("[create::Create] ", "Wheeldrop sensor not supported!");
+      return false;
+    }
+  }
+
   bool Create::isLeftBumper() const {
     if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
       return (GET_DATA(ID_BUMP_WHEELDROP) & 0x02) != 0;
@@ -420,6 +713,86 @@ namespace create {
     }
   }
 
+  bool Create::isCliffLeft() const {
+    if (data->isValidPacketID(ID_CLIFF_LEFT)) {
+      return GET_DATA(ID_CLIFF_LEFT) == 1;
+    }
+    else {
+      CERR("[create::Create] ", "Left cliff sensors not supported!");
+      return false;
+    }
+  }
+
+  bool Create::isCliffFrontLeft() const {
+    if (data->isValidPacketID(ID_CLIFF_FRONT_LEFT)) {
+      return GET_DATA(ID_CLIFF_FRONT_LEFT) == 1;
+    }
+    else {
+      CERR("[create::Create] ", "Front left cliff sensors not supported!");
+      return false;
+    }
+  }
+
+  bool Create::isCliffRight() const {
+    if (data->isValidPacketID(ID_CLIFF_RIGHT)) {
+      return GET_DATA(ID_CLIFF_RIGHT) == 1;
+    }
+    else {
+      CERR("[create::Create] ", "Rightt cliff sensors not supported!");
+      return false;
+    }
+  }
+
+  bool Create::isCliffFrontRight() const {
+    if (data->isValidPacketID(ID_CLIFF_FRONT_RIGHT)) {
+      return GET_DATA(ID_CLIFF_FRONT_RIGHT) == 1;
+    }
+    else {
+      CERR("[create::Create] ", "Front right cliff sensors not supported!");
+      return false;
+    }
+  }
+
+  uint16_t Create::getCliffSignalLeft() const {
+    if (data->isValidPacketID(ID_CLIFF_LEFT)) {
+      return GET_DATA(ID_CLIFF_LEFT_SIGNAL);
+    }
+    else {
+      CERR("[create::Create] ", "Left cliff sensor signals not supported!");
+      return false;
+      }
+  }
+
+  uint16_t Create::getCliffSignalFrontLeft() const {
+    if (data->isValidPacketID(ID_CLIFF_FRONT_LEFT)) {
+      return GET_DATA(ID_CLIFF_FRONT_LEFT_SIGNAL);
+    }
+    else {
+      CERR("[create::Create] ", "Front left cliff sensor signals not supported!");
+      return false;
+    }
+  }
+
+  uint16_t Create::getCliffSignalRight() const {
+    if (data->isValidPacketID(ID_CLIFF_RIGHT)) {
+      return GET_DATA(ID_CLIFF_RIGHT_SIGNAL);
+    }
+    else {
+      CERR("[create::Create] ", "Rightt cliff sensor signals not supported!");
+      return false;
+    }
+  }
+
+  uint16_t Create::getCliffSignalFrontRight() const {
+    if (data->isValidPacketID(ID_CLIFF_FRONT_RIGHT)) {
+      return GET_DATA(ID_CLIFF_FRONT_RIGHT_SIGNAL);
+    }
+    else {
+      CERR("[create::Create] ", "Front right cliff sensor signals not supported!");
+      return false;
+    }
+  }
+
   bool Create::isVirtualWall() const {
     if (data->isValidPacketID(ID_VIRTUAL_WALL)) {
       return GET_DATA(ID_VIRTUAL_WALL);
@@ -431,8 +804,8 @@ namespace create {
   }
 
   uint8_t Create::getDirtDetect() const {
-    if (data->isValidPacketID(ID_DIRT_DETECT)) {
-      return GET_DATA(ID_DIRT_DETECT);
+    if (data->isValidPacketID(ID_DIRT_DETECT_LEFT)) {
+      return GET_DATA(ID_DIRT_DETECT_LEFT);
     }
     else {
       CERR("[create::Create] ", "Dirt detector not supported!");
@@ -473,7 +846,6 @@ namespace create {
   ChargingState Create::getChargingState() const {
     if (data->isValidPacketID(ID_CHARGE_STATE)) {
       uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
-      assert(chargeState >= 0);
       assert(chargeState <= 5);
       return (ChargingState) chargeState;
     }
@@ -747,21 +1119,85 @@ namespace create {
     }
   }
 
-  create::CreateMode Create::getMode() const {
-    if (data->isValidPacketID(ID_OI_MODE)) {
-      return (create::CreateMode) GET_DATA(ID_OI_MODE);
+  bool Create::isSideBrushOvercurrent() const {
+    if (data->isValidPacketID(ID_OVERCURRENTS)) {
+      return (GET_DATA(ID_OVERCURRENTS) & 0x01) != 0;
     }
     else {
-      CERR("[create::Create] ", "Querying Mode not supported!");
-      return create::MODE_UNAVAILABLE;
+      CERR("[create::Create] ", "Overcurrent sensor not supported!");
+      return false;
     }
   }
 
-  const Pose& Create::getPose() const {
+  bool Create::isMainBrushOvercurrent() const {
+    if (data->isValidPacketID(ID_OVERCURRENTS)) {
+      return (GET_DATA(ID_OVERCURRENTS) & 0x04) != 0;
+    }
+    else {
+      CERR("[create::Create] ", "Overcurrent sensor not supported!");
+      return false;
+    }
+  }
+
+  bool Create::isWheelOvercurrent() const {
+    if (data->isValidPacketID(ID_OVERCURRENTS)) {
+      return (GET_DATA(ID_OVERCURRENTS) & 0x18) != 0;
+    }
+    else {
+      CERR("[create::Create] ", "Overcurrent sensor not supported!");
+      return false;
+    }
+  }
+
+  float Create::getLeftWheelDistance() const {
+    return totalLeftDist;
+  }
+
+  float Create::getRightWheelDistance() const {
+    return totalRightDist;
+  }
+
+  float Create::getMeasuredLeftWheelVel() const {
+    return measuredLeftVel;
+  }
+
+  float Create::getMeasuredRightWheelVel() const {
+    return measuredRightVel;
+  }
+
+  float Create::getRequestedLeftWheelVel() const {
+    return requestedLeftVel;
+  }
+
+  float Create::getRequestedRightWheelVel() const {
+    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)) {
+      if (modeReportWorkaround) {
+        mode = (create::CreateMode) (GET_DATA(ID_OI_MODE) - 1);
+      } else {
+        mode = (create::CreateMode) GET_DATA(ID_OI_MODE);
+      }
+    }
+
+    return mode;
+  }
+
+  Pose Create::getPose() const {
     return pose;
   }
 
-  const Vel& Create::getVel() const {
+  Vel Create::getVel() const {
     return vel;
   }
 

src/data.cpp

@@ -1,75 +1,55 @@
 #include "create/data.h"
 
-#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared<Packet>(nbytes,info))
+#define ADD_PACKET(id,nbytes,info,enabledVersion) if ((enabledVersion) & version) packets[id]=std::make_shared<Packet>(nbytes,info)
 
 namespace create {
 
-  Data::Data(RobotModel model) {
+  Data::Data(ProtocolVersion version) {
     // Populate data map
     /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
      *  WARNING: Adding too many packets will flood the serial   *
      *           buffer and corrupt the stream.                  *
      * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
-    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");
-    ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right");
-    ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right");
-    ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode");
-    ADD_PACKET(ID_BUTTONS, 1, "buttons");
-    ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state");
-    ADD_PACKET(ID_VOLTAGE, 2, "voltage");
-    ADD_PACKET(ID_CURRENT, 2, "current");
-    ADD_PACKET(ID_TEMP, 1, "temperature");
-    ADD_PACKET(ID_CHARGE , 2, "battery_charge");
-    ADD_PACKET(ID_CAPACITY, 2, "battery_capacity");
-    ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall");
-    ADD_PACKET(ID_OI_MODE, 1, "oi_mode");
-
-    if (model == CREATE_1) {
-      ADD_PACKET(ID_DISTANCE, 2, "distance");
-      ADD_PACKET(ID_ANGLE, 2, "angle");
-    }
-    else if (model == CREATE_2) {
-      //ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents");
-      ADD_PACKET(ID_DIRT_DETECT, 1, "dirt_detect");
-      //ADD_PACKET(ID_UNUSED_1, 1, "unused 1");
-      //ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_signal");
-      //ADD_PACKET(ID_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal");
-      //ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal");
-      //ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal");
-      //ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal");
-      //ADD_PACKET(ID_UNUSED_2, 1, "unused 2");
-      //ADD_PACKET(ID_UNUSED_3, 2, "unused 3");
-      //ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available");
-      //ADD_PACKET(ID_SONG_NUM, 1, "song_number");
-      //ADD_PACKET(ID_PLAYING, 1, "song_playing");
-      //ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets");
-      //ADD_PACKET(ID_VEL, 2, "velocity");
-      //ADD_PACKET(ID_RADIUS, 2, "radius");
-      //ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
-      //ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");
-      ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left");
-      ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right");
-      ADD_PACKET(ID_LIGHT, 1, "light_bumper");
-      ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left");
-      ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left");
-      ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left");
-      ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right");
-      ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right");
-      ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right");
-      ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left");
-      ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right");
-      //ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_motor_current");
-      //ADD_PACKET(ID_RIGHT_MOTOR_CURRENT, 2, "right_motor_current");
-      //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");
-    } // if CREATE_2
+    ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops", V_ALL);
+    ADD_PACKET(ID_WALL, 1, "wall", V_ALL);
+    ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left", V_ALL);
+    ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left", V_ALL);
+    ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right", V_ALL);
+    ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right", V_ALL);
+    ADD_PACKET(ID_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal", V_3);
+    ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal", V_3);
+    ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal", V_3);
+    ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal", V_3);
+    ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall", V_ALL);
+    ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents", V_ALL);
+    ADD_PACKET(ID_DIRT_DETECT_LEFT, 1, "dirt_detect_left", V_ALL);
+    ADD_PACKET(ID_DIRT_DETECT_RIGHT, 1, "dirt_detect_right", V_1);
+    ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode", V_ALL);
+    ADD_PACKET(ID_BUTTONS, 1, "buttons", V_ALL);
+    ADD_PACKET(ID_DISTANCE, 2, "distance", V_1 | V_2);
+    ADD_PACKET(ID_ANGLE, 2, "angle", V_1 | V_2);
+    ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state", V_ALL);
+    ADD_PACKET(ID_VOLTAGE, 2, "voltage", V_ALL);
+    ADD_PACKET(ID_CURRENT, 2, "current", V_ALL);
+    ADD_PACKET(ID_TEMP, 1, "temperature", V_ALL);
+    ADD_PACKET(ID_CHARGE , 2, "battery_charge", V_ALL);
+    ADD_PACKET(ID_CAPACITY, 2, "battery_capacity", V_ALL);
+    ADD_PACKET(ID_OI_MODE, 1, "oi_mode", V_2 | V_3);
+    ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left", V_3);
+    ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right", V_3);
+    ADD_PACKET(ID_LIGHT, 1, "light_bumper", V_3);
+    ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left", V_3);
+    ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left", V_3);
+    ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left", V_3);
+    ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right", V_3);
+    ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right", V_3);
+    ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right", V_3);
+    ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left", V_3);
+    ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right", V_3);
+    ADD_PACKET(ID_STASIS, 1, "stasis", V_3);
 
     totalDataBytes = 0;
-    for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
+    for (std::map<uint8_t, std::shared_ptr<Packet> >::iterator it = packets.begin();
          it != packets.end();
          ++it) {
       ids.push_back(it->first);
@@ -86,16 +66,15 @@ namespace create {
     return false;
   }
 
-  boost::shared_ptr<Packet> Data::getPacket(uint8_t id) {
+  std::shared_ptr<Packet> Data::getPacket(uint8_t id) {
     if (isValidPacketID(id)) {
       return packets[id];
     }
-    std::cout << "Invalid packet " << (int) id << " requested" << std::endl;
-    return boost::shared_ptr<Packet>();
+    return std::shared_ptr<Packet>();
   }
 
   void Data::validateAll() {
-    for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
+    for (std::map<uint8_t, std::shared_ptr<Packet> >::iterator it = packets.begin();
          it != packets.end();
          ++it) {
       it->second->validate();

src/packet.cpp

@@ -1,32 +1,34 @@
+#include <memory>
+
 #include "create/packet.h"
 
 namespace create {
 
   Packet::Packet(const uint8_t& numBytes, const std::string& comment) :
-    nbytes(numBytes),
-    info(comment),
     data(0),
-    tmpData(0) { }
+    tmpData(0),
+    nbytes(numBytes),
+    info(comment) { }
 
   Packet::~Packet() { }
 
-  void Packet::setTempData(const uint16_t& tmp) {
-    boost::mutex::scoped_lock lock(tmpDataMutex);
+  void Packet::setDataToValidate(const uint16_t& tmp) {
+    std::lock_guard<std::mutex> lock(tmpDataMutex);
     tmpData = tmp;
   }
 
   void Packet::validate() {
-    boost::mutex::scoped_lock lock(tmpDataMutex);
+    std::lock_guard<std::mutex> lock(tmpDataMutex);
     setData(tmpData);
   }
 
   void Packet::setData(const uint16_t& d) {
-    boost::mutex::scoped_lock lock(dataMutex);
+    std::lock_guard<std::mutex> lock(dataMutex);
     data = d;
   }
 
   uint16_t Packet::getData() const {
-    boost::mutex::scoped_lock lock(dataMutex);
+    std::lock_guard<std::mutex> lock(dataMutex);
     return data;
   }
 

src/serial.cpp

@@ -1,3 +1,5 @@
+#include <chrono>
+#include <functional>
 #include <iostream>
 
 #include "create/serial.h"
@@ -5,40 +7,61 @@
 
 namespace create {
 
-  Serial::Serial(boost::shared_ptr<Data> d, const uint8_t& header) :
-    data(d),
-    headerByte(header),
+  Serial::Serial(std::shared_ptr<Data> d, bool install_signal_handler) :
+    signals(io),
     port(io),
-    readState(READ_HEADER),
-    isReading(false),
     dataReady(false),
+    isReading(false),
+    data(d),
     corruptPackets(0),
-    totalPackets(0) {
+    totalPackets(0)
+  {
+    if (install_signal_handler) {
+      signals.add(SIGINT);
+      signals.add(SIGTERM);
+    }
   }
 
   Serial::~Serial() {
     disconnect();
   }
 
-  bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
+  void Serial::signalHandler(const boost::system::error_code& error, int signal_number) {
+    if (!error) {
+      if (connected()) {
+        // Ensure not in Safe/Full modes
+        sendOpcode(OC_START);
+        // Stop OI
+        sendOpcode(OC_STOP);
+        exit(signal_number);
+      }
+    }
+  }
+
+  bool Serial::connect(const std::string& portName, const int& baud, std::function<void()> cb) {
     using namespace boost::asio;
-    port.open(portName);
-    port.set_option(serial_port::baud_rate(baud));
-    port.set_option(serial_port::flow_control(serial_port::flow_control::none));
+    if (!openPort(portName, baud)) {
+      return false;
+    }
+
+    signals.async_wait(std::bind(&Serial::signalHandler, this, std::placeholders::_1, std::placeholders::_2));
 
     usleep(1000000);
 
-    if (port.is_open()) {
+    if (!port.is_open()) {
+      return false;
+    }
+
     callback = cb;
     bool startReadSuccess = startReading();
     if (!startReadSuccess) {
-        port.close();
-      }
-      return startReadSuccess;
-    }
+      closePort();
       return false;
     }
 
+    return true;
+  }
+
   void Serial::disconnect() {
     if (isReading) {
       stopReading();
@@ -53,6 +76,33 @@ namespace create {
     }
   }
 
+  bool Serial::openPort(const std::string& portName, const int& baud) {
+    using namespace boost::asio;
+    try {
+      port.open(portName);
+      port.set_option(serial_port::baud_rate(baud));
+      port.set_option(serial_port::character_size(8));
+      port.set_option(serial_port::parity(serial_port::parity::none));
+      port.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
+      port.set_option(serial_port::flow_control(serial_port::flow_control::none));
+    } catch (const boost::system::system_error& /*e*/) {
+      CERR("[create::Serial] ", "failed to open port: " << portName);
+      return false;
+    }
+    return true;
+  }
+
+  bool Serial::closePort() {
+    using namespace boost::asio;
+    try {
+      port.close();
+    } catch (const boost::system::system_error& /*e*/) {
+      CERR("[create::Serial] ", "failed to close port");
+      return false;
+    }
+    return true;
+  }
+
   bool Serial::startReading() {
     if (!connected()) return false;
 
@@ -64,44 +114,32 @@ namespace create {
     // Only allow once
     if (isReading) return true;
 
-    // Request from Create that we want a stream containing all packets
-    uint8_t numPackets = data->getNumPackets();
-    std::vector<uint8_t> packetIDs = data->getPacketIDs();
-    uint8_t streamReq[2 + numPackets];
-    streamReq[0] = OC_STREAM;
-    streamReq[1] = numPackets;
-    int i = 2;
-    for (std::vector<uint8_t>::iterator it = packetIDs.begin(); it != packetIDs.end(); ++it) {
-      streamReq[i] = *it;
-      i++;
-    }
-
     // Start OI
     sendOpcode(OC_START);
 
-    // Start streaming data
-    send(streamReq, 2 + numPackets);
-
-    expectedNumBytes = data->getTotalDataBytes() + numPackets;
-
-    //TODO: handle boost exceptions
+    if (!startSensorStream()) return false;
 
     io.reset();
 
     // Start continuously reading one byte at a time
     boost::asio::async_read(port,
                             boost::asio::buffer(&byteRead, 1),
-                            boost::bind(&Serial::onData, this, _1, _2));
+                            std::bind(&Serial::onData,
+                                      shared_from_this(),
+                                      std::placeholders::_1,
+                                      std::placeholders::_2));
 
-    ioThread = boost::thread(boost::bind(&boost::asio::io_service::run, &io));
+    ioThread = std::thread(std::bind(
+        static_cast<std::size_t(boost::asio::io_service::*)(void)>(
+          &boost::asio::io_service::run), &io));
 
     // Wait for first complete read to finish
-    boost::unique_lock<boost::mutex> lock(dataReadyMut);
+    std::unique_lock<std::mutex> lock(dataReadyMut);
 
     int attempts = 1;
     int maxAttempts = 10;
     while (!dataReady) {
-      if (!dataReadyCond.timed_wait(lock, boost::get_system_time() + boost::posix_time::milliseconds(500))) {
+      if (dataReadyCond.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) {
         if (attempts >= maxAttempts) {
           CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!");
           io.stop();
@@ -112,7 +150,7 @@ namespace create {
 
         // Request data again
         sendOpcode(OC_START);
-        send(streamReq, 2 + numPackets);
+        startSensorStream();
       }
     }
 
@@ -126,7 +164,7 @@ namespace create {
       ioThread.join();
       isReading = false;
       {
-        boost::lock_guard<boost::mutex> lock(dataReadyMut);
+        std::lock_guard<std::mutex> lock(dataReadyMut);
         dataReady = false;
       }
     }
@@ -139,7 +177,7 @@ namespace create {
 
     // Notify first data packets ready
     {
-      boost::lock_guard<boost::mutex> lock(dataReadyMut);
+      std::lock_guard<std::mutex> lock(dataReadyMut);
       if (!dataReady) {
         dataReady = true;
         dataReadyCond.notify_one();
@@ -158,80 +196,16 @@ namespace create {
 
     // Should have read exactly one byte
     if (size == 1) {
-      numBytesRead++;
-      byteSum += byteRead;
-      switch (readState) {
-        case READ_HEADER:
-          if (byteRead == headerByte) {
-            readState = READ_NBYTES;
-            byteSum = byteRead;
-          }
-        break;
-
-        case READ_NBYTES:
-          if (byteRead == expectedNumBytes) {
-            readState = READ_PACKET_ID;
-            numBytesRead = 0;
-          }
-          else {
-            //notifyDataReady();
-            readState = READ_HEADER;
-          }
-        break;
-
-        case READ_PACKET_ID:
-          packetID = byteRead;
-          if (data->isValidPacketID(packetID)) {
-            expectedNumDataBytes = data->getPacket(packetID)->nbytes;
-            assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
-            numDataBytesRead = 0;
-            packetBytes = 0;
-            readState = READ_PACKET_BYTES;
-          }
-          else {
-            //notifyDataReady();
-            readState = READ_HEADER;
-          }
-        break;
-
-        case READ_PACKET_BYTES:
-          numDataBytesRead++;
-          if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
-            // High byte first
-            packetBytes = (byteRead << 8) & 0xff00;
-          }
-          else {
-            // Low byte
-            packetBytes += byteRead;
-          }
-          if (numDataBytesRead >= expectedNumDataBytes) {
-            data->getPacket(packetID)->setTempData(packetBytes);
-            if (numBytesRead >= expectedNumBytes)
-              readState = READ_CHECKSUM;
-            else
-              readState = READ_PACKET_ID;
-          }
-        break;
-
-        case READ_CHECKSUM:
-          if ((byteSum & 0xFF) == 0) {
-            notifyDataReady();
-          }
-          else {
-            // Corrupt data
-            corruptPackets++;
-          }
-          totalPackets++;
-          // Start again
-          readState = READ_HEADER;
-        break;
-      } // end switch (readState)
+      processByte(byteRead);
     } // end if (size == 1)
 
     // Read the next byte
     boost::asio::async_read(port,
                             boost::asio::buffer(&byteRead, 1),
-                            boost::bind(&Serial::onData, this, _1, _2));
+                            std::bind(&Serial::onData,
+                                      shared_from_this(),
+                                      std::placeholders::_1,
+                                      std::placeholders::_2));
   }
 
   bool Serial::send(const uint8_t* bytes, unsigned int numBytes) {
@@ -239,8 +213,13 @@ namespace create {
       CERR("[create::Serial] ", "send failed, not connected.");
       return false;
     }
-    // TODO: catch boost exceptions
+    
+    try {
       boost::asio::write(port, boost::asio::buffer(bytes, numBytes));
+    } catch (const boost::system::system_error & e) {
+      CERR("[create::Serial] ", "failed to write bytes to port");
+      return false;
+    }
     return true;
   }
 

src/serial_query.cpp

@@ -0,0 +1,73 @@
+#include <iostream>
+#include <memory>
+
+#include "create/serial_query.h"
+#include "create/types.h"
+
+#define SENSORS_RESPONSE_LENGTH 20
+
+namespace create {
+
+  SerialQuery::SerialQuery(std::shared_ptr<Data> d, bool install_signal_handler)
+  : Serial(d, install_signal_handler),
+    streamRecoveryTimer(io),
+    packetID(ID_BUMP_WHEELDROP),
+    packetByte(0),
+    packetData(0),
+    maxPacketID(ID_CAPACITY) {
+  }
+
+  bool SerialQuery::startSensorStream() {
+    if (!started) {
+      requestSensorData();
+      started = true;
+    }
+    return true;
+  }
+
+  void SerialQuery::requestSensorData() {
+    static const uint8_t requestPacket[2] = { OC_SENSORS, ID_GROUP_0 };
+    // Prevents previous packet from corrupting next one
+    flushInput();
+    send(requestPacket, 2);
+    // Automatically resend request if no response is received
+    streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50));
+    streamRecoveryTimer.async_wait(
+      std::bind(&SerialQuery::restartSensorStream, this, std::placeholders::_1));
+  }
+
+  void SerialQuery::restartSensorStream(const boost::system::error_code& err) {
+    if (err != boost::asio::error::operation_aborted) {
+      if (packetID != ID_BUMP_WHEELDROP) {
+        ++corruptPackets;
+      }
+      requestSensorData();
+    }
+  }
+
+  void SerialQuery::flushInput() {
+    // Only works with POSIX support
+    tcflush(port.lowest_layer().native_handle(), TCIFLUSH);
+  }
+
+  void SerialQuery::processByte(uint8_t byteRead) {
+    packetData |= (static_cast<uint16_t>(byteRead) << (8 * packetByte));
+
+    if (packetByte > 0) {
+      --packetByte;
+    } else if (packetID < maxPacketID) {
+      // New packet
+      data->getPacket(packetID)->setDataToValidate(packetData);
+      packetData = 0;
+      ++packetID;
+      packetByte = data->getPacket(packetID)->nbytes - 1;
+    } else {
+      // Response finished
+      packetID = ID_BUMP_WHEELDROP;
+      packetByte = 0;
+      packetData = 0;
+      notifyDataReady();
+      requestSensorData();
+    }
+  }
+} // namespace create

src/serial_stream.cpp

@@ -0,0 +1,98 @@
+#include <iostream>
+#include <memory>
+
+#include "create/serial_stream.h"
+#include "create/types.h"
+
+namespace create {
+
+  SerialStream::SerialStream(std::shared_ptr<Data> d, const uint8_t& header, bool install_signal_handler) : Serial(d, install_signal_handler), readState(READ_HEADER), headerByte(header) {
+  }
+
+  bool SerialStream::startSensorStream() {
+    // Request from Create that we want a stream containing all packets
+    const uint8_t numPackets = data->getNumPackets();
+    std::vector<uint8_t> packetIDs = data->getPacketIDs();
+    packetIDs.insert(packetIDs.begin(), numPackets);
+    packetIDs.insert(packetIDs.begin(), OC_STREAM);
+
+    // Start streaming data
+    send(packetIDs.data(), packetIDs.size());
+
+    expectedNumBytes = data->getTotalDataBytes() + numPackets;
+
+    return true;
+  }
+
+  void SerialStream::processByte(uint8_t byteRead) {
+    numBytesRead++;
+    byteSum += byteRead;
+    switch (readState) {
+      case READ_HEADER:
+        if (byteRead == headerByte) {
+          readState = READ_NBYTES;
+          byteSum = byteRead;
+        }
+        break;
+
+      case READ_NBYTES:
+        if (byteRead == expectedNumBytes) {
+          readState = READ_PACKET_ID;
+          numBytesRead = 0;
+        }
+        else {
+          //notifyDataReady();
+          readState = READ_HEADER;
+        }
+        break;
+
+      case READ_PACKET_ID:
+        packetID = byteRead;
+        if (data->isValidPacketID(packetID)) {
+          expectedNumDataBytes = data->getPacket(packetID)->nbytes;
+          assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
+          numDataBytesRead = 0;
+          packetBytes = 0;
+          readState = READ_PACKET_BYTES;
+        }
+        else {
+          //notifyDataReady();
+          readState = READ_HEADER;
+        }
+        break;
+
+      case READ_PACKET_BYTES:
+        numDataBytesRead++;
+        if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
+          // High byte first
+          packetBytes = (byteRead << 8) & 0xff00;
+        }
+        else {
+          // Low byte
+          packetBytes += byteRead;
+        }
+        if (numDataBytesRead >= expectedNumDataBytes) {
+          data->getPacket(packetID)->setDataToValidate(packetBytes);
+          if (numBytesRead >= expectedNumBytes)
+            readState = READ_CHECKSUM;
+          else
+            readState = READ_PACKET_ID;
+        }
+        break;
+
+      case READ_CHECKSUM:
+        if ((byteSum & 0xFF) == 0) {
+          notifyDataReady();
+        }
+        else {
+          // Corrupt data
+          corruptPackets++;
+        }
+        totalPackets++;
+        // Start again
+        readState = READ_HEADER;
+        break;
+    } // end switch (readState)
+  }
+
+} // namespace create

src/types.cpp

@@ -0,0 +1,52 @@
+#include "create/types.h"
+
+namespace create {
+
+  RobotModel::RobotModel(const ProtocolVersion version, const float axleLength, const unsigned int baud, const float maxVelocity, const float wheelDiameter):
+    id(nextId),
+    version(version),
+    axleLength(axleLength),
+    baud(baud),
+    maxVelocity(maxVelocity),
+    wheelDiameter(wheelDiameter) {
+    nextId <<= 1;
+  }
+
+  bool RobotModel::operator ==(RobotModel& other) const {
+    return id == other.id;
+  }
+
+  RobotModel::operator uint32_t() const {
+    return id;
+  }
+
+  uint32_t RobotModel::getId() const {
+    return id;
+  }
+
+  ProtocolVersion RobotModel::getVersion() const {
+    return version;
+  }
+
+  float RobotModel::getAxleLength() const {
+    return axleLength;
+  }
+
+  unsigned int RobotModel::getBaud() const {
+    return baud;
+  }
+
+  float RobotModel::getMaxVelocity() const {
+    return maxVelocity;
+  }
+
+  float RobotModel::getWheelDiameter() const {
+    return wheelDiameter;
+  }
+
+  uint32_t RobotModel::nextId = 1;
+
+  RobotModel RobotModel::ROOMBA_400(V_1, 0.258, 57600);
+  RobotModel RobotModel::CREATE_1(V_2, 0.258, 57600);
+  RobotModel RobotModel::CREATE_2(V_3, 0.235, 115200, 0.5, 0.072);
+}

tests/test_create.cpp

@@ -0,0 +1,65 @@
+/**
+Software License Agreement (BSD)
+
+\file      test_create.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/create.h"
+#include "create/types.h"
+
+#include "gtest/gtest.h"
+
+TEST(CreateTest, ConstructorSingleParam)
+{
+  create::Create create_default;
+
+  create::Create create_1(create::RobotModel::CREATE_1);
+
+  create::Create create_2(create::RobotModel::CREATE_2);
+
+  create::Create create_roomba_400(create::RobotModel::ROOMBA_400);
+}
+
+// TEST(CreateTest, ConstructorMultiParam)
+// {
+//   TODO(jacobperron): Document exception thrown and consider defining custom exception
+//   create::Create create(std::string("/dev/ttyUSB0"), 11520);
+// }
+
+TEST(CreateTest, Connected)
+{
+  create::Create create;
+  // Nothing to be connected to
+  EXPECT_FALSE(create.connected());
+}
+
+TEST(CreateTest, Disconnect)
+{
+  create::Create create;
+  // Even though not connected, this should not crash
+  create.disconnect();
+}

tests/test_data.cpp

@@ -0,0 +1,169 @@
+/**
+Software License Agreement (BSD)
+
+\file      test_data.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/data.h"
+#include "create/packet.h"
+#include "create/types.h"
+
+#include "gtest/gtest.h"
+
+#include <memory>
+
+TEST(DataTest, Constructor)
+{
+  create::Data data_default;
+
+  create::Data data_v_1(create::V_1);
+
+  create::Data data_v_2(create::V_2);
+
+  create::Data data_v_3(create::V_3);
+
+  create::Data data_v_all(create::V_ALL);
+}
+
+// Number of packets for a given protocol are determined in the Data() constructor
+TEST(DataTest, GetNumPackets)
+{
+  // Number of packets shared by all protocols is 17
+  create::Data data_v_1(create::V_1);
+  // Number exclusive to V_1 = 3
+  // 17 + 3 = 20
+  EXPECT_EQ(static_cast<int>(data_v_1.getNumPackets()), 20);
+
+  create::Data data_v_2(create::V_2);
+  // Number exclusive to V_2 = 3
+  // 17 + 3 = 20
+  EXPECT_EQ(static_cast<int>(data_v_2.getNumPackets()), 20);
+
+  create::Data data_v_3(create::V_3);
+  // Number exclusive to V_3 = 13
+  // 17 + 17 = 34
+  EXPECT_EQ(static_cast<int>(data_v_3.getNumPackets()), 34);
+
+  create::Data data_v_all(create::V_ALL);
+  EXPECT_EQ(static_cast<int>(data_v_all.getNumPackets()), 37);
+}
+
+TEST(DataTest, GetPacket)
+{
+  // Get a packet exclusive to V_1
+  create::Data data_v_1(create::V_1);
+  std::shared_ptr<create::Packet> v_1_packet_ptr = data_v_1.getPacket(create::ID_OVERCURRENTS);
+  EXPECT_NE(v_1_packet_ptr, std::shared_ptr<create::Packet>())
+      << "ID_OVERCURRENTS packet not found for protocol V_1";
+  EXPECT_EQ(static_cast<int>(v_1_packet_ptr->nbytes), 1);
+  EXPECT_EQ(v_1_packet_ptr->info, std::string("overcurrents"));
+
+  // Get a packet for V_2
+  create::Data data_v_2(create::V_2);
+  std::shared_ptr<create::Packet> v_2_packet_ptr = data_v_2.getPacket(create::ID_DISTANCE);
+  EXPECT_NE(v_2_packet_ptr, std::shared_ptr<create::Packet>())
+      << "ID_DISTANCE packet not found for protocol V_2";
+  EXPECT_EQ(static_cast<int>(v_2_packet_ptr->nbytes), 2);
+  EXPECT_EQ(v_2_packet_ptr->info, std::string("distance"));
+
+  // Get a packet exclusive to V_3
+  create::Data data_v_3(create::V_3);
+  std::shared_ptr<create::Packet> v_3_packet_ptr = data_v_3.getPacket(create::ID_LIGHT_FRONT_RIGHT);
+  EXPECT_NE(v_3_packet_ptr, std::shared_ptr<create::Packet>())
+      << "ID_LIGHT_FRONT_RIGHT packet not found for protocol V_3";
+  EXPECT_EQ(static_cast<int>(v_3_packet_ptr->nbytes), 2);
+  EXPECT_EQ(v_3_packet_ptr->info, std::string("light_bumper_front_right"));
+
+  // Get a non-existent packet
+  std::shared_ptr<create::Packet> not_a_packet_ptr = data_v_3.getPacket(60);
+  EXPECT_EQ(not_a_packet_ptr, std::shared_ptr<create::Packet>());
+}
+
+TEST(DataTest, GetPacketIDs)
+{
+  create::Data data_v_3(create::V_3);
+  const std::vector<uint8_t> packet_ids = data_v_3.getPacketIDs();
+  // Vector should have same length as reported by getNumPackets()
+  ASSERT_EQ(static_cast<int>(packet_ids.size()), 34);
+
+  // Vector should contain ID_LEFT_ENC
+  bool found = false;
+  for (std::size_t i = 0; (i < packet_ids.size()) && !found; i++)
+  {
+    if (packet_ids[i] == create::ID_LEFT_ENC)
+    {
+      found = true;
+    }
+  }
+  EXPECT_TRUE(found) << "ID_LEFT_ENC packet ID not returned for protocol V_3";
+}
+
+TEST(DataTest, GetTotalDataBytes)
+{
+  // All protocols have 21 mutual data bytes
+  // V_1 has an additional 5 bytes
+  create::Data data_v_1(create::V_1);
+  EXPECT_EQ(static_cast<int>(data_v_1.getTotalDataBytes()), 26);
+
+  // V_2 has an additional 5 bytes
+  create::Data data_v_2(create::V_2);
+  EXPECT_EQ(static_cast<int>(data_v_2.getTotalDataBytes()), 26);
+
+  // V_3 has an additional 29 bytes
+  create::Data data_v_3(create::V_3);
+  EXPECT_EQ(static_cast<int>(data_v_3.getTotalDataBytes()), 50);
+}
+
+TEST(DataTest, IsValidPacketID)
+{
+  create::Data data_v_1(create::V_1);
+  EXPECT_TRUE(data_v_1.isValidPacketID(create::ID_DIRT_DETECT_RIGHT))
+      << "ID_DIRT_DETECT_RIGHT packet not found for protocol V_1";
+  EXPECT_FALSE(data_v_1.isValidPacketID(create::ID_OI_MODE))
+      << "ID_OI_MODE packet should not exist for protocol V_1";
+
+  // V_2 has an additional 5 bytes
+  create::Data data_v_2(create::V_2);
+  EXPECT_TRUE(data_v_2.isValidPacketID(create::ID_ANGLE))
+      << "ID_ANGLE packet not found for protocol V_2";
+  EXPECT_FALSE(data_v_2.isValidPacketID(create::ID_LIGHT))
+      << "ID_LIGHT packet should not exist for protocol V_2";
+
+  // V_3 has an additional 21 bytes
+  create::Data data_v_3(create::V_3);
+  EXPECT_TRUE(data_v_3.isValidPacketID(create::ID_STASIS))
+      << "ID_STATIS packet not found for protocol V_3";
+  EXPECT_FALSE(data_v_3.isValidPacketID(create::ID_DISTANCE))
+      << "ID_DISTANCE packet should not exist for protocol V_3";
+}
+
+TEST(DataTest, ValidateAll)
+{
+  create::Data data_v_3(create::V_3);
+  // Don't crash
+  data_v_3.validateAll();
+}

tests/test_packet.cpp

@@ -0,0 +1,73 @@
+/**
+Software License Agreement (BSD)
+
+\file      test_packet.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/packet.h"
+#include "create/types.h"
+
+#include "gtest/gtest.h"
+
+TEST(PacketTest, Constructor)
+{
+  create::Packet empty_packet(0, std::string(""));
+  EXPECT_EQ(static_cast<int>(empty_packet.nbytes), 0);
+  EXPECT_EQ(empty_packet.info, std::string(""));
+
+  create::Packet some_packet(2, std::string("test_packet"));
+  EXPECT_EQ(static_cast<int>(some_packet.nbytes), 2);
+  EXPECT_EQ(some_packet.info, std::string("test_packet"));
+}
+
+TEST(PacketTest, SetValidateAndGetData)
+{
+  create::Packet packet(2, std::string("test_packet"));
+
+  // Set some data and validate it
+  const uint16_t some_data = 123;
+  packet.setDataToValidate(some_data);
+  packet.validate();
+  // Confirm data was validated
+  const uint16_t some_data_result = packet.getData();
+  EXPECT_EQ(some_data_result, some_data);
+
+  // Set zero data and validate it
+  const uint16_t zero_data = 0;
+  packet.setDataToValidate(zero_data);
+  packet.validate();
+  // Confirm data was validated
+  const uint16_t zero_data_result = packet.getData();
+  EXPECT_EQ(zero_data_result, zero_data);
+
+  // Set some data but do not validate it
+  const uint16_t do_not_validate_data = 321;
+  packet.setDataToValidate(do_not_validate_data);
+  // Confirm data was not validated
+  const uint16_t unvalidated_data_result = packet.getData();
+  EXPECT_NE(unvalidated_data_result, do_not_validate_data);
+}

tests/test_robot_model.cpp

@@ -0,0 +1,43 @@
+/**
+Software License Agreement (BSD)
+
+\file      test_robot_model.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/types.h"
+
+#include "gtest/gtest.h"
+
+TEST(RobotModelTest, Equality)
+{
+  EXPECT_EQ(create::RobotModel::CREATE_1, create::RobotModel::CREATE_1);
+  EXPECT_EQ(create::RobotModel::CREATE_2, create::RobotModel::CREATE_2);
+  EXPECT_EQ(create::RobotModel::ROOMBA_400, create::RobotModel::ROOMBA_400);
+  EXPECT_NE(create::RobotModel::CREATE_1, create::RobotModel::CREATE_2);
+  EXPECT_NE(create::RobotModel::CREATE_1, create::RobotModel::ROOMBA_400);
+  EXPECT_NE(create::RobotModel::CREATE_2, create::RobotModel::ROOMBA_400);
+}

tests/test_serial_query.cpp

@@ -0,0 +1,90 @@
+/**
+Software License Agreement (BSD)
+
+\file      test_serial_query.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/data.h"
+#include "create/serial_query.h"
+
+#include "gtest/gtest.h"
+
+#include <memory>
+
+TEST(SerialQueryTest, Constructor)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialQuery serial_query(data_ptr);
+}
+
+TEST(SerialQueryTest, Connected)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialQuery serial_query(data_ptr);
+
+  // Did not call connect and nothing to connect to, so expect false
+  EXPECT_FALSE(serial_query.connected());
+}
+
+TEST(SerialQueryTest, Disconnect)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialQuery serial_query(data_ptr);
+
+  // Not connected, but should not fail
+  serial_query.disconnect();
+}
+
+TEST(SerialQueryTest, NumPackets)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialQuery serial_query(data_ptr);
+
+  // Not connected, so zero packets should have been received
+  EXPECT_EQ(serial_query.getNumCorruptPackets(), 0);
+  EXPECT_EQ(serial_query.getTotalPackets(), 0);
+}
+
+TEST(SerialQueryTest, Send)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialQuery serial_query(data_ptr);
+
+  // Some bytes to send (to set date)
+  uint8_t bytes[4] = { create::OC_DATE, 0, 1, 2 };
+  // Not connected, so failure expected
+  EXPECT_FALSE(serial_query.send(bytes, 4));
+}
+
+TEST(SerialQueryTest, SendOpcode)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialQuery serial_query(data_ptr);
+
+  // Not connected, so failure expected
+  EXPECT_FALSE(serial_query.sendOpcode(create::OC_POWER));
+}

tests/test_serial_stream.cpp

@@ -0,0 +1,90 @@
+/**
+Software License Agreement (BSD)
+
+\file      test_serial_stream.cpp
+\authors   Jacob Perron <jperron@sfu.ca>
+\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+   this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+ * Neither the name of Autonomy Lab nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+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
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+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
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "create/data.h"
+#include "create/serial_stream.h"
+
+#include "gtest/gtest.h"
+
+#include <memory>
+
+TEST(SerialStreamTest, Constructor)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialStream serial_stream(data_ptr);
+}
+
+TEST(SerialStreamTest, Connected)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialStream serial_stream(data_ptr);
+
+  // Did not call connect and nothing to connect to, so expect false
+  EXPECT_FALSE(serial_stream.connected());
+}
+
+TEST(SerialStreamTest, Disconnect)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialStream serial_stream(data_ptr);
+
+  // Not connected, but should not fail
+  serial_stream.disconnect();
+}
+
+TEST(SerialStreamTest, NumPackets)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialStream serial_stream(data_ptr);
+
+  // Not connected, so zero packets should have been received
+  EXPECT_EQ(serial_stream.getNumCorruptPackets(), 0);
+  EXPECT_EQ(serial_stream.getTotalPackets(), 0);
+}
+
+TEST(SerialStreamTest, Send)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialStream serial_stream(data_ptr);
+
+  // Some bytes to send (to set date)
+  uint8_t bytes[4] = { create::OC_DATE, 0, 1, 2 };
+  // Not connected, so failure expected
+  EXPECT_FALSE(serial_stream.send(bytes, 4));
+}
+
+TEST(SerialStreamTest, SendOpcode)
+{
+  std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
+  create::SerialStream serial_stream(data_ptr);
+
+  // Not connected, so failure expected
+  EXPECT_FALSE(serial_stream.sendOpcode(create::OC_POWER));
+}