diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..d71d4b3 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,2 @@ +.github +ci/Dockerfile \ No newline at end of file diff --git a/.forgejo/workflows/package.yaml b/.forgejo/workflows/package.yaml new file mode 100644 index 0000000..b090971 --- /dev/null +++ b/.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}" diff --git a/.forgejo/workflows/test.yaml b/.forgejo/workflows/test.yaml new file mode 100644 index 0000000..2b55f1a --- /dev/null +++ b/.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 }} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..28408a6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +# CMake/CPack +build/ +_packages/ diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index d5c9e58..0000000 --- a/.travis.yml +++ /dev/null @@ -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 diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..0af0f5b --- /dev/null +++ b/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 `_) +* Update links to serial protocol documentation +* Add option to disable signal handlers (`#65 `_) +* Fix 'maybe-uninitialized' warnings +* Remove travis.yml +* Add GitHub workflow for CI +* Fix motor setting (`#62 `_) +* Use average dt values for velocity calculation (`#60 `_) +* Use steady clock for computing velocity (`#59 `_) +* Replace boost features with C++11 equivalents (`#58 `_) +* Implement methods for getting overcurrent status (`#57 `_) +* Use OC_MOTORS instead of OC_MOTORS_PWM on V_1 models (`#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 `_) + * 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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 036c026..e39cf16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,41 +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) - -## Specify libraries to link a library or executable target against -target_link_libraries(create_demo - ${Boost_LIBRARIES} - create +# Declare example executables +set(EXAMPLES + battery_level + bumpers + cliffs + drive_circle + leds + packets + play_song + wheeldrop ) -## 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) +foreach(EXAMPLE ${EXAMPLES}) + add_executable(${EXAMPLE} examples/${EXAMPLE}.cpp) + + target_link_libraries(${EXAMPLE} + ${Boost_LIBRARIES} + ${LIBRARY_NAME} + ) +endforeach() + +################# +# Configuration # +################# + +# set(CMAKE_INSTALL_PREFIX "/usr/") // complib needs this, riplib doesn't + +# Install directories layout: +# * /lib/ +# * /bin/ +# * /include/ +# * /lib/cmake/ +# * /share/ +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 '-config-version.cmake' +write_basic_package_version_file( + "${VERSION_CONFIG}" + VERSION "${PACKAGE_VERSION}" + COMPATIBILITY SameMajorVersion +) + +# Configure '-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 +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) \ No newline at end of file diff --git a/README.md b/README.md index bbe6737..b1eb336 100644 --- a/README.md +++ b/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. - -Example compile line: `g++ create_demo.cpp -lcreate -lboost_system -lboost_thread` +#### Serial Permissions #### -## Bugs +User permission is requried to connect to Create over serial. You can add your user to the dialout group to get permission: -* _Clock_ and _Schedule_ button presses are not detected. This is a known problem to the developers at iRobot. + sudo usermod -a -G dialout $USER -## Build Status +Logout and login again for this to take effect. -![Build Status](https://api.travis-ci.org/AutonomyLab/libcreate.svg?branch=master) +## Build ## + +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()` diff --git a/cmake/Packing.cmake b/cmake/Packing.cmake new file mode 100644 index 0000000..58d81b8 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/cmake/Versioning.cmake b/cmake/Versioning.cmake new file mode 100644 index 0000000..d659c5c --- /dev/null +++ b/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() \ No newline at end of file diff --git a/config.cmake.in b/config.cmake.in new file mode 100644 index 0000000..fbd7630 --- /dev/null +++ b/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@") diff --git a/examples/battery_level.cpp b/examples/battery_level.cpp new file mode 100644 index 0000000..24a4f54 --- /dev/null +++ b/examples/battery_level.cpp @@ -0,0 +1,78 @@ +/** +Software License Agreement (BSD) + +\file battery_level.cpp +\authors Jacob Perron +\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 + +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; +} diff --git a/examples/bumpers.cpp b/examples/bumpers.cpp new file mode 100644 index 0000000..80a62be --- /dev/null +++ b/examples/bumpers.cpp @@ -0,0 +1,115 @@ +/** +Software License Agreement (BSD) + +\file bumpers.cpp +\authors Jacob Perron +\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 + +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; +} diff --git a/examples/cliffs.cpp b/examples/cliffs.cpp new file mode 100644 index 0000000..0f004cc --- /dev/null +++ b/examples/cliffs.cpp @@ -0,0 +1,86 @@ +/** +Software License Agreement (BSD) + +\file cliffs.cpp +\authors Jacob Perron +\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 + +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; +} diff --git a/examples/create_demo.cpp b/examples/create_demo.cpp deleted file mode 100644 index 668014b..0000000 --- a/examples/create_demo.cpp +++ /dev/null @@ -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; -} diff --git a/examples/drive_circle.cpp b/examples/drive_circle.cpp new file mode 100644 index 0000000..5c4c8b9 --- /dev/null +++ b/examples/drive_circle.cpp @@ -0,0 +1,81 @@ +/** +Software License Agreement (BSD) + +\file drive_circle.cpp +\authors Jacob Perron +\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 +#include + +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; +} diff --git a/examples/leds.cpp b/examples/leds.cpp new file mode 100644 index 0000000..e4c566f --- /dev/null +++ b/examples/leds.cpp @@ -0,0 +1,94 @@ +/** +Software License Agreement (BSD) + +\file leds.cpp +\authors Jacob Perron +\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 + +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; +} diff --git a/examples/packets.cpp b/examples/packets.cpp new file mode 100644 index 0000000..d367487 --- /dev/null +++ b/examples/packets.cpp @@ -0,0 +1,72 @@ +/** +Software License Agreement (BSD) + +\file packets.cpp +\authors Jacob Perron +\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 + +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; +} diff --git a/examples/play_song.cpp b/examples/play_song.cpp new file mode 100644 index 0000000..61132b9 --- /dev/null +++ b/examples/play_song.cpp @@ -0,0 +1,93 @@ +/** +Software License Agreement (BSD) + +\file play_song.cpp +\authors Jacob Perron +\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 + +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; +} diff --git a/examples/wheeldrop.cpp b/examples/wheeldrop.cpp new file mode 100644 index 0000000..5e0bf66 --- /dev/null +++ b/examples/wheeldrop.cpp @@ -0,0 +1,85 @@ +/** +Software License Agreement (BSD) + +\file wheeldrop.cpp +\authors Jacob Perron +\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 + +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; +} diff --git a/include/create/create.h b/include/create/create.h index 9a4877a..186192e 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -32,11 +32,15 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_H #define CREATE_H -#include +#include +#include +#include #include #include +#include -#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 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 prevOnDataTime; + std::deque 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 data; - boost::shared_ptr serial; + std::shared_ptr data; + std::shared_ptr 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 + * 
+                 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
+
* * \return true if successful, false otherwise */ - //TODO (https://github.com/AutonomyLab/libcreate/issues/7) - //bool setDigits(const std::vector& segments) const; + bool setDigits(const std::vector& 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,186 +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; - //TODO (https://github.com/AutonomyLab/libcreate/issues/3) - //uint16_t getDistLeft() const; - //uint16_t getDistFrontLeft() const; - //uint16_t getDistCenterLeft() const; - //uint16_t getDistRight() const; - //uint16_t getDistFrontRight() const; - //uint16_t getDistCenterRight() const; + /** + * \brief Get the signal strength from the left light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalLeft() const; - /* Return true if Create is moving forward, false otherwise. + /** + * \brief Get the signal strength from the front-left light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalFrontLeft() const; + + /** + * \brief Get the signal strength from the center-left light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalCenterLeft() const; + + /** + * \brief Get the signal strength from the right light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalRight() const; + + /** + * \brief Get the signal strength from the front-right light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalFrontRight() const; + + /** + * \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. */ 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 diff --git a/include/create/data.h b/include/create/data.h index 02728f8..c4392e8 100644 --- a/include/create/data.h +++ b/include/create/data.h @@ -32,9 +32,8 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_DATA_H #define CREATE_DATA_H -#include -#include #include +#include #include #include "create/packet.h" @@ -43,16 +42,16 @@ POSSIBILITY OF SUCH DAMAGE. namespace create { class Data { private: - std::map > packets; + std::map > packets; uint32_t totalDataBytes; std::vector ids; public: - Data(RobotModel = CREATE_2); + Data(ProtocolVersion version = V_3); ~Data(); bool isValidPacketID(const uint8_t id) const; - boost::shared_ptr getPacket(const uint8_t id); + std::shared_ptr getPacket(const uint8_t id); void validateAll(); uint32_t getTotalDataBytes() const; uint8_t getNumPackets() const; diff --git a/include/create/packet.h b/include/create/packet.h index 8f2f426..60ff0f2 100644 --- a/include/create/packet.h +++ b/include/create/packet.h @@ -31,15 +31,20 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_PACKET_H #define CREATE_PACKET_H -#include +#include +#include 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; }; diff --git a/include/create/serial.h b/include/create/serial.h index 2f641e7..ccfdaea 100644 --- a/include/create/serial.h +++ b/include/create/serial.h @@ -35,66 +35,62 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_SERIAL_H #define CREATE_SERIAL_H +#include +#include +#include +#include +#include + #include -#include -#include -#include -#include #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 { - boost::shared_ptr 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 callback; + std::function 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; + // 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, const uint8_t& header = create::util::CREATE_2_HEADER); + Serial(std::shared_ptr data, bool install_signal_handler); ~Serial(); - bool connect(const std::string& port, const int& baud = 115200, boost::function cb = 0); + bool connect(const std::string& port, const int& baud = 115200, std::function cb = 0); void disconnect(); inline bool connected() const { return port.is_open(); }; bool send(const uint8_t* bytes, const uint32_t numBytes); diff --git a/include/create/serial_query.h b/include/create/serial_query.h new file mode 100644 index 0000000..ebd876a --- /dev/null +++ b/include/create/serial_query.h @@ -0,0 +1,75 @@ +/** +Software License Agreement (BSD) + +\file serial_query.h +\authors Jacob Perron +\authors Ben Wolsieffer +\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 + +#include + +#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, bool install_signal_handler = true); + virtual ~SerialQuery() = default; + }; +} // namespace create + +#endif // CREATE_SERIAL_H diff --git a/include/create/serial_stream.h b/include/create/serial_stream.h new file mode 100644 index 0000000..eeb2cb6 --- /dev/null +++ b/include/create/serial_stream.h @@ -0,0 +1,81 @@ +/** +Software License Agreement (BSD) + +\file serial_stream.h +\authors Jacob Perron +\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 + +#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, + const uint8_t& header = create::util::STREAM_HEADER, + bool install_signal_handler = true); + virtual ~SerialStream() = default; + + }; +} // namespace create + +#endif // CREATE_SERIAL_H diff --git a/include/create/types.h b/include/create/types.h index 9e65c08..4cf374f 100644 --- a/include/create/types.h +++ b/include/create/types.h @@ -32,10 +32,56 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_TYPES_H #define CREATE_TYPES_H +#include +#include +#include +#include + 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 covariance; }; typedef Pose Vel; diff --git a/include/create/util.h b/include/create/util.h index e91d959..aa3d8f5 100644 --- a/include/create/util.h +++ b/include/create/util.h @@ -32,7 +32,7 @@ POSSIBILITY OF SUCH DAMAGE. #ifndef CREATE_UTIL_H #define CREATE_UTIL_H -#include +#include #define COUT(prefix,msg) (std::cout< 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::max() - std::abs(a) ); } } // namespace util } // namespace create diff --git a/mainpage.dox b/mainpage.dox new file mode 100644 index 0000000..679c1be --- /dev/null +++ b/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. + +*/ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..85101b3 --- /dev/null +++ b/package.xml @@ -0,0 +1,30 @@ + + + libcreate + 3.1.0 + C++ library for interfacing with iRobot's Create 1 and Create 2 + + Jacob Perron + + BSD + + http://wiki.ros.org/libcreate + https://github.com/AutonomyLab/libcreate + https://github.com/AutonomyLab/libcreate/issues + + Jacob Perron + + cmake + + boost + + catkin + + doxygen + + gtest + + + cmake + + diff --git a/src/create.cpp b/src/create.cpp index b0cc473..75f28c5 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -1,20 +1,20 @@ -#include -#include #include #include #include +#include #include #include "create/create.h" #define GET_DATA(id) (data->getPacket(id)->getData()) -#define BOUND(val,min,max) (valmax?val=max:val=val)) +#define BOUND_CONST(val,min,max) (valmax?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(9, 0.0); vel.x = 0; vel.y = 0; vel.yaw = 0; - data = boost::shared_ptr(new Data(model)); - serial = boost::make_shared(data); + vel.covariance = std::vector(9, 0.0); + poseCovar = Matrix(3, 3, 0.0); + requestedLeftVel = 0; + requestedRightVel = 0; + dtHistoryLength = 100; + modeReportWorkaround = false; + data = std::shared_ptr(new Data(model.getVersion())); + if (model.getVersion() == V_1) { + serial = std::make_shared(data, install_signal_handler); + } else { + serial = std::make_shared( + 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::min() : std::numeric_limits::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>(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; - // 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; - deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(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; + wheelDistDiff = rightWheelDist - leftWheelDist; + deltaYaw = wheelDistDiff / model.getAxleLength(); } - else { + + // 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); + } 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)); + } + + 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(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(vel_mm >> 8), + static_cast(vel_mm & 0xff), + static_cast(radius_mm >> 8), + static_cast(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 + uint8_t cmd[5] = { OC_DRIVE_DIRECT, + static_cast(rightCmd >> 8), + static_cast(rightCmd & 0xff), + static_cast(leftCmd >> 8), + static_cast(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(rightPwm >> 8), + static_cast(rightPwm & 0xff), + static_cast(leftPwm >> 8), + static_cast(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,34 +502,43 @@ namespace create { sideMotorPower = roundf(side * 127); vacuumMotorPower = roundf(vacuum * 127); + if (model.getVersion() == V_1) { + uint8_t cmd[2] = { OC_MOTORS, + static_cast((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, - vacuumMotorPower - }; + mainMotorPower, + sideMotorPower, + vacuumMotorPower + }; return serial->send(cmd, 4); } bool Create::setMainMotor(const float& main) { - return setAllMotors(main, sideMotorPower, vacuumMotorPower); + return setAllMotors(main, static_cast(sideMotorPower) / 127.0, static_cast(vacuumMotorPower) / 127.0); } bool Create::setSideMotor(const float& side) { - return setAllMotors(mainMotorPower, side, vacuumMotorPower); + return setAllMotors(static_cast(mainMotorPower) / 127.0, side, static_cast(vacuumMotorPower) / 127.0); } bool Create::setVacuumMotor(const float& vacuum) { - return setAllMotors(mainMotorPower, sideMotorPower, vacuum); + return setAllMotors(static_cast(mainMotorPower) / 127.0, static_cast(sideMotorPower) / 127.0, vacuum); } bool Create::updateLEDs() { uint8_t LEDByte = debrisLED + spotLED + dockLED + checkLED; uint8_t cmd[4] = { OC_LEDS, - LEDByte, - powerLED, - powerLEDIntensity - }; + LEDByte, + powerLED, + powerLEDIntensity + }; return serial->send(cmd, 4); } @@ -340,7 +609,7 @@ namespace create { const float* durations) const { int i, j; uint8_t duration; - uint8_t cmd[2 * songLength + 3]; + std::vector 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; } @@ -677,6 +1049,66 @@ namespace create { } } + uint16_t Create::getLightSignalLeft() const { + if (data->isValidPacketID(ID_LIGHT_LEFT)) { + return GET_DATA(ID_LIGHT_LEFT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalFrontLeft() const { + if (data->isValidPacketID(ID_LIGHT_FRONT_LEFT)) { + return GET_DATA(ID_LIGHT_FRONT_LEFT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalCenterLeft() const { + if (data->isValidPacketID(ID_LIGHT_CENTER_LEFT)) { + return GET_DATA(ID_LIGHT_CENTER_LEFT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalRight() const { + if (data->isValidPacketID(ID_LIGHT_RIGHT)) { + return GET_DATA(ID_LIGHT_RIGHT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalFrontRight() const { + if (data->isValidPacketID(ID_LIGHT_FRONT_RIGHT)) { + return GET_DATA(ID_LIGHT_FRONT_RIGHT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalCenterRight() const { + if (data->isValidPacketID(ID_LIGHT_CENTER_RIGHT)) { + return GET_DATA(ID_LIGHT_CENTER_RIGHT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + bool Create::isMovingForward() const { if (data->isValidPacketID(ID_STASIS)) { return GET_DATA(ID_STASIS) == 1; @@ -687,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; } diff --git a/src/data.cpp b/src/data.cpp index fe6dc29..be20717 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -1,75 +1,55 @@ #include "create/data.h" -#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared(nbytes,info)) +#define ADD_PACKET(id,nbytes,info,enabledVersion) if ((enabledVersion) & version) packets[id]=std::make_shared(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 >::iterator it = packets.begin(); + for (std::map >::iterator it = packets.begin(); it != packets.end(); ++it) { ids.push_back(it->first); @@ -86,16 +66,15 @@ namespace create { return false; } - boost::shared_ptr Data::getPacket(uint8_t id) { + std::shared_ptr Data::getPacket(uint8_t id) { if (isValidPacketID(id)) { return packets[id]; } - std::cout << "Invalid packet " << (int) id << " requested" << std::endl; - return boost::shared_ptr(); + return std::shared_ptr(); } void Data::validateAll() { - for (std::map >::iterator it = packets.begin(); + for (std::map >::iterator it = packets.begin(); it != packets.end(); ++it) { it->second->validate(); diff --git a/src/packet.cpp b/src/packet.cpp index 799d9f7..517e90d 100644 --- a/src/packet.cpp +++ b/src/packet.cpp @@ -1,32 +1,34 @@ +#include + #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 lock(tmpDataMutex); tmpData = tmp; } void Packet::validate() { - boost::mutex::scoped_lock lock(tmpDataMutex); + std::lock_guard lock(tmpDataMutex); setData(tmpData); } void Packet::setData(const uint16_t& d) { - boost::mutex::scoped_lock lock(dataMutex); + std::lock_guard lock(dataMutex); data = d; } uint16_t Packet::getData() const { - boost::mutex::scoped_lock lock(dataMutex); + std::lock_guard lock(dataMutex); return data; } diff --git a/src/serial.cpp b/src/serial.cpp index 62206f5..6617e50 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -1,3 +1,5 @@ +#include +#include #include #include "create/serial.h" @@ -5,38 +7,59 @@ namespace create { - Serial::Serial(boost::shared_ptr d, const uint8_t& header) : - data(d), - headerByte(header), + Serial::Serial(std::shared_ptr 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 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 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()) { - callback = cb; - bool startReadSuccess = startReading(); - if (!startReadSuccess) { - port.close(); - } - return startReadSuccess; + if (!port.is_open()) { + return false; } - return false; + + callback = cb; + bool startReadSuccess = startReading(); + if (!startReadSuccess) { + closePort(); + return false; + } + + return true; } void Serial::disconnect() { @@ -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 packetIDs = data->getPacketIDs(); - uint8_t streamReq[2 + numPackets]; - streamReq[0] = OC_STREAM; - streamReq[1] = numPackets; - int i = 2; - for (std::vector::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( + &boost::asio::io_service::run), &io)); // Wait for first complete read to finish - boost::unique_lock lock(dataReadyMut); + std::unique_lock 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 lock(dataReadyMut); + std::lock_guard lock(dataReadyMut); dataReady = false; } } @@ -139,7 +177,7 @@ namespace create { // Notify first data packets ready { - boost::lock_guard lock(dataReadyMut); + std::lock_guard 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 - boost::asio::write(port, boost::asio::buffer(bytes, numBytes)); + + 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; } diff --git a/src/serial_query.cpp b/src/serial_query.cpp new file mode 100644 index 0000000..2f069db --- /dev/null +++ b/src/serial_query.cpp @@ -0,0 +1,73 @@ +#include +#include + +#include "create/serial_query.h" +#include "create/types.h" + +#define SENSORS_RESPONSE_LENGTH 20 + +namespace create { + + SerialQuery::SerialQuery(std::shared_ptr 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(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 diff --git a/src/serial_stream.cpp b/src/serial_stream.cpp new file mode 100644 index 0000000..96cd123 --- /dev/null +++ b/src/serial_stream.cpp @@ -0,0 +1,98 @@ +#include +#include + +#include "create/serial_stream.h" +#include "create/types.h" + +namespace create { + + SerialStream::SerialStream(std::shared_ptr 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 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 diff --git a/src/types.cpp b/src/types.cpp new file mode 100644 index 0000000..f9b02e2 --- /dev/null +++ b/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); +} diff --git a/tests/test_create.cpp b/tests/test_create.cpp new file mode 100644 index 0000000..aaa8a26 --- /dev/null +++ b/tests/test_create.cpp @@ -0,0 +1,65 @@ +/** +Software License Agreement (BSD) + +\file test_create.cpp +\authors Jacob Perron +\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(); +} diff --git a/tests/test_data.cpp b/tests/test_data.cpp new file mode 100644 index 0000000..e347fb1 --- /dev/null +++ b/tests/test_data.cpp @@ -0,0 +1,169 @@ +/** +Software License Agreement (BSD) + +\file test_data.cpp +\authors Jacob Perron +\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 + +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(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(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(data_v_3.getNumPackets()), 34); + + create::Data data_v_all(create::V_ALL); + EXPECT_EQ(static_cast(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 v_1_packet_ptr = data_v_1.getPacket(create::ID_OVERCURRENTS); + EXPECT_NE(v_1_packet_ptr, std::shared_ptr()) + << "ID_OVERCURRENTS packet not found for protocol V_1"; + EXPECT_EQ(static_cast(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 v_2_packet_ptr = data_v_2.getPacket(create::ID_DISTANCE); + EXPECT_NE(v_2_packet_ptr, std::shared_ptr()) + << "ID_DISTANCE packet not found for protocol V_2"; + EXPECT_EQ(static_cast(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 v_3_packet_ptr = data_v_3.getPacket(create::ID_LIGHT_FRONT_RIGHT); + EXPECT_NE(v_3_packet_ptr, std::shared_ptr()) + << "ID_LIGHT_FRONT_RIGHT packet not found for protocol V_3"; + EXPECT_EQ(static_cast(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 not_a_packet_ptr = data_v_3.getPacket(60); + EXPECT_EQ(not_a_packet_ptr, std::shared_ptr()); +} + +TEST(DataTest, GetPacketIDs) +{ + create::Data data_v_3(create::V_3); + const std::vector packet_ids = data_v_3.getPacketIDs(); + // Vector should have same length as reported by getNumPackets() + ASSERT_EQ(static_cast(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(data_v_1.getTotalDataBytes()), 26); + + // V_2 has an additional 5 bytes + create::Data data_v_2(create::V_2); + EXPECT_EQ(static_cast(data_v_2.getTotalDataBytes()), 26); + + // V_3 has an additional 29 bytes + create::Data data_v_3(create::V_3); + EXPECT_EQ(static_cast(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(); +} diff --git a/tests/test_packet.cpp b/tests/test_packet.cpp new file mode 100644 index 0000000..f1482ce --- /dev/null +++ b/tests/test_packet.cpp @@ -0,0 +1,73 @@ +/** +Software License Agreement (BSD) + +\file test_packet.cpp +\authors Jacob Perron +\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(empty_packet.nbytes), 0); + EXPECT_EQ(empty_packet.info, std::string("")); + + create::Packet some_packet(2, std::string("test_packet")); + EXPECT_EQ(static_cast(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); +} diff --git a/tests/test_robot_model.cpp b/tests/test_robot_model.cpp new file mode 100644 index 0000000..7ddcc86 --- /dev/null +++ b/tests/test_robot_model.cpp @@ -0,0 +1,43 @@ +/** +Software License Agreement (BSD) + +\file test_robot_model.cpp +\authors Jacob Perron +\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); +} diff --git a/tests/test_serial_query.cpp b/tests/test_serial_query.cpp new file mode 100644 index 0000000..b88b101 --- /dev/null +++ b/tests/test_serial_query.cpp @@ -0,0 +1,90 @@ +/** +Software License Agreement (BSD) + +\file test_serial_query.cpp +\authors Jacob Perron +\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 + +TEST(SerialQueryTest, Constructor) +{ + std::shared_ptr data_ptr = std::make_shared(); + create::SerialQuery serial_query(data_ptr); +} + +TEST(SerialQueryTest, Connected) +{ + std::shared_ptr data_ptr = std::make_shared(); + 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 data_ptr = std::make_shared(); + create::SerialQuery serial_query(data_ptr); + + // Not connected, but should not fail + serial_query.disconnect(); +} + +TEST(SerialQueryTest, NumPackets) +{ + std::shared_ptr data_ptr = std::make_shared(); + 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 data_ptr = std::make_shared(); + 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 data_ptr = std::make_shared(); + create::SerialQuery serial_query(data_ptr); + + // Not connected, so failure expected + EXPECT_FALSE(serial_query.sendOpcode(create::OC_POWER)); +} diff --git a/tests/test_serial_stream.cpp b/tests/test_serial_stream.cpp new file mode 100644 index 0000000..9ae789c --- /dev/null +++ b/tests/test_serial_stream.cpp @@ -0,0 +1,90 @@ +/** +Software License Agreement (BSD) + +\file test_serial_stream.cpp +\authors Jacob Perron +\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 + +TEST(SerialStreamTest, Constructor) +{ + std::shared_ptr data_ptr = std::make_shared(); + create::SerialStream serial_stream(data_ptr); +} + +TEST(SerialStreamTest, Connected) +{ + std::shared_ptr data_ptr = std::make_shared(); + 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 data_ptr = std::make_shared(); + create::SerialStream serial_stream(data_ptr); + + // Not connected, but should not fail + serial_stream.disconnect(); +} + +TEST(SerialStreamTest, NumPackets) +{ + std::shared_ptr data_ptr = std::make_shared(); + 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 data_ptr = std::make_shared(); + 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 data_ptr = std::make_shared(); + create::SerialStream serial_stream(data_ptr); + + // Not connected, so failure expected + EXPECT_FALSE(serial_stream.sendOpcode(create::OC_POWER)); +}