Compare commits
No commits in common. "master" and "1.1.0" have entirely different histories.
44 changed files with 739 additions and 3461 deletions
|
|
@ -1,2 +0,0 @@
|
|||
.github
|
||||
ci/Dockerfile
|
||||
|
|
@ -1,76 +0,0 @@
|
|||
---
|
||||
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}"
|
||||
|
|
@ -1,34 +0,0 @@
|
|||
---
|
||||
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 }}
|
||||
3
.gitignore
vendored
3
.gitignore
vendored
|
|
@ -1,3 +0,0 @@
|
|||
# CMake/CPack
|
||||
build/
|
||||
_packages/
|
||||
12
.travis.yml
Normal file
12
.travis.yml
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
language: cpp
|
||||
|
||||
compiler:
|
||||
- gcc
|
||||
|
||||
before_install:
|
||||
- sudo apt-get update -qq
|
||||
- sudo apt-get install libboost-system-dev libboost-thread-dev
|
||||
|
||||
script:
|
||||
- cmake .
|
||||
- make
|
||||
170
CHANGELOG.rst
170
CHANGELOG.rst
|
|
@ -1,170 +0,0 @@
|
|||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package libcreate
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
3.1.0 (2023-05-09)
|
||||
------------------
|
||||
* Address warnings and errors
|
||||
* Catch boost exceptions in Serial.h
|
||||
* Contributors: Swapnil Patel
|
||||
|
||||
3.0.0 (2022-04-06)
|
||||
------------------
|
||||
* Add option to workaround bug where firmware reports unexpected OI mode (`#67 <https://github.com/AutonomyLab/libcreate/issues/67>`_)
|
||||
* Update links to serial protocol documentation
|
||||
* Add option to disable signal handlers (`#65 <https://github.com/AutonomyLab/libcreate/issues/65>`_)
|
||||
* Fix 'maybe-uninitialized' warnings
|
||||
* Remove travis.yml
|
||||
* Add GitHub workflow for CI
|
||||
* Fix motor setting (`#62 <https://github.com/AutonomyLab/libcreate/issues/62>`_)
|
||||
* Use average dt values for velocity calculation (`#60 <https://github.com/AutonomyLab/libcreate/issues/60>`_)
|
||||
* Use steady clock for computing velocity (`#59 <https://github.com/AutonomyLab/libcreate/issues/59>`_)
|
||||
* Replace boost features with C++11 equivalents (`#58 <https://github.com/AutonomyLab/libcreate/issues/58>`_)
|
||||
* Implement methods for getting overcurrent status (`#57 <https://github.com/AutonomyLab/libcreate/issues/57>`_)
|
||||
* Use OC_MOTORS instead of OC_MOTORS_PWM on V_1 models (`#55 <https://github.com/AutonomyLab/libcreate/issues/55>`_)
|
||||
* Contributors: Daniel Smith, Jacob Perron, Josh Gadeken, Stefan Krupop, tim-fan
|
||||
|
||||
2.0.0 (2019-09-02)
|
||||
------------------
|
||||
* Cleanup examples
|
||||
* Use std::chrono instead of custom timestamp function
|
||||
* Remove Trusty CI job
|
||||
* Since it is EOL.
|
||||
* Default to C++11
|
||||
* Add compiler flags '-Wall -Wextra -Wpedantic'
|
||||
* Fix warnings as a result.
|
||||
* Disconnect from serial cleanly on SIGINT
|
||||
* Send the STOP opcode before exiting the program to ensure the robot is not left in a state that could potentially drain the battery.
|
||||
* Initialize variable for compiler compatibility
|
||||
* Add other serial communication options
|
||||
Otherwise, it's possible that Operation "7" (0x07) is confused for "135" (0x87)
|
||||
* Add cliff sensor example
|
||||
* Add API for getting left and right cliff detections
|
||||
* Update wheeldrop example
|
||||
* Add API for getting left and right wheeldrop
|
||||
* Use shared pointer when binding callback for serial read (`#38 <https://github.com/autonomylab/libcreate/issues/38>`_)
|
||||
* Resolves an issue with ROS Melodic on 18.04.
|
||||
* Fix for compatibility with Boost 1.66
|
||||
* Compatibility with at least as early as Boost 1.58 still persists
|
||||
* Update wheel diameter for Create 2
|
||||
* Now matches the spec from iRobot.
|
||||
* Add Bionic CI job
|
||||
* Add static cast to fix compiler warnings
|
||||
* Use package.xml format 3
|
||||
* Make catkin dependency conditional on ROS 1.
|
||||
* Add Xenial build to CI
|
||||
* Remove std::cout statement
|
||||
* Contributors: Anton Gerasimov, Jacob Perron, Ryota Suzuki, Yutaka Kondo
|
||||
|
||||
1.6.1 (2018-04-21)
|
||||
------------------
|
||||
* Build and install gtest as part of CI
|
||||
* Update README with instructions for building and running unit tests
|
||||
* Remove external cmake project for gtest
|
||||
Now only build tests if a gtest installation already exists on the system. This should expedite time to build for users that do not care about building/running unit tests and also eliminates the need for internet access when building.
|
||||
* Add test depend to gtest in package.xml
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
1.6.0 (2018-04-07)
|
||||
------------------
|
||||
* Add unit tests (gtests)
|
||||
* Refactor Packet API
|
||||
* Declare setData member as protected
|
||||
* Rename 'setTempData' to 'setDataToValidate'
|
||||
* Remove redundant packets from Data constructor
|
||||
* Updated setDigits function API comments
|
||||
* added HTML to adjust for spacing in diagram, showing the proper ordering of segments.
|
||||
* Update examples
|
||||
* More concise and focusing on individual features:
|
||||
* Battery level
|
||||
* Bumpers
|
||||
* Drive circle
|
||||
* LEDs
|
||||
* Serial packets
|
||||
* Play song
|
||||
* Wheeldrop
|
||||
* Update README
|
||||
* Refactor cmake files
|
||||
* Contributors: Jacob Perron, K.Moriarty
|
||||
|
||||
1.5.0 (2017-12-17)
|
||||
------------------
|
||||
* Add APIs for getting the measured velocities of the wheels
|
||||
* Add ability to drive the wheels with direct pwm duty
|
||||
* Update documentation
|
||||
* Add mainpage.dox
|
||||
* Use package.xml format 2
|
||||
* Add doxygen as doc dependency
|
||||
* Contributors: Erik Schembor, Jacob Perron
|
||||
|
||||
1.4.0 (2016-10-16)
|
||||
------------------
|
||||
* Switch to trusty for CI
|
||||
* Set mimumum cmake version to 2.8.12
|
||||
* Update CMakeLists.txt configuration and install rules
|
||||
* Add package.xml
|
||||
* Add config.cmake.in
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
1.3.0 (2016-08-23)
|
||||
------------------
|
||||
* Add support for early model Roomba 400s and other robots using the original SCI protocol.
|
||||
* Expose individual wheel distances and requested velocities. Fix wheel distance calculation for the Create 1.
|
||||
* Manually link to thread library. This allows libcreate to build on ARM.
|
||||
* Fix odometry inversion for Create 1.
|
||||
* Contributors: Ben Wolsieffer, Jacob Perron
|
||||
|
||||
1.2.1 (2016-04-30)
|
||||
------------------
|
||||
* Make velocity relative to base frame, not odometry frame
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
1.2.0 (2016-04-15)
|
||||
------------------
|
||||
* Add covariance info to Pose and Vel
|
||||
* Fix getMode bug
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
1.1.1 (2016-04-07)
|
||||
------------------
|
||||
* Fix odometry sign error
|
||||
* Add warning in code regarding Create 1 odometry issue
|
||||
* Add odom_example.cpp
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
1.1.0 (2016-04-02)
|
||||
------------------
|
||||
* Add API to get light sensor signals
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
1.0.0 (2016-04-01)
|
||||
------------------
|
||||
* Fix odometry for Create 1
|
||||
* Fix odom angle sign error
|
||||
* Convert units to base units
|
||||
* Implement 'getMode'
|
||||
* Rename 'isIRDetect*' functions to 'isLightBumper*'
|
||||
* Documentation / code cleanup
|
||||
* Add function 'driveRadius'
|
||||
* Add function 'isVirtualWall'
|
||||
* Fix sign error on returned 'current' and 'temperature'
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
0.1.1 (2016-03-25)
|
||||
------------------
|
||||
* Fix odometry bug
|
||||
* Contributors: Jacob Perron
|
||||
|
||||
0.1.0 (2016-03-24)
|
||||
------------------
|
||||
* Add enum of special IR characters
|
||||
* Fix bug: convert distance measurement to meters
|
||||
* Add support for first generation Create (Roomba 400 series)
|
||||
* Fix bug: Too many packets requested corrupting serial buffer
|
||||
* Expose functions for getting number of corrupt packets and total packets in Create class
|
||||
* Add getters for number of corrupt and total packets received over serial
|
||||
* Update README.md
|
||||
* Added build badge
|
||||
* Added CI (travis)
|
||||
* Instantaneous velocity now available
|
||||
* Contributors: Jacob Perron
|
||||
238
CMakeLists.txt
238
CMakeLists.txt
|
|
@ -1,227 +1,45 @@
|
|||
#########
|
||||
# Setup #
|
||||
#########
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(libcreate)
|
||||
|
||||
cmake_minimum_required(VERSION 3.25)
|
||||
find_package(Boost REQUIRED system thread)
|
||||
|
||||
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
|
||||
## Specify additional locations of header files
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
# Declare cpp library
|
||||
add_library(${LIBRARY_NAME} SHARED
|
||||
## Declare cpp library
|
||||
add_library(create
|
||||
src/create.cpp
|
||||
src/serial.cpp
|
||||
src/serial_stream.cpp
|
||||
src/serial_query.cpp
|
||||
src/data.cpp
|
||||
src/packet.cpp
|
||||
src/types.cpp
|
||||
)
|
||||
|
||||
# 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}
|
||||
target_link_libraries(create
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
# Declare example executables
|
||||
set(EXAMPLES
|
||||
battery_level
|
||||
bumpers
|
||||
cliffs
|
||||
drive_circle
|
||||
leds
|
||||
packets
|
||||
play_song
|
||||
wheeldrop
|
||||
## Declare example executables
|
||||
add_executable(create_demo examples/create_demo.cpp)
|
||||
add_executable(bumper_example examples/bumper_example.cpp)
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(create_demo
|
||||
${Boost_LIBRARIES}
|
||||
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:
|
||||
# * <prefix>/lib/
|
||||
# * <prefix>/bin/
|
||||
# * <prefix>/include/
|
||||
# * <prefix>/lib/cmake/<PROJECT-NAME>
|
||||
# * <prefix>/share/<PROJECT_NAME>
|
||||
set(LIB_INSTALL_DIR "lib")
|
||||
set(BIN_INSTALL_DIR "bin")
|
||||
set(INCLUDE_INSTALL_DIR "include")
|
||||
set(CONFIG_INSTALL_DIR "${LIB_INSTALL_DIR}/cmake/${PROJECT_NAME}")
|
||||
set(SHARE_INSTALL_DIR "share/${PROJECT_NAME}")
|
||||
|
||||
set(GENERATED_DIR "${CMAKE_CURRENT_BINARY_DIR}/generated")
|
||||
set(VERSION_CONFIG "${GENERATED_DIR}/${PROJECT_NAME}-config-version.cmake")
|
||||
set(PROJECT_CONFIG "${GENERATED_DIR}/${PROJECT_NAME}-config.cmake")
|
||||
set(TARGETS_EXPORT_NAME "${PROJECT_NAME}-targets")
|
||||
|
||||
include(CMakePackageConfigHelpers)
|
||||
|
||||
# Configure '<PROJECT-NAME>-config-version.cmake'
|
||||
write_basic_package_version_file(
|
||||
"${VERSION_CONFIG}"
|
||||
VERSION "${PACKAGE_VERSION}"
|
||||
COMPATIBILITY SameMajorVersion
|
||||
target_link_libraries(bumper_example
|
||||
${Boost_LIBRARIES}
|
||||
create
|
||||
)
|
||||
|
||||
# Configure '<PROJECT-NAME>-config.cmake'
|
||||
configure_package_config_file(
|
||||
"config.cmake.in"
|
||||
"${PROJECT_CONFIG}"
|
||||
INSTALL_DESTINATION "${CONFIG_INSTALL_DIR}"
|
||||
PATH_VARS
|
||||
INCLUDE_INSTALL_DIR
|
||||
LIBRARY_NAME
|
||||
)
|
||||
|
||||
###########
|
||||
# Install #
|
||||
###########
|
||||
|
||||
# Install targets
|
||||
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)
|
||||
## 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)
|
||||
|
|
|
|||
81
README.md
81
README.md
|
|
@ -1,76 +1,33 @@
|
|||
# libcreate #
|
||||
# libcreate
|
||||
|
||||
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.
|
||||
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).
|
||||
|
||||
* [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)
|
||||
* 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)
|
||||
|
||||
## Build Status ##
|
||||
|
||||
|
||||
|
||||
## Dependencies ##
|
||||
## 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
|
||||
|
||||
sudo apt-get install build-essential cmake libboost-system-dev libboost-thread-dev
|
||||
* `cmake CMakeLists.txt`
|
||||
* `make`
|
||||
* `sudo make install`
|
||||
|
||||
# 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
|
||||
## Example
|
||||
|
||||
#### Serial Permissions ####
|
||||
See source for examples.
|
||||
|
||||
Example compile line: `g++ create_demo.cpp -lcreate -lboost_system -lboost_thread`
|
||||
|
||||
User permission is requried to connect to Create over serial. You can add your user to the dialout group to get permission:
|
||||
## Bugs
|
||||
|
||||
sudo usermod -a -G dialout $USER
|
||||
* _Clock_ and _Schedule_ button presses are not detected. This is a known problem to the developers at iRobot.
|
||||
|
||||
Logout and login again for this to take effect.
|
||||
## Build Status
|
||||
|
||||
## Build ##
|
||||
|
||||
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()`
|
||||
|
||||
|
|
|
|||
|
|
@ -1,41 +0,0 @@
|
|||
# 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)
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
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()
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
@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@")
|
||||
|
|
@ -1,78 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file battery_level.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud))
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Switch to Full mode
|
||||
robot.setMode(create::MODE_FULL);
|
||||
|
||||
// Get battery capacity (max charge)
|
||||
const float battery_capacity = robot.getBatteryCapacity();
|
||||
|
||||
float battery_charge = 0.0f;
|
||||
while (true) {
|
||||
// Get battery charge
|
||||
battery_charge = robot.getBatteryCharge();
|
||||
|
||||
// Print battery percentage
|
||||
std::cout << "\rBattery level: " << (battery_charge / battery_capacity) * 100.0 << "%";
|
||||
|
||||
usleep(100000); // 10 Hz
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
73
examples/bumper_example.cpp
Normal file
73
examples/bumper_example.cpp
Normal file
|
|
@ -0,0 +1,73 @@
|
|||
#include "create/create.h"
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
create::RobotModel model = create::CREATE_2;
|
||||
|
||||
create::Create* robot = new create::Create(model);
|
||||
|
||||
// Attempt to connect to Create
|
||||
if (robot->connect(port, baud))
|
||||
std::cout << "Successfully connected to Create" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to Create on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
robot->setMode(create::MODE_FULL);
|
||||
|
||||
uint16_t signals[6];
|
||||
bool contact_bumpers[2];
|
||||
bool light_bumpers[6];
|
||||
|
||||
// Stop program when clean button is pressed
|
||||
while (!robot->isCleanButtonPressed()) {
|
||||
signals[0] = robot->getLightSignalLeft();
|
||||
signals[1] = robot->getLightSignalFrontLeft();
|
||||
signals[2] = robot->getLightSignalCenterLeft();
|
||||
signals[3] = robot->getLightSignalCenterRight();
|
||||
signals[4] = robot->getLightSignalFrontRight();
|
||||
signals[5] = robot->getLightSignalRight();
|
||||
|
||||
contact_bumpers[0] = robot->isLeftBumper();
|
||||
contact_bumpers[1] = robot->isRightBumper();
|
||||
|
||||
light_bumpers[0] = robot->isLightBumperLeft();
|
||||
light_bumpers[1] = robot->isLightBumperFrontLeft();
|
||||
light_bumpers[2] = robot->isLightBumperCenterLeft();
|
||||
light_bumpers[3] = robot->isLightBumperCenterRight();
|
||||
light_bumpers[4] = robot->isLightBumperFrontRight();
|
||||
light_bumpers[5] = robot->isLightBumperRight();
|
||||
|
||||
// print signals from left to right
|
||||
std::cout << "[ " << signals[0] << " , "
|
||||
<< signals[1] << " , "
|
||||
<< signals[2] << " , "
|
||||
<< signals[3] << " , "
|
||||
<< signals[4] << " , "
|
||||
<< signals[5]
|
||||
<< " ]" << std::endl;
|
||||
std::cout << "[ " << light_bumpers[0] << " , "
|
||||
<< light_bumpers[1] << " , "
|
||||
<< light_bumpers[2] << " , "
|
||||
<< light_bumpers[3] << " , "
|
||||
<< light_bumpers[4] << " , "
|
||||
<< light_bumpers[5]
|
||||
<< " ]" << std::endl;
|
||||
std::cout << "[ " << contact_bumpers[0] << " , "
|
||||
<< contact_bumpers[1]
|
||||
<< " ]" << std::endl;
|
||||
std::cout << std::endl;
|
||||
|
||||
usleep(1000 * 100); //10hz
|
||||
}
|
||||
|
||||
std::cout << "Stopping Create" << std::endl;
|
||||
|
||||
// Disconnect from robot
|
||||
robot->disconnect();
|
||||
delete robot;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,115 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file bumpers.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud))
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Switch to Full mode
|
||||
robot.setMode(create::MODE_FULL);
|
||||
|
||||
uint16_t light_signals[6] = {0, 0, 0, 0, 0, 0};
|
||||
bool light_bumpers[6] = {false, false, false, false, false, false};
|
||||
bool contact_bumpers[2] = {false, false};
|
||||
|
||||
while (true) {
|
||||
// Get light sensor data (only available for Create 2 or later robots)
|
||||
if (model == create::RobotModel::CREATE_2) {
|
||||
// Get raw light signal values
|
||||
light_signals[0] = robot.getLightSignalLeft();
|
||||
light_signals[1] = robot.getLightSignalFrontLeft();
|
||||
light_signals[2] = robot.getLightSignalCenterLeft();
|
||||
light_signals[3] = robot.getLightSignalCenterRight();
|
||||
light_signals[4] = robot.getLightSignalFrontRight();
|
||||
light_signals[5] = robot.getLightSignalRight();
|
||||
|
||||
// Get obstacle data from light sensors (true/false)
|
||||
light_bumpers[0] = robot.isLightBumperLeft();
|
||||
light_bumpers[1] = robot.isLightBumperFrontLeft();
|
||||
light_bumpers[2] = robot.isLightBumperCenterLeft();
|
||||
light_bumpers[3] = robot.isLightBumperCenterRight();
|
||||
light_bumpers[4] = robot.isLightBumperFrontRight();
|
||||
light_bumpers[5] = robot.isLightBumperRight();
|
||||
}
|
||||
|
||||
// Get state of bumpers
|
||||
contact_bumpers[0] = robot.isLeftBumper();
|
||||
contact_bumpers[1] = robot.isRightBumper();
|
||||
|
||||
// Print signals from left to right
|
||||
std::cout << "[ " << light_signals[0] << " , "
|
||||
<< light_signals[1] << " , "
|
||||
<< light_signals[2] << " , "
|
||||
<< light_signals[3] << " , "
|
||||
<< light_signals[4] << " , "
|
||||
<< light_signals[5]
|
||||
<< " ]" << std::endl;
|
||||
std::cout << "[ " << light_bumpers[0] << " , "
|
||||
<< light_bumpers[1] << " , "
|
||||
<< light_bumpers[2] << " , "
|
||||
<< light_bumpers[3] << " , "
|
||||
<< light_bumpers[4] << " , "
|
||||
<< light_bumpers[5]
|
||||
<< " ]" << std::endl;
|
||||
std::cout << "[ " << contact_bumpers[0] << " , "
|
||||
<< contact_bumpers[1]
|
||||
<< " ]" << std::endl;
|
||||
std::cout << std::endl;
|
||||
|
||||
usleep(100000); // 10 Hz
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,86 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file cliffs.cpp
|
||||
\authors Jacob Perron <jacobmperron@gmail.com>
|
||||
\copyright Copyright (c) 2019, Jacob Perron, All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the copyright holder nor the names of its contributors
|
||||
may be used to endorse or promote products derived from this software
|
||||
without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud)) {
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Switch to Full mode
|
||||
robot.setMode(create::MODE_FULL);
|
||||
|
||||
while (true) {
|
||||
// Get cliff status
|
||||
const bool cliff_left = robot.isCliffLeft();
|
||||
const bool cliff_front_left = robot.isCliffFrontLeft();
|
||||
const bool cliff_front_right = robot.isCliffFrontRight();
|
||||
const bool cliff_right = robot.isCliffRight();
|
||||
|
||||
// Print status
|
||||
std::cout << "\rCliffs (left to right): [ " <<
|
||||
cliff_left <<
|
||||
", " <<
|
||||
cliff_front_left <<
|
||||
", " <<
|
||||
cliff_front_right <<
|
||||
", " <<
|
||||
cliff_right <<
|
||||
" ]";
|
||||
|
||||
usleep(10000); // 10 Hz
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
133
examples/create_demo.cpp
Normal file
133
examples/create_demo.cpp
Normal file
|
|
@ -0,0 +1,133 @@
|
|||
#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;
|
||||
}
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file drive_circle.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud))
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Switch to Full mode
|
||||
robot.setMode(create::MODE_FULL);
|
||||
|
||||
// There's a delay between switching modes and when the robot will accept drive commands
|
||||
usleep(100000);
|
||||
|
||||
// Command robot to drive a radius of 0.15 metres at 0.2 m/s
|
||||
robot.driveRadius(0.2, 0.15);
|
||||
|
||||
while (true) {
|
||||
// Get robot odometry and print
|
||||
const create::Pose pose = robot.getPose();
|
||||
|
||||
std::cout << std::fixed << std::setprecision(2) << "\rOdometry (x, y, yaw): ("
|
||||
<< pose.x << ", " << pose.y << ", " << pose.yaw << ") ";
|
||||
|
||||
usleep(10000); // 10 Hz
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,94 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file leds.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud))
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Switch to Full mode
|
||||
robot.setMode(create::MODE_FULL);
|
||||
|
||||
bool enable_check_robot_led = true;
|
||||
bool enable_debris_led = false;
|
||||
bool enable_dock_led = true;
|
||||
bool enable_spot_led = false;
|
||||
uint8_t power_led = 0;
|
||||
char digit_buffer[5];
|
||||
|
||||
while (true) {
|
||||
// Set LEDs
|
||||
robot.enableCheckRobotLED(enable_check_robot_led);
|
||||
robot.enableDebrisLED(enable_debris_led);
|
||||
robot.enableDockLED(enable_dock_led);
|
||||
robot.enableSpotLED(enable_spot_led);
|
||||
robot.setPowerLED(power_led);
|
||||
|
||||
// Set 7-segment displays
|
||||
const int len = sprintf(digit_buffer, "%d", power_led);
|
||||
for (int i = len; i < 4; i++) digit_buffer[i] = ' ';
|
||||
robot.setDigitsASCII(digit_buffer[0], digit_buffer[1], digit_buffer[2], digit_buffer[3]);
|
||||
|
||||
// Update LED values
|
||||
enable_check_robot_led = !enable_check_robot_led;
|
||||
enable_debris_led = !enable_debris_led;
|
||||
enable_dock_led = !enable_dock_led;
|
||||
enable_spot_led = !enable_spot_led;
|
||||
power_led++;
|
||||
|
||||
usleep(250000); // 5 Hz
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,72 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file packets.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud))
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
while (true) {
|
||||
// Get serial packet statistics
|
||||
const uint64_t total_packets = robot.getTotalPackets();
|
||||
const uint64_t num_corrupt_packets = robot.getNumCorruptPackets();
|
||||
|
||||
// Print packet stats
|
||||
std::cout << "\rTotal packets: " << total_packets << " Corrupt packets: " << num_corrupt_packets;
|
||||
|
||||
usleep(100000); // 10 Hz
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,93 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file play_song.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud))
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Switch to Full mode
|
||||
robot.setMode(create::MODE_FULL);
|
||||
|
||||
// Useful note defintions
|
||||
const uint8_t G = 55;
|
||||
const uint8_t AS = 58;
|
||||
const uint8_t DS = 51;
|
||||
const float half = 1.0f;
|
||||
const float quarter = 0.5f;
|
||||
const float dotted_eigth = 0.375f;
|
||||
const float sixteenth = 0.125f;
|
||||
|
||||
// Define a song
|
||||
const uint8_t song_len = 9;
|
||||
const uint8_t notes[song_len] = { G, G, G, DS, AS, G, DS, AS, G };
|
||||
const float durations[song_len] = { quarter, quarter, quarter, dotted_eigth, sixteenth, quarter, dotted_eigth, sixteenth, half };
|
||||
robot.defineSong(0, song_len, notes, durations);
|
||||
|
||||
// Sleep to provide time for song to register
|
||||
usleep(1000000);
|
||||
|
||||
std::cout << "Playing a song!" << std::endl;
|
||||
|
||||
// Request to play the song we just defined
|
||||
robot.playSong(0);
|
||||
|
||||
// Expect the song to take about four seconds
|
||||
usleep(4500000);
|
||||
|
||||
// Call disconnect to avoid leaving robot in Full mode
|
||||
robot.disconnect();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,85 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file wheeldrop.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Select robot. Assume Create 2 unless argument says otherwise
|
||||
create::RobotModel model = create::RobotModel::CREATE_2;
|
||||
std::string port = "/dev/ttyUSB0";
|
||||
int baud = 115200;
|
||||
if (argc > 1 && std::string(argv[1]) == "create1") {
|
||||
model = create::RobotModel::CREATE_1;
|
||||
baud = 57600;
|
||||
std::cout << "Running driver for Create 1" << std::endl;
|
||||
}
|
||||
else {
|
||||
std::cout << "Running driver for Create 2" << std::endl;
|
||||
}
|
||||
|
||||
// Construct robot object
|
||||
create::Create robot(model);
|
||||
|
||||
// Connect to robot
|
||||
if (robot.connect(port, baud))
|
||||
std::cout << "Connected to robot" << std::endl;
|
||||
else {
|
||||
std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Switch to Full mode
|
||||
robot.setMode(create::MODE_FULL);
|
||||
|
||||
while (true) {
|
||||
// Get wheeldrop status
|
||||
const bool wheeldrop_left = robot.isLeftWheeldrop();
|
||||
const bool wheeldrop_right = robot.isRightWheeldrop();
|
||||
|
||||
// Print status
|
||||
std::cout << "\rWheeldrop status (left and right): [ " <<
|
||||
wheeldrop_left <<
|
||||
", " <<
|
||||
wheeldrop_right <<
|
||||
" ]";
|
||||
|
||||
// If dropped, then make light red
|
||||
if (wheeldrop_left || wheeldrop_right)
|
||||
robot.setPowerLED(255); // Red
|
||||
else
|
||||
robot.setPowerLED(0); // Green
|
||||
|
||||
usleep(10000); // 10 Hz
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -32,15 +32,11 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
#ifndef CREATE_H
|
||||
#define CREATE_H
|
||||
|
||||
#include <boost/numeric/ublas/matrix.hpp>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <string>
|
||||
#include <unistd.h>
|
||||
#include <deque>
|
||||
|
||||
#include "create/serial_stream.h"
|
||||
#include "create/serial_query.h"
|
||||
#include "create/serial.h"
|
||||
#include "create/data.h"
|
||||
#include "create/types.h"
|
||||
#include "create/util.h"
|
||||
|
|
@ -48,8 +44,6 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
namespace create {
|
||||
class Create {
|
||||
private:
|
||||
typedef boost::numeric::ublas::matrix<float> Matrix;
|
||||
|
||||
enum CreateLED {
|
||||
LED_DEBRIS = 1,
|
||||
LED_SPOT = 2,
|
||||
|
|
@ -71,119 +65,71 @@ namespace create {
|
|||
uint8_t powerLED;
|
||||
uint8_t powerLEDIntensity;
|
||||
|
||||
CreateMode mode;
|
||||
|
||||
create::Pose pose;
|
||||
create::Vel vel;
|
||||
|
||||
uint32_t prevTicksLeft;
|
||||
uint32_t prevTicksRight;
|
||||
float totalLeftDist;
|
||||
float totalRightDist;
|
||||
float prevLeftVel;
|
||||
float prevRightVel;
|
||||
bool firstOnData;
|
||||
std::chrono::time_point<std::chrono::steady_clock> prevOnDataTime;
|
||||
std::deque<float> dtHistory;
|
||||
uint8_t dtHistoryLength;
|
||||
util::timestamp_t prevOnDataTime;
|
||||
|
||||
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();
|
||||
void init();
|
||||
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;
|
||||
void onData();
|
||||
|
||||
protected:
|
||||
std::shared_ptr<create::Data> data;
|
||||
std::shared_ptr<create::Serial> serial;
|
||||
boost::shared_ptr<create::Data> data;
|
||||
boost::shared_ptr<create::Serial> serial;
|
||||
|
||||
public:
|
||||
/**
|
||||
* \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.
|
||||
/* Default constructor.
|
||||
* Does not attempt to establish serial connection to Create.
|
||||
*/
|
||||
Create(RobotModel model = RobotModel::CREATE_2, bool install_signal_handler = true);
|
||||
Create(RobotModel = CREATE_2);
|
||||
|
||||
/**
|
||||
* \brief Attempts to establish serial connection to Create.
|
||||
*
|
||||
/* 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. 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.
|
||||
* \param model type of robot.
|
||||
*/
|
||||
Create(const std::string& port, const int& baud, RobotModel model = RobotModel::CREATE_2, bool install_signal_handler = true);
|
||||
Create(const std::string& port, const int& baud, RobotModel model = CREATE_2);
|
||||
|
||||
/**
|
||||
* \brief Attempts to disconnect from serial.
|
||||
*/
|
||||
~Create();
|
||||
|
||||
/**
|
||||
* \brief Resets the create as if the battery was removed and reinserted.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* \brief Make a serial connection to Create.
|
||||
*
|
||||
/* 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(); };
|
||||
|
||||
/**
|
||||
* \brief Disconnect from serial.
|
||||
/* Disconnect from serial.
|
||||
*/
|
||||
void disconnect();
|
||||
|
||||
/**
|
||||
* \brief Change Create mode.
|
||||
* \param mode to change Create to.
|
||||
/* Change Create mode.
|
||||
* \param mode to put Create in.
|
||||
* \return true if successful, false otherwise
|
||||
*/
|
||||
bool setMode(const create::CreateMode& mode);
|
||||
|
||||
/**
|
||||
* \brief Starts a cleaning mode.
|
||||
/* Starts a cleaning mode.
|
||||
* Changes mode to MODE_PASSIVE.
|
||||
* \return true if successful, false otherwise
|
||||
*/
|
||||
bool clean(const create::CleanMode& mode = CLEAN_DEFAULT);
|
||||
|
||||
/**
|
||||
* \brief Starts the docking behaviour.
|
||||
/* Starts the docking behaviour.
|
||||
* Changes mode to MODE_PASSIVE.
|
||||
* \return true if successful, false otherwise
|
||||
*/
|
||||
bool dock() const;
|
||||
|
||||
/**
|
||||
* \brief Sets the internal clock of Create.
|
||||
/* Sets the internal clock of Create.
|
||||
* \param day in range [0, 6]
|
||||
* \param hour in range [0, 23]
|
||||
* \param min in range [0, 59]
|
||||
|
|
@ -191,8 +137,7 @@ namespace create {
|
|||
*/
|
||||
bool setDate(const create::DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const;
|
||||
|
||||
/**
|
||||
* \brief Set the average wheel velocity and turning radius of Create.
|
||||
/* 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,
|
||||
|
|
@ -200,55 +145,41 @@ 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);
|
||||
bool driveRadius(const float& velocity, const float& radius) const;
|
||||
|
||||
/**
|
||||
* \brief Set the velocities for the left and right wheels.
|
||||
/* 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);
|
||||
bool driveWheels(const float& leftWheel, const float& rightWheel) const;
|
||||
|
||||
/**
|
||||
* \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.
|
||||
/* 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);
|
||||
bool drive(const float& xVel, const float& angularVel) const;
|
||||
|
||||
/**
|
||||
* \brief Set the power to the side brush motor.
|
||||
/* 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);
|
||||
|
||||
/**
|
||||
* \brief Set the power to the main brush motor.
|
||||
/* 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);
|
||||
|
||||
/**
|
||||
* \brief Set the power to the vacuum motor.
|
||||
/* 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);
|
||||
|
||||
/**
|
||||
* \brief Set the power of all motors.
|
||||
/* 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]
|
||||
|
|
@ -256,65 +187,55 @@ namespace create {
|
|||
*/
|
||||
bool setAllMotors(const float& mainPower, const float& sidePower, const float& vacuumPower);
|
||||
|
||||
/**
|
||||
* \brief Set the blue "debris" LED on/off.
|
||||
/* Set the blue "debris" LED on/off.
|
||||
* \param enable
|
||||
* \return true if successful, false otherwise
|
||||
*/
|
||||
bool enableDebrisLED(const bool& enable);
|
||||
|
||||
/**
|
||||
* \brief Set the green "spot" LED on/off.
|
||||
/* Set the green "spot" LED on/off.
|
||||
* \param enable
|
||||
* \return true if successful, false otherwise
|
||||
*/
|
||||
bool enableSpotLED(const bool& enable);
|
||||
|
||||
/**
|
||||
* \brief Set the green "dock" LED on/off.
|
||||
/* Set the green "dock" LED on/off.
|
||||
* \param enable
|
||||
* \return true if successful, false otherwise
|
||||
*/
|
||||
bool enableDockLED(const bool& enable);
|
||||
|
||||
/**
|
||||
* \brief Set the orange "check Create" LED on/off.
|
||||
/* Set the orange "check Create" LED on/off.
|
||||
* \param enable
|
||||
* \return true if successful, false otherwise
|
||||
*/
|
||||
bool enableCheckRobotLED(const bool& enable);
|
||||
|
||||
/**
|
||||
* \brief Set the center power LED.
|
||||
/* 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);
|
||||
|
||||
/**
|
||||
* \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
|
||||
/* Set the four 7-segment display digits from left to right.
|
||||
* \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:
|
||||
*
|
||||
* <pre>
|
||||
0 7 14 21
|
||||
|‾‾‾| |‾‾‾| |‾‾‾| |‾‾‾|
|
||||
1 |___| 2 8 |___| 9 15 |___| 16 22 |___| 23
|
||||
| 3 | | 10| | 17| | 24|
|
||||
4 |___| 5 11|___| 12 18 |___| 19 25 |___| 26
|
||||
6 13 20 27
|
||||
</pre>
|
||||
* 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
|
||||
*/
|
||||
bool setDigits(const std::vector<bool>& segments) const;
|
||||
//TODO (https://github.com/AutonomyLab/libcreate/issues/7)
|
||||
//bool setDigits(const std::vector<bool>& segments) const;
|
||||
|
||||
/**
|
||||
* \brief Set the four 7-segment display digits from left to right with ASCII codes.
|
||||
/* 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]
|
||||
|
|
@ -325,8 +246,7 @@ namespace create {
|
|||
bool setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
|
||||
const uint8_t& digit3, const uint8_t& digit4) const;
|
||||
|
||||
/**
|
||||
* \brief Defines a song from the provided notes and labels it with a song number.
|
||||
/* 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.
|
||||
|
|
@ -340,383 +260,208 @@ namespace create {
|
|||
const uint8_t* notes,
|
||||
const float* durations) const;
|
||||
|
||||
/**
|
||||
* \brief Play a previously created song.
|
||||
/* 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;
|
||||
|
||||
/**
|
||||
* \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.
|
||||
/* True if a left or right wheeldrop is detected.
|
||||
*/
|
||||
bool isWheeldrop() const;
|
||||
|
||||
/**
|
||||
* \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.
|
||||
/* Returns true if left bumper is pressed, false otherwise.
|
||||
*/
|
||||
bool isLeftBumper() const;
|
||||
|
||||
/**
|
||||
* \return true if right bumper is pressed, false otherwise.
|
||||
/* Returns true if right bumper is pressed, false otherwise.
|
||||
*/
|
||||
bool isRightBumper() const;
|
||||
|
||||
/**
|
||||
* \return true if wall is seen to right of Create, false otherwise.
|
||||
/* True if wall is seen to right of Create, false otherwise.
|
||||
*/
|
||||
bool isWall() const;
|
||||
|
||||
/**
|
||||
* \return true if there are any cliff detections, false otherwise.
|
||||
/* True if there are any cliff detections, false otherwise.
|
||||
*/
|
||||
bool isCliff() const;
|
||||
|
||||
/**
|
||||
* \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.
|
||||
/* True if there is a virtual wall signal is being received.
|
||||
*/
|
||||
bool isVirtualWall() 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 isWheelOvercurrent() 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 isMainBrushOvercurrent() const;
|
||||
|
||||
/**
|
||||
* \todo Not implemented yet (https://github.com/AutonomyLab/libcreate/issues/8)
|
||||
* \return true if side brush motor is overcurrent.
|
||||
*/
|
||||
bool isSideBrushOvercurrent() const;
|
||||
//TODO (https://github.com/AutonomyLab/libcreate/issues/8)
|
||||
//bool isSideBrushOvercurrent() const;
|
||||
|
||||
/**
|
||||
* \brief Get level of the dirt detect sensor.
|
||||
/* Get level of the dirt detect sensor.
|
||||
* \return value in range [0, 255]
|
||||
*/
|
||||
uint8_t getDirtDetect() const;
|
||||
|
||||
/**
|
||||
* \brief Get value of 8-bit IR character currently being received by omnidirectional sensor.
|
||||
/* Get value of 8-bit IR character currently being received by omnidirectional sensor.
|
||||
* \return value in range [0, 255]
|
||||
*/
|
||||
uint8_t getIROmni() const;
|
||||
|
||||
/**
|
||||
* \brief Get value of 8-bit IR character currently being received by left sensor.
|
||||
/* Get value of 8-bit IR character currently being received by left sensor.
|
||||
* \return value in range [0, 255]
|
||||
*/
|
||||
|
||||
uint8_t getIRLeft() const;
|
||||
|
||||
/**
|
||||
* \brief Get value of 8-bit IR character currently being received by right sensor.
|
||||
/* Get value of 8-bit IR character currently being received by right sensor.
|
||||
* \return value in range [0, 255]
|
||||
*/
|
||||
uint8_t getIRRight() const;
|
||||
|
||||
/**
|
||||
* \brief Get state of 'clean' button ('play' button on Create 1).
|
||||
* \return true if button is pressed, false otherwise.
|
||||
/* Get state of 'clean' button ('play' button on Create 1).
|
||||
*/
|
||||
bool isCleanButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Not supported by any firmware!
|
||||
/* Not supported by any firmware!
|
||||
*/
|
||||
bool isClockButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Not supported by any firmware!
|
||||
/* Not supported by any firmware!
|
||||
*/
|
||||
bool isScheduleButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Get state of 'day' button.
|
||||
* \return true if button is pressed, false otherwise.
|
||||
/* Get state of 'day' button.
|
||||
*/
|
||||
bool isDayButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Get state of 'hour' button.
|
||||
* \return true if button is pressed, false otherwise.
|
||||
/* Get state of 'hour' button.
|
||||
*/
|
||||
bool isHourButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Get state of 'min' button.
|
||||
* \return true if button is pressed, false otherwise.
|
||||
/* Get state of 'min' button.
|
||||
*/
|
||||
bool isMinButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Get state of 'dock' button ('advance' button on Create 1).
|
||||
* \return true if button is pressed, false otherwise.
|
||||
/* Get state of 'dock' button ('advance' button on Create 1).
|
||||
*/
|
||||
bool isDockButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Get state of 'spot' button.
|
||||
* \return true if button is pressed, false otherwise.
|
||||
/* Get state of 'spot' button.
|
||||
*/
|
||||
bool isSpotButtonPressed() const;
|
||||
|
||||
/**
|
||||
* \brief Get battery voltage.
|
||||
/* Get battery voltage.
|
||||
* \return value in volts
|
||||
*/
|
||||
float getVoltage() const;
|
||||
|
||||
/**
|
||||
* \brief Get current flowing in/out of battery.
|
||||
/* Get current flowing in/out of battery.
|
||||
* A positive current implies Create is charging.
|
||||
* \return value in amps
|
||||
*/
|
||||
float getCurrent() const;
|
||||
|
||||
/**
|
||||
* \brief Get the temperature of battery.
|
||||
/* Get the temperature of battery.
|
||||
* \return value in Celsius
|
||||
*/
|
||||
int8_t getTemperature() const;
|
||||
|
||||
/**
|
||||
* \brief Get battery's remaining charge.
|
||||
/* Get battery's remaining charge.
|
||||
* \return value in amp-hours
|
||||
*/
|
||||
float getBatteryCharge() const;
|
||||
|
||||
/**
|
||||
* \brief Get estimated battery charge capacity.
|
||||
/* 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;
|
||||
|
||||
/**
|
||||
* \brief Get the signal strength from the left light sensor.
|
||||
/* Return the signal strength from the left light sensor.
|
||||
* \return value in range [0, 4095]
|
||||
*/
|
||||
uint16_t getLightSignalLeft() const;
|
||||
|
||||
/**
|
||||
* \brief Get the signal strength from the front-left light sensor.
|
||||
/* Return 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 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 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 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 the signal strength from the center-right light sensor.
|
||||
* \return value in range [0, 4095]
|
||||
*/
|
||||
uint16_t getLightSignalCenterRight() const;
|
||||
|
||||
/**
|
||||
* \return true if Create is moving forward, false otherwise.
|
||||
/* Return true if Create is moving forward, false otherwise.
|
||||
*/
|
||||
bool isMovingForward() const;
|
||||
|
||||
/**
|
||||
* \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.
|
||||
/* Get the current charging state.
|
||||
*/
|
||||
create::ChargingState getChargingState() const;
|
||||
|
||||
/**
|
||||
* \brief Get the current mode reported by Create.
|
||||
* \return mode.
|
||||
/* Get the current mode reported by Create.
|
||||
*/
|
||||
create::CreateMode getMode();
|
||||
create::CreateMode getMode() const;
|
||||
|
||||
/**
|
||||
* \brief Get the estimated pose of Create based on wheel encoders.
|
||||
* \return pose (x-y position in meters and yaw angle in Radians)
|
||||
/* Get the estimated position of Create based on wheel encoders.
|
||||
*/
|
||||
create::Pose getPose() const;
|
||||
const create::Pose& getPose() const;
|
||||
|
||||
/**
|
||||
* \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)
|
||||
/* Get the estimated velocity of Create based on wheel encoders.
|
||||
*/
|
||||
create::Vel getVel() const;
|
||||
const create::Vel& getVel() const;
|
||||
|
||||
/**
|
||||
* \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.
|
||||
/* Get the number of corrupt serial packets since first connecting to Create.
|
||||
*/
|
||||
uint64_t getNumCorruptPackets() const;
|
||||
|
||||
/**
|
||||
* \brief Get the total number of serial packets received (including corrupt packets) since first connecting to Create.
|
||||
* \return total number of serial packets.
|
||||
/* Get the total number of serial packets (including corrupt packets) since first connecting to Create.
|
||||
*/
|
||||
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
|
||||
|
|
|
|||
|
|
@ -32,8 +32,9 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
#ifndef CREATE_DATA_H
|
||||
#define CREATE_DATA_H
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "create/packet.h"
|
||||
|
|
@ -42,16 +43,16 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
namespace create {
|
||||
class Data {
|
||||
private:
|
||||
std::map<uint8_t, std::shared_ptr<Packet> > packets;
|
||||
std::map<uint8_t, boost::shared_ptr<Packet> > packets;
|
||||
uint32_t totalDataBytes;
|
||||
std::vector<uint8_t> ids;
|
||||
|
||||
public:
|
||||
Data(ProtocolVersion version = V_3);
|
||||
Data(RobotModel = CREATE_2);
|
||||
~Data();
|
||||
|
||||
bool isValidPacketID(const uint8_t id) const;
|
||||
std::shared_ptr<Packet> getPacket(const uint8_t id);
|
||||
boost::shared_ptr<Packet> getPacket(const uint8_t id);
|
||||
void validateAll();
|
||||
uint32_t getTotalDataBytes() const;
|
||||
uint8_t getNumPackets() const;
|
||||
|
|
|
|||
|
|
@ -31,20 +31,15 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
#ifndef CREATE_PACKET_H
|
||||
#define CREATE_PACKET_H
|
||||
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
namespace create {
|
||||
class Packet {
|
||||
private:
|
||||
uint16_t data;
|
||||
uint16_t tmpData;
|
||||
mutable std::mutex dataMutex;
|
||||
mutable std::mutex tmpDataMutex;
|
||||
|
||||
protected:
|
||||
// Thread safe
|
||||
void setData(const uint16_t& d);
|
||||
mutable boost::mutex dataMutex;
|
||||
mutable boost::mutex tmpDataMutex;
|
||||
|
||||
public:
|
||||
const uint8_t nbytes;
|
||||
|
|
@ -53,11 +48,10 @@ namespace create {
|
|||
Packet(const uint8_t& nbytes, const std::string& info);
|
||||
~Packet();
|
||||
|
||||
// Thread safe
|
||||
void setDataToValidate(const uint16_t& td);
|
||||
// Thread safe
|
||||
// All of the following are thread safe
|
||||
void setTempData(const uint16_t& td);
|
||||
void validate();
|
||||
// Thread safe
|
||||
void setData(const uint16_t& d);
|
||||
uint16_t getData() const;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -35,62 +35,66 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
#ifndef CREATE_SERIAL_H
|
||||
#define CREATE_SERIAL_H
|
||||
|
||||
#include <condition_variable>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include <boost/asio.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/condition_variable.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include "create/data.h"
|
||||
#include "create/types.h"
|
||||
#include "create/util.h"
|
||||
|
||||
namespace create {
|
||||
class Serial : public std::enable_shared_from_this<Serial> {
|
||||
|
||||
protected:
|
||||
boost::asio::io_service io;
|
||||
boost::asio::signal_set signals;
|
||||
boost::asio::serial_port port;
|
||||
|
||||
class Serial {
|
||||
private:
|
||||
std::thread ioThread;
|
||||
std::condition_variable dataReadyCond;
|
||||
std::mutex dataReadyMut;
|
||||
enum ReadState {
|
||||
READ_HEADER,
|
||||
READ_NBYTES,
|
||||
READ_PACKET_ID,
|
||||
READ_PACKET_BYTES,
|
||||
READ_CHECKSUM
|
||||
};
|
||||
|
||||
boost::shared_ptr<Data> data;
|
||||
boost::asio::io_service io;
|
||||
boost::asio::serial_port port;
|
||||
boost::thread ioThread;
|
||||
boost::condition_variable dataReadyCond;
|
||||
boost::mutex dataReadyMut;
|
||||
ReadState readState;
|
||||
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
|
||||
std::function<void()> callback;
|
||||
boost::function<void()> callback;
|
||||
// Start and stop reading data from Create
|
||||
bool startReading();
|
||||
void stopReading();
|
||||
bool openPort(const std::string& portName, const int& baud);
|
||||
bool closePort();
|
||||
|
||||
protected:
|
||||
std::shared_ptr<Data> data;
|
||||
// These are for possible diagnostics
|
||||
uint64_t corruptPackets;
|
||||
uint64_t totalPackets;
|
||||
|
||||
virtual bool startSensorStream() = 0;
|
||||
virtual void processByte(uint8_t byteRead) = 0;
|
||||
|
||||
void signalHandler(const boost::system::error_code& error, int signal_number);
|
||||
// Notifies main thread that data is fresh and makes the user callback
|
||||
void notifyDataReady();
|
||||
|
||||
public:
|
||||
Serial(std::shared_ptr<Data> data, bool install_signal_handler);
|
||||
Serial(boost::shared_ptr<Data> data, const uint8_t& header = create::util::CREATE_2_HEADER);
|
||||
~Serial();
|
||||
bool connect(const std::string& port, const int& baud = 115200, std::function<void()> cb = 0);
|
||||
bool connect(const std::string& port, const int& baud = 115200, boost::function<void()> cb = 0);
|
||||
void disconnect();
|
||||
inline bool connected() const { return port.is_open(); };
|
||||
bool send(const uint8_t* bytes, const uint32_t numBytes);
|
||||
|
|
|
|||
|
|
@ -1,75 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file serial_query.h
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\authors Ben Wolsieffer <benwolsieffer@gmail.com>
|
||||
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
// Based on example from:
|
||||
// https://github.com/labust/labust-ros-pkg/wiki/Create-a-Serial-Port-application
|
||||
|
||||
#ifndef CREATE_SERIAL_QUERY_H
|
||||
#define CREATE_SERIAL_QUERY_H
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <boost/asio.hpp>
|
||||
|
||||
#include "create/data.h"
|
||||
#include "create/types.h"
|
||||
#include "create/util.h"
|
||||
#include "create/serial.h"
|
||||
|
||||
namespace create {
|
||||
class SerialQuery : public Serial {
|
||||
|
||||
private:
|
||||
boost::asio::deadline_timer streamRecoveryTimer;
|
||||
uint8_t packetID;
|
||||
int8_t packetByte;
|
||||
uint16_t packetData;
|
||||
const uint8_t maxPacketID;
|
||||
|
||||
bool started;
|
||||
|
||||
void requestSensorData();
|
||||
void restartSensorStream(const boost::system::error_code& err);
|
||||
|
||||
void flushInput();
|
||||
|
||||
protected:
|
||||
bool startSensorStream();
|
||||
void processByte(uint8_t byteRead);
|
||||
|
||||
public:
|
||||
SerialQuery(std::shared_ptr<Data> data, bool install_signal_handler = true);
|
||||
virtual ~SerialQuery() = default;
|
||||
};
|
||||
} // namespace create
|
||||
|
||||
#endif // CREATE_SERIAL_H
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file serial_stream.h
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
// Based on example from:
|
||||
// https://github.com/labust/labust-ros-pkg/wiki/Create-a-Serial-Port-application
|
||||
|
||||
#ifndef CREATE_SERIAL_STREAM_H
|
||||
#define CREATE_SERIAL_STREAM_H
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "create/data.h"
|
||||
#include "create/types.h"
|
||||
#include "create/util.h"
|
||||
#include "create/serial.h"
|
||||
|
||||
namespace create {
|
||||
class SerialStream : public Serial {
|
||||
private:
|
||||
enum ReadState {
|
||||
READ_HEADER,
|
||||
READ_NBYTES,
|
||||
READ_PACKET_ID,
|
||||
READ_PACKET_BYTES,
|
||||
READ_CHECKSUM
|
||||
};
|
||||
|
||||
// State machine variables
|
||||
ReadState readState;
|
||||
uint8_t headerByte;
|
||||
uint8_t packetID;
|
||||
uint8_t expectedNumBytes;
|
||||
uint16_t packetBytes;
|
||||
uint8_t numBytesRead;
|
||||
uint32_t byteSum;
|
||||
uint8_t numDataBytesRead;
|
||||
uint8_t expectedNumDataBytes;
|
||||
|
||||
protected:
|
||||
bool startSensorStream();
|
||||
void processByte(uint8_t byteRead);
|
||||
|
||||
public:
|
||||
SerialStream(
|
||||
std::shared_ptr<Data> data,
|
||||
const uint8_t& header = create::util::STREAM_HEADER,
|
||||
bool install_signal_handler = true);
|
||||
virtual ~SerialStream() = default;
|
||||
|
||||
};
|
||||
} // namespace create
|
||||
|
||||
#endif // CREATE_SERIAL_H
|
||||
|
|
@ -32,56 +32,10 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
#ifndef CREATE_TYPES_H
|
||||
#define CREATE_TYPES_H
|
||||
|
||||
#include <vector>
|
||||
#include <stdint.h>
|
||||
#include <string>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace create {
|
||||
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 RobotModel {
|
||||
CREATE_1 = 0, // Roomba 400 series
|
||||
CREATE_2 // Roomba 600 series
|
||||
};
|
||||
|
||||
enum SensorPacketID {
|
||||
|
|
@ -104,8 +58,8 @@ namespace create {
|
|||
ID_CLIFF_RIGHT = 12,
|
||||
ID_VIRTUAL_WALL = 13,
|
||||
ID_OVERCURRENTS = 14,
|
||||
ID_DIRT_DETECT_LEFT = 15,
|
||||
ID_DIRT_DETECT_RIGHT = 16,
|
||||
ID_DIRT_DETECT = 15,
|
||||
ID_UNUSED_1 = 16,
|
||||
ID_IR_OMNI = 17,
|
||||
ID_IR_LEFT = 52,
|
||||
ID_IR_RIGHT = 53,
|
||||
|
|
@ -123,8 +77,8 @@ namespace create {
|
|||
ID_CLIFF_FRONT_LEFT_SIGNAL = 29,
|
||||
ID_CLIFF_FRONT_RIGHT_SIGNAL = 30,
|
||||
ID_CLIFF_RIGHT_SIGNAL = 31,
|
||||
ID_CARGO_BAY_DIGITAL_INPUTS = 32,
|
||||
ID_CARGO_BAY_ANALOG_SIGNAL = 33,
|
||||
ID_UNUSED_2 = 32,
|
||||
ID_UNUSED_3 = 33,
|
||||
ID_CHARGE_SOURCE = 34,
|
||||
ID_OI_MODE = 35,
|
||||
ID_SONG_NUM = 36,
|
||||
|
|
@ -156,7 +110,6 @@ namespace create {
|
|||
OC_RESET = 7,
|
||||
OC_STOP = 173,
|
||||
OC_BAUD = 129,
|
||||
OC_CONTROL = 130,
|
||||
OC_SAFE = 131,
|
||||
OC_FULL = 132,
|
||||
OC_CLEAN = 135,
|
||||
|
|
@ -181,7 +134,7 @@ namespace create {
|
|||
OC_SENSORS= 142,
|
||||
OC_QUERY_LIST=149,
|
||||
OC_STREAM = 148,
|
||||
OC_TOGGLE_STREAM = 150
|
||||
OC_TOGGLE_STREAM = 150,
|
||||
};
|
||||
|
||||
enum BAUDCODE {
|
||||
|
|
@ -200,10 +153,10 @@ namespace create {
|
|||
};
|
||||
|
||||
enum CreateMode {
|
||||
MODE_OFF = 0,
|
||||
MODE_PASSIVE = 1,
|
||||
MODE_SAFE = 2,
|
||||
MODE_FULL = 3,
|
||||
MODE_OFF = OC_POWER,
|
||||
MODE_PASSIVE = OC_START,
|
||||
MODE_SAFE = OC_SAFE,
|
||||
MODE_FULL = OC_FULL,
|
||||
MODE_UNAVAILABLE = -1
|
||||
};
|
||||
|
||||
|
|
@ -269,18 +222,10 @@ namespace create {
|
|||
IR_CHAR_VIRTUAL_WALL = 162
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Represents a robot pose.
|
||||
*/
|
||||
struct Pose {
|
||||
float x;
|
||||
float y;
|
||||
float yaw;
|
||||
|
||||
/**
|
||||
* \brief 3x3 covariance matrix in row-major order.
|
||||
*/
|
||||
std::vector<float> covariance;
|
||||
};
|
||||
|
||||
typedef Pose Vel;
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
#ifndef CREATE_UTIL_H
|
||||
#define CREATE_UTIL_H
|
||||
|
||||
#include <cmath>
|
||||
#include <sys/time.h>
|
||||
|
||||
#define COUT(prefix,msg) (std::cout<<prefix<<msg<<std::endl)
|
||||
#define CERR(prefix,msg) (std::cerr<<prefix<<msg<<std::endl)
|
||||
|
|
@ -40,12 +40,15 @@ POSSIBILITY OF SUCH DAMAGE.
|
|||
namespace create {
|
||||
namespace util {
|
||||
|
||||
static const uint8_t STREAM_HEADER = 19;
|
||||
static const float V_3_TICKS_PER_REV = 508.8;
|
||||
static const uint32_t V_3_MAX_ENCODER_TICKS = 65535;
|
||||
static const float MAX_RADIUS = 2.0;
|
||||
static const float STRAIGHT_RADIUS = 32.768;
|
||||
static const float IN_PLACE_RADIUS = 0.001;
|
||||
static const uint8_t CREATE_2_HEADER = 19;
|
||||
static const float CREATE_2_AXLE_LENGTH = 0.235;
|
||||
static const float CREATE_2_TICKS_PER_REV = 508.8;
|
||||
static const uint32_t CREATE_2_MAX_ENCODER_TICKS = 65535;
|
||||
static const float CREATE_2_WHEEL_DIAMETER = 0.078;
|
||||
static const float CREATE_2_MAX_VEL = 0.5;
|
||||
static const float CREATE_2_MAX_RADIUS = 2.0;
|
||||
static const float CREATE_2_STRAIGHT_RADIUS = 32.768;
|
||||
static const float CREATE_2_IN_PLACE_RADUIS = 0.001;
|
||||
static const float PI = 3.14159;
|
||||
static const float TWO_PI = 6.28318;
|
||||
static const float EPS = 0.0001;
|
||||
|
|
@ -55,10 +58,16 @@ namespace create {
|
|||
while (a < -PI) a += TWO_PI;
|
||||
while (a > PI) a -= TWO_PI;
|
||||
return a;
|
||||
}
|
||||
};
|
||||
|
||||
inline bool willFloatOverflow(const float a, const float b) {
|
||||
return ( (a < 0.0) == (b < 0.0) && std::abs(b) > std::numeric_limits<float>::max() - std::abs(a) );
|
||||
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;
|
||||
}
|
||||
} // namespace util
|
||||
} // namespace create
|
||||
|
|
|
|||
|
|
@ -1,7 +0,0 @@
|
|||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
\b libcreate is a C++ library for interfacing with iRobot's Create mobile robot platforms and various Roomba models.
|
||||
|
||||
*/
|
||||
30
package.xml
30
package.xml
|
|
@ -1,30 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>libcreate</name>
|
||||
<version>3.1.0</version>
|
||||
<description>C++ library for interfacing with iRobot's Create 1 and Create 2</description>
|
||||
|
||||
<maintainer email="jacobmperron@gmail.com">Jacob Perron</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://wiki.ros.org/libcreate</url>
|
||||
<url type="repository">https://github.com/AutonomyLab/libcreate</url>
|
||||
<url type="bugtracker">https://github.com/AutonomyLab/libcreate/issues</url>
|
||||
|
||||
<author email="jacobmperron@gmail.com">Jacob Perron</author>
|
||||
|
||||
<buildtool_depend>cmake</buildtool_depend>
|
||||
|
||||
<depend>boost</depend>
|
||||
|
||||
<exec_depend condition="$ROS_VERSION == 1">catkin</exec_depend>
|
||||
|
||||
<doc_depend>doxygen</doc_depend>
|
||||
|
||||
<test_depend>gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
652
src/create.cpp
652
src/create.cpp
|
|
@ -1,20 +1,20 @@
|
|||
#include <boost/bind.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <ctime>
|
||||
#include <memory>
|
||||
#include <assert.h>
|
||||
|
||||
#include "create/create.h"
|
||||
|
||||
#define GET_DATA(id) (data->getPacket(id)->getData())
|
||||
#define BOUND_CONST(val,min,max) (val<min?min:(val>max?max:val))
|
||||
#define BOUND(val,min,max) (val = BOUND_CONST(val,min,max))
|
||||
#define BOUND(val,min,max) (val<min?val=min:(val>max?val=max:val=val))
|
||||
|
||||
namespace create {
|
||||
|
||||
namespace ublas = boost::numeric::ublas;
|
||||
// TODO: Handle SIGINT to do clean disconnect
|
||||
|
||||
void Create::init(bool install_signal_handler) {
|
||||
void Create::init() {
|
||||
mainMotorPower = 0;
|
||||
sideMotorPower = 0;
|
||||
vacuumMotorPower = 0;
|
||||
|
|
@ -26,40 +26,23 @@ namespace create {
|
|||
powerLEDIntensity = 0;
|
||||
prevTicksLeft = 0;
|
||||
prevTicksRight = 0;
|
||||
totalLeftDist = 0.0;
|
||||
totalRightDist = 0.0;
|
||||
firstOnData = true;
|
||||
mode = MODE_OFF;
|
||||
pose.x = 0;
|
||||
pose.y = 0;
|
||||
pose.yaw = 0;
|
||||
pose.covariance = std::vector<float>(9, 0.0);
|
||||
vel.x = 0;
|
||||
vel.y = 0;
|
||||
vel.yaw = 0;
|
||||
vel.covariance = std::vector<float>(9, 0.0);
|
||||
poseCovar = Matrix(3, 3, 0.0);
|
||||
requestedLeftVel = 0;
|
||||
requestedRightVel = 0;
|
||||
dtHistoryLength = 100;
|
||||
modeReportWorkaround = false;
|
||||
data = std::shared_ptr<Data>(new Data(model.getVersion()));
|
||||
if (model.getVersion() == V_1) {
|
||||
serial = std::make_shared<SerialQuery>(data, install_signal_handler);
|
||||
} else {
|
||||
serial = std::make_shared<SerialStream>(
|
||||
data, create::util::STREAM_HEADER, install_signal_handler);
|
||||
}
|
||||
data = boost::shared_ptr<Data>(new Data(model));
|
||||
serial = boost::make_shared<Serial>(data);
|
||||
}
|
||||
|
||||
Create::Create(RobotModel m, bool install_signal_handler) : model(m) {
|
||||
init(install_signal_handler);
|
||||
Create::Create(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);
|
||||
Create::Create(const std::string& dev, const int& baud, RobotModel m) : model(m) {
|
||||
init();
|
||||
serial->connect(dev, baud);
|
||||
}
|
||||
|
||||
|
|
@ -67,235 +50,80 @@ namespace create {
|
|||
disconnect();
|
||||
}
|
||||
|
||||
void Create::reset() {
|
||||
if (!connected()) {
|
||||
CERR("[create::Serial] ", "send failed, not connected.");
|
||||
return;
|
||||
}
|
||||
serial->sendOpcode(OC_START);
|
||||
serial->sendOpcode(OC_RESET);
|
||||
}
|
||||
|
||||
Create::Matrix Create::addMatrices(const Matrix &A, const Matrix &B) const {
|
||||
size_t rows = A.size1();
|
||||
size_t cols = A.size2();
|
||||
|
||||
assert(rows == B.size1());
|
||||
assert(cols == B.size2());
|
||||
|
||||
Matrix C(rows, cols);
|
||||
for (size_t i = 0u; i < rows; i++) {
|
||||
for (size_t j = 0u; j < cols; j++) {
|
||||
const float a = A(i, j);
|
||||
const float b = B(i, j);
|
||||
if (util::willFloatOverflow(a, b)) {
|
||||
// If overflow, set to float min or max depending on direction of overflow
|
||||
C(i, j) = (a < 0.0) ? std::numeric_limits<float>::min() : std::numeric_limits<float>::max();
|
||||
}
|
||||
else {
|
||||
C(i, j) = a + b;
|
||||
}
|
||||
}
|
||||
}
|
||||
return C;
|
||||
}
|
||||
|
||||
void Create::onData() {
|
||||
if (firstOnData) {
|
||||
if (model.getVersion() >= V_3) {
|
||||
if (model == CREATE_2) {
|
||||
// Initialize tick counts
|
||||
prevTicksLeft = GET_DATA(ID_LEFT_ENC);
|
||||
prevTicksRight = GET_DATA(ID_RIGHT_ENC);
|
||||
}
|
||||
prevOnDataTime = std::chrono::steady_clock::now();
|
||||
prevOnDataTime = util::getTimestamp();
|
||||
firstOnData = false;
|
||||
}
|
||||
|
||||
// Get current time
|
||||
auto curTime = std::chrono::steady_clock::now();
|
||||
float dt = static_cast<std::chrono::duration<float>>(curTime - prevOnDataTime).count();
|
||||
float deltaDist = 0.0f;
|
||||
float deltaX = 0.0f;
|
||||
float deltaY = 0.0f;
|
||||
float deltaYaw = 0.0f;
|
||||
float leftWheelDist = 0.0f;
|
||||
float rightWheelDist = 0.0f;
|
||||
float wheelDistDiff = 0.0f;
|
||||
|
||||
// Protocol versions 1 and 2 use distance and angle fields for odometry
|
||||
int16_t angleField = 0;
|
||||
if (model.getVersion() <= V_2) {
|
||||
// This is a standards compliant way of doing unsigned to signed conversion
|
||||
uint16_t distanceRaw = GET_DATA(ID_DISTANCE);
|
||||
int16_t distance;
|
||||
std::memcpy(&distance, &distanceRaw, sizeof(distance));
|
||||
deltaDist = distance / 1000.0; // mm -> m
|
||||
|
||||
// Angle is processed differently in versions 1 and 2
|
||||
uint16_t angleRaw = GET_DATA(ID_ANGLE);
|
||||
std::memcpy(&angleField, &angleRaw, sizeof(angleField));
|
||||
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) );
|
||||
}
|
||||
|
||||
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) {
|
||||
else if (model == CREATE_2) {
|
||||
// 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
|
||||
leftWheelDist = (ticksLeft / util::V_3_TICKS_PER_REV)
|
||||
* model.getWheelDiameter() * util::PI;
|
||||
rightWheelDist = (ticksRight / util::V_3_TICKS_PER_REV)
|
||||
* model.getWheelDiameter() * util::PI;
|
||||
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;
|
||||
deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
|
||||
|
||||
wheelDistDiff = rightWheelDist - leftWheelDist;
|
||||
deltaYaw = wheelDistDiff / model.getAxleLength();
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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 {
|
||||
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);
|
||||
|
|
@ -312,7 +140,7 @@ namespace create {
|
|||
float maxWait = 30; // seconds
|
||||
float retryInterval = 5; //seconds
|
||||
time(&start);
|
||||
while (!serial->connect(port, baud, std::bind(&Create::onData, this)) && !timeout) {
|
||||
while (!serial->connect(port, baud, boost::bind(&Create::onData, this)) && !timeout) {
|
||||
time(&now);
|
||||
if (difftime(now, start) > maxWait) {
|
||||
timeout = true;
|
||||
|
|
@ -339,39 +167,7 @@ namespace create {
|
|||
//}
|
||||
|
||||
bool Create::setMode(const CreateMode& 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;
|
||||
return serial->sendOpcode((Opcode) mode);
|
||||
}
|
||||
|
||||
bool Create::clean(const CleanMode& mode) {
|
||||
|
|
@ -384,111 +180,55 @@ namespace create {
|
|||
|
||||
bool Create::setDate(const DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const {
|
||||
if (day < 0 || day > 6 ||
|
||||
hour > 23 ||
|
||||
min > 59)
|
||||
hour < 0 || hour > 23 ||
|
||||
min < 0 || min > 59)
|
||||
return false;
|
||||
|
||||
uint8_t cmd[4] = { OC_DATE, static_cast<uint8_t>(day), hour, min };
|
||||
uint8_t cmd[4] = { OC_DATE, day, hour, min };
|
||||
return serial->send(cmd, 4);
|
||||
}
|
||||
|
||||
bool Create::driveRadius(const float& vel, const float& radius) {
|
||||
// Bound velocity
|
||||
float boundedVel = BOUND_CONST(vel, -model.getMaxVelocity(), model.getMaxVelocity());
|
||||
|
||||
bool Create::driveRadius(const float& vel, const float& radius) const {
|
||||
// Expects each parameter as two bytes each and in millimeters
|
||||
int16_t vel_mm = roundf(boundedVel * 1000);
|
||||
int16_t vel_mm = roundf(vel * 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::MAX_RADIUS * 1000, util::MAX_RADIUS * 1000);
|
||||
BOUND(radius_mm, -util::CREATE_2_MAX_RADIUS, util::CREATE_2_MAX_RADIUS);
|
||||
}
|
||||
|
||||
uint8_t cmd[5] = { OC_DRIVE,
|
||||
static_cast<uint8_t>(vel_mm >> 8),
|
||||
static_cast<uint8_t>(vel_mm & 0xff),
|
||||
static_cast<uint8_t>(radius_mm >> 8),
|
||||
static_cast<uint8_t>(radius_mm & 0xff)
|
||||
vel_mm >> 8,
|
||||
vel_mm & 0xff,
|
||||
radius_mm >> 8,
|
||||
radius_mm & 0xff
|
||||
};
|
||||
|
||||
return serial->send(cmd, 5);
|
||||
}
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
uint8_t cmd[5] = { OC_DRIVE_DIRECT,
|
||||
static_cast<uint8_t>(rightCmd >> 8),
|
||||
static_cast<uint8_t>(rightCmd & 0xff),
|
||||
static_cast<uint8_t>(leftCmd >> 8),
|
||||
static_cast<uint8_t>(leftCmd & 0xff)
|
||||
};
|
||||
return serial->send(cmd, 5);
|
||||
} else {
|
||||
float radius;
|
||||
// Prevent divide by zero when driving straight
|
||||
if (boundedLeftVel != boundedRightVel) {
|
||||
radius = -((model.getAxleLength() / 2.0) * (boundedLeftVel + boundedRightVel)) /
|
||||
(boundedLeftVel - boundedRightVel);
|
||||
} else {
|
||||
radius = util::STRAIGHT_RADIUS;
|
||||
}
|
||||
|
||||
float vel;
|
||||
// Fix signs for spin in place
|
||||
if (boundedLeftVel == -boundedRightVel || std::abs(roundf(radius * 1000)) <= 1) {
|
||||
radius = util::IN_PLACE_RADIUS;
|
||||
vel = boundedRightVel;
|
||||
} else {
|
||||
vel = (std::abs(boundedLeftVel) + std::abs(boundedRightVel)) / 2.0 * ((boundedLeftVel + boundedRightVel) > 0 ? 1.0 : -1.0);
|
||||
}
|
||||
|
||||
// Radius turns have a maximum radius of 2.0 meters
|
||||
// When the radius is > 2 but <= 10, use a 2 meter radius
|
||||
// When it is > 10, drive straight
|
||||
// TODO: alternate between these 2 meter radius and straight to
|
||||
// fake a larger radius
|
||||
if (radius > 10.0) {
|
||||
radius = util::STRAIGHT_RADIUS;
|
||||
}
|
||||
|
||||
return driveRadius(vel, radius);
|
||||
}
|
||||
}
|
||||
|
||||
bool Create::driveWheelsPwm(const float& leftWheel, const float& rightWheel)
|
||||
{
|
||||
static const int16_t PWM_COUNTS = 255;
|
||||
|
||||
if (leftWheel < -1.0 || leftWheel > 1.0 ||
|
||||
rightWheel < -1.0 || rightWheel > 1.0)
|
||||
return false;
|
||||
|
||||
int16_t leftPwm = roundf(leftWheel * PWM_COUNTS);
|
||||
int16_t rightPwm = roundf(rightWheel * PWM_COUNTS);
|
||||
|
||||
uint8_t cmd[5] = { OC_DRIVE_PWM,
|
||||
static_cast<uint8_t>(rightPwm >> 8),
|
||||
static_cast<uint8_t>(rightPwm & 0xff),
|
||||
static_cast<uint8_t>(leftPwm >> 8),
|
||||
static_cast<uint8_t>(leftPwm & 0xff)
|
||||
uint8_t cmd[5] = { OC_DRIVE_DIRECT,
|
||||
rightCmd >> 8,
|
||||
rightCmd & 0xff,
|
||||
leftCmd >> 8,
|
||||
leftCmd & 0xff
|
||||
};
|
||||
|
||||
return serial->send(cmd, 5);
|
||||
}
|
||||
|
||||
bool Create::drive(const float& xVel, const float& angularVel) {
|
||||
bool Create::drive(const float& xVel, const float& angularVel) const {
|
||||
// Compute left and right wheel velocities
|
||||
float leftVel = xVel - ((model.getAxleLength() / 2.0) * angularVel);
|
||||
float rightVel = xVel + ((model.getAxleLength() / 2.0) * angularVel);
|
||||
float leftVel = xVel - ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
|
||||
float rightVel = xVel + ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
|
||||
return driveWheels(leftVel, rightVel);
|
||||
}
|
||||
|
||||
|
|
@ -502,43 +242,34 @@ namespace create {
|
|||
sideMotorPower = roundf(side * 127);
|
||||
vacuumMotorPower = roundf(vacuum * 127);
|
||||
|
||||
if (model.getVersion() == V_1) {
|
||||
uint8_t cmd[2] = { OC_MOTORS,
|
||||
static_cast<uint8_t>((side != 0.0 ? 1 : 0) |
|
||||
(vacuum != 0.0 ? 2 : 0) |
|
||||
(main != 0.0 ? 4 : 0))
|
||||
};
|
||||
return serial->send(cmd, 2);
|
||||
}
|
||||
|
||||
uint8_t cmd[4] = { OC_MOTORS_PWM,
|
||||
mainMotorPower,
|
||||
sideMotorPower,
|
||||
vacuumMotorPower
|
||||
};
|
||||
mainMotorPower,
|
||||
sideMotorPower,
|
||||
vacuumMotorPower
|
||||
};
|
||||
|
||||
return serial->send(cmd, 4);
|
||||
}
|
||||
|
||||
bool Create::setMainMotor(const float& main) {
|
||||
return setAllMotors(main, static_cast<float>(sideMotorPower) / 127.0, static_cast<float>(vacuumMotorPower) / 127.0);
|
||||
return setAllMotors(main, sideMotorPower, vacuumMotorPower);
|
||||
}
|
||||
|
||||
bool Create::setSideMotor(const float& side) {
|
||||
return setAllMotors(static_cast<float>(mainMotorPower) / 127.0, side, static_cast<float>(vacuumMotorPower) / 127.0);
|
||||
return setAllMotors(mainMotorPower, side, vacuumMotorPower);
|
||||
}
|
||||
|
||||
bool Create::setVacuumMotor(const float& vacuum) {
|
||||
return setAllMotors(static_cast<float>(mainMotorPower) / 127.0, static_cast<float>(sideMotorPower) / 127.0, vacuum);
|
||||
return setAllMotors(mainMotorPower, sideMotorPower, 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);
|
||||
}
|
||||
|
|
@ -609,7 +340,7 @@ namespace create {
|
|||
const float* durations) const {
|
||||
int i, j;
|
||||
uint8_t duration;
|
||||
std::vector<uint8_t> cmd(2 * songLength + 3);
|
||||
uint8_t cmd[2 * songLength + 3];
|
||||
cmd[0] = OC_SONG;
|
||||
cmd[1] = songNumber;
|
||||
cmd[2] = songLength;
|
||||
|
|
@ -623,20 +354,16 @@ namespace create {
|
|||
j++;
|
||||
}
|
||||
|
||||
return serial->send(cmd.data(), cmd.size());
|
||||
return serial->send(cmd, 2 * songLength + 3);
|
||||
}
|
||||
|
||||
bool Create::playSong(const uint8_t& songNumber) const {
|
||||
if (songNumber > 4)
|
||||
if (songNumber < 0 || 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;
|
||||
|
|
@ -647,26 +374,6 @@ 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;
|
||||
|
|
@ -713,86 +420,6 @@ 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);
|
||||
|
|
@ -804,8 +431,8 @@ namespace create {
|
|||
}
|
||||
|
||||
uint8_t Create::getDirtDetect() const {
|
||||
if (data->isValidPacketID(ID_DIRT_DETECT_LEFT)) {
|
||||
return GET_DATA(ID_DIRT_DETECT_LEFT);
|
||||
if (data->isValidPacketID(ID_DIRT_DETECT)) {
|
||||
return GET_DATA(ID_DIRT_DETECT);
|
||||
}
|
||||
else {
|
||||
CERR("[create::Create] ", "Dirt detector not supported!");
|
||||
|
|
@ -846,6 +473,7 @@ 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;
|
||||
}
|
||||
|
|
@ -1119,85 +747,21 @@ namespace create {
|
|||
}
|
||||
}
|
||||
|
||||
bool Create::isSideBrushOvercurrent() const {
|
||||
if (data->isValidPacketID(ID_OVERCURRENTS)) {
|
||||
return (GET_DATA(ID_OVERCURRENTS) & 0x01) != 0;
|
||||
}
|
||||
else {
|
||||
CERR("[create::Create] ", "Overcurrent sensor not supported!");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
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() {
|
||||
create::CreateMode Create::getMode() const {
|
||||
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 (create::CreateMode) GET_DATA(ID_OI_MODE);
|
||||
}
|
||||
else {
|
||||
CERR("[create::Create] ", "Querying Mode not supported!");
|
||||
return create::MODE_UNAVAILABLE;
|
||||
}
|
||||
|
||||
return mode;
|
||||
}
|
||||
|
||||
Pose Create::getPose() const {
|
||||
const Pose& Create::getPose() const {
|
||||
return pose;
|
||||
}
|
||||
|
||||
Vel Create::getVel() const {
|
||||
const Vel& Create::getVel() const {
|
||||
return vel;
|
||||
}
|
||||
|
||||
|
|
|
|||
107
src/data.cpp
107
src/data.cpp
|
|
@ -1,55 +1,75 @@
|
|||
#include "create/data.h"
|
||||
|
||||
#define ADD_PACKET(id,nbytes,info,enabledVersion) if ((enabledVersion) & version) packets[id]=std::make_shared<Packet>(nbytes,info)
|
||||
#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared<Packet>(nbytes,info))
|
||||
|
||||
namespace create {
|
||||
|
||||
Data::Data(ProtocolVersion version) {
|
||||
Data::Data(RobotModel model) {
|
||||
// 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", 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);
|
||||
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
|
||||
|
||||
totalDataBytes = 0;
|
||||
for (std::map<uint8_t, std::shared_ptr<Packet> >::iterator it = packets.begin();
|
||||
for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
|
||||
it != packets.end();
|
||||
++it) {
|
||||
ids.push_back(it->first);
|
||||
|
|
@ -66,15 +86,16 @@ namespace create {
|
|||
return false;
|
||||
}
|
||||
|
||||
std::shared_ptr<Packet> Data::getPacket(uint8_t id) {
|
||||
boost::shared_ptr<Packet> Data::getPacket(uint8_t id) {
|
||||
if (isValidPacketID(id)) {
|
||||
return packets[id];
|
||||
}
|
||||
return std::shared_ptr<Packet>();
|
||||
std::cout << "Invalid packet " << (int) id << " requested" << std::endl;
|
||||
return boost::shared_ptr<Packet>();
|
||||
}
|
||||
|
||||
void Data::validateAll() {
|
||||
for (std::map<uint8_t, std::shared_ptr<Packet> >::iterator it = packets.begin();
|
||||
for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
|
||||
it != packets.end();
|
||||
++it) {
|
||||
it->second->validate();
|
||||
|
|
|
|||
|
|
@ -1,34 +1,32 @@
|
|||
#include <memory>
|
||||
|
||||
#include "create/packet.h"
|
||||
|
||||
namespace create {
|
||||
|
||||
Packet::Packet(const uint8_t& numBytes, const std::string& comment) :
|
||||
data(0),
|
||||
tmpData(0),
|
||||
nbytes(numBytes),
|
||||
info(comment) { }
|
||||
info(comment),
|
||||
data(0),
|
||||
tmpData(0) { }
|
||||
|
||||
Packet::~Packet() { }
|
||||
|
||||
void Packet::setDataToValidate(const uint16_t& tmp) {
|
||||
std::lock_guard<std::mutex> lock(tmpDataMutex);
|
||||
void Packet::setTempData(const uint16_t& tmp) {
|
||||
boost::mutex::scoped_lock lock(tmpDataMutex);
|
||||
tmpData = tmp;
|
||||
}
|
||||
|
||||
void Packet::validate() {
|
||||
std::lock_guard<std::mutex> lock(tmpDataMutex);
|
||||
boost::mutex::scoped_lock lock(tmpDataMutex);
|
||||
setData(tmpData);
|
||||
}
|
||||
|
||||
void Packet::setData(const uint16_t& d) {
|
||||
std::lock_guard<std::mutex> lock(dataMutex);
|
||||
boost::mutex::scoped_lock lock(dataMutex);
|
||||
data = d;
|
||||
}
|
||||
|
||||
uint16_t Packet::getData() const {
|
||||
std::lock_guard<std::mutex> lock(dataMutex);
|
||||
boost::mutex::scoped_lock lock(dataMutex);
|
||||
return data;
|
||||
}
|
||||
|
||||
|
|
|
|||
209
src/serial.cpp
209
src/serial.cpp
|
|
@ -1,5 +1,3 @@
|
|||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
|
||||
#include "create/serial.h"
|
||||
|
|
@ -7,59 +5,38 @@
|
|||
|
||||
namespace create {
|
||||
|
||||
Serial::Serial(std::shared_ptr<Data> d, bool install_signal_handler) :
|
||||
signals(io),
|
||||
port(io),
|
||||
dataReady(false),
|
||||
isReading(false),
|
||||
Serial::Serial(boost::shared_ptr<Data> d, const uint8_t& header) :
|
||||
data(d),
|
||||
headerByte(header),
|
||||
port(io),
|
||||
readState(READ_HEADER),
|
||||
isReading(false),
|
||||
dataReady(false),
|
||||
corruptPackets(0),
|
||||
totalPackets(0)
|
||||
{
|
||||
if (install_signal_handler) {
|
||||
signals.add(SIGINT);
|
||||
signals.add(SIGTERM);
|
||||
}
|
||||
totalPackets(0) {
|
||||
}
|
||||
|
||||
Serial::~Serial() {
|
||||
disconnect();
|
||||
}
|
||||
|
||||
void Serial::signalHandler(const boost::system::error_code& error, int signal_number) {
|
||||
if (!error) {
|
||||
if (connected()) {
|
||||
// Ensure not in Safe/Full modes
|
||||
sendOpcode(OC_START);
|
||||
// Stop OI
|
||||
sendOpcode(OC_STOP);
|
||||
exit(signal_number);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Serial::connect(const std::string& portName, const int& baud, std::function<void()> cb) {
|
||||
bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
|
||||
using namespace boost::asio;
|
||||
if (!openPort(portName, baud)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
signals.async_wait(std::bind(&Serial::signalHandler, this, std::placeholders::_1, std::placeholders::_2));
|
||||
port.open(portName);
|
||||
port.set_option(serial_port::baud_rate(baud));
|
||||
port.set_option(serial_port::flow_control(serial_port::flow_control::none));
|
||||
|
||||
usleep(1000000);
|
||||
|
||||
if (!port.is_open()) {
|
||||
return false;
|
||||
if (port.is_open()) {
|
||||
callback = cb;
|
||||
bool startReadSuccess = startReading();
|
||||
if (!startReadSuccess) {
|
||||
port.close();
|
||||
}
|
||||
return startReadSuccess;
|
||||
}
|
||||
|
||||
callback = cb;
|
||||
bool startReadSuccess = startReading();
|
||||
if (!startReadSuccess) {
|
||||
closePort();
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
void Serial::disconnect() {
|
||||
|
|
@ -76,33 +53,6 @@ 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;
|
||||
|
||||
|
|
@ -114,32 +64,44 @@ namespace create {
|
|||
// Only allow once
|
||||
if (isReading) return true;
|
||||
|
||||
// Request from Create that we want a stream containing all packets
|
||||
uint8_t numPackets = data->getNumPackets();
|
||||
std::vector<uint8_t> packetIDs = data->getPacketIDs();
|
||||
uint8_t streamReq[2 + numPackets];
|
||||
streamReq[0] = OC_STREAM;
|
||||
streamReq[1] = numPackets;
|
||||
int i = 2;
|
||||
for (std::vector<uint8_t>::iterator it = packetIDs.begin(); it != packetIDs.end(); ++it) {
|
||||
streamReq[i] = *it;
|
||||
i++;
|
||||
}
|
||||
|
||||
// Start OI
|
||||
sendOpcode(OC_START);
|
||||
|
||||
if (!startSensorStream()) return false;
|
||||
// Start streaming data
|
||||
send(streamReq, 2 + numPackets);
|
||||
|
||||
expectedNumBytes = data->getTotalDataBytes() + numPackets;
|
||||
|
||||
//TODO: handle boost exceptions
|
||||
|
||||
io.reset();
|
||||
|
||||
// Start continuously reading one byte at a time
|
||||
boost::asio::async_read(port,
|
||||
boost::asio::buffer(&byteRead, 1),
|
||||
std::bind(&Serial::onData,
|
||||
shared_from_this(),
|
||||
std::placeholders::_1,
|
||||
std::placeholders::_2));
|
||||
boost::bind(&Serial::onData, this, _1, _2));
|
||||
|
||||
ioThread = std::thread(std::bind(
|
||||
static_cast<std::size_t(boost::asio::io_service::*)(void)>(
|
||||
&boost::asio::io_service::run), &io));
|
||||
ioThread = boost::thread(boost::bind(&boost::asio::io_service::run, &io));
|
||||
|
||||
// Wait for first complete read to finish
|
||||
std::unique_lock<std::mutex> lock(dataReadyMut);
|
||||
boost::unique_lock<boost::mutex> lock(dataReadyMut);
|
||||
|
||||
int attempts = 1;
|
||||
int maxAttempts = 10;
|
||||
while (!dataReady) {
|
||||
if (dataReadyCond.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) {
|
||||
if (!dataReadyCond.timed_wait(lock, boost::get_system_time() + boost::posix_time::milliseconds(500))) {
|
||||
if (attempts >= maxAttempts) {
|
||||
CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!");
|
||||
io.stop();
|
||||
|
|
@ -150,7 +112,7 @@ namespace create {
|
|||
|
||||
// Request data again
|
||||
sendOpcode(OC_START);
|
||||
startSensorStream();
|
||||
send(streamReq, 2 + numPackets);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -164,7 +126,7 @@ namespace create {
|
|||
ioThread.join();
|
||||
isReading = false;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(dataReadyMut);
|
||||
boost::lock_guard<boost::mutex> lock(dataReadyMut);
|
||||
dataReady = false;
|
||||
}
|
||||
}
|
||||
|
|
@ -177,7 +139,7 @@ namespace create {
|
|||
|
||||
// Notify first data packets ready
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(dataReadyMut);
|
||||
boost::lock_guard<boost::mutex> lock(dataReadyMut);
|
||||
if (!dataReady) {
|
||||
dataReady = true;
|
||||
dataReadyCond.notify_one();
|
||||
|
|
@ -196,16 +158,80 @@ namespace create {
|
|||
|
||||
// Should have read exactly one byte
|
||||
if (size == 1) {
|
||||
processByte(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)->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)
|
||||
} // end if (size == 1)
|
||||
|
||||
// Read the next byte
|
||||
boost::asio::async_read(port,
|
||||
boost::asio::buffer(&byteRead, 1),
|
||||
std::bind(&Serial::onData,
|
||||
shared_from_this(),
|
||||
std::placeholders::_1,
|
||||
std::placeholders::_2));
|
||||
boost::bind(&Serial::onData, this, _1, _2));
|
||||
}
|
||||
|
||||
bool Serial::send(const uint8_t* bytes, unsigned int numBytes) {
|
||||
|
|
@ -213,13 +239,8 @@ namespace create {
|
|||
CERR("[create::Serial] ", "send failed, not connected.");
|
||||
return false;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
// TODO: catch boost exceptions
|
||||
boost::asio::write(port, boost::asio::buffer(bytes, numBytes));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,73 +0,0 @@
|
|||
#include <iostream>
|
||||
#include <memory>
|
||||
|
||||
#include "create/serial_query.h"
|
||||
#include "create/types.h"
|
||||
|
||||
#define SENSORS_RESPONSE_LENGTH 20
|
||||
|
||||
namespace create {
|
||||
|
||||
SerialQuery::SerialQuery(std::shared_ptr<Data> d, bool install_signal_handler)
|
||||
: Serial(d, install_signal_handler),
|
||||
streamRecoveryTimer(io),
|
||||
packetID(ID_BUMP_WHEELDROP),
|
||||
packetByte(0),
|
||||
packetData(0),
|
||||
maxPacketID(ID_CAPACITY) {
|
||||
}
|
||||
|
||||
bool SerialQuery::startSensorStream() {
|
||||
if (!started) {
|
||||
requestSensorData();
|
||||
started = true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void SerialQuery::requestSensorData() {
|
||||
static const uint8_t requestPacket[2] = { OC_SENSORS, ID_GROUP_0 };
|
||||
// Prevents previous packet from corrupting next one
|
||||
flushInput();
|
||||
send(requestPacket, 2);
|
||||
// Automatically resend request if no response is received
|
||||
streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50));
|
||||
streamRecoveryTimer.async_wait(
|
||||
std::bind(&SerialQuery::restartSensorStream, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void SerialQuery::restartSensorStream(const boost::system::error_code& err) {
|
||||
if (err != boost::asio::error::operation_aborted) {
|
||||
if (packetID != ID_BUMP_WHEELDROP) {
|
||||
++corruptPackets;
|
||||
}
|
||||
requestSensorData();
|
||||
}
|
||||
}
|
||||
|
||||
void SerialQuery::flushInput() {
|
||||
// Only works with POSIX support
|
||||
tcflush(port.lowest_layer().native_handle(), TCIFLUSH);
|
||||
}
|
||||
|
||||
void SerialQuery::processByte(uint8_t byteRead) {
|
||||
packetData |= (static_cast<uint16_t>(byteRead) << (8 * packetByte));
|
||||
|
||||
if (packetByte > 0) {
|
||||
--packetByte;
|
||||
} else if (packetID < maxPacketID) {
|
||||
// New packet
|
||||
data->getPacket(packetID)->setDataToValidate(packetData);
|
||||
packetData = 0;
|
||||
++packetID;
|
||||
packetByte = data->getPacket(packetID)->nbytes - 1;
|
||||
} else {
|
||||
// Response finished
|
||||
packetID = ID_BUMP_WHEELDROP;
|
||||
packetByte = 0;
|
||||
packetData = 0;
|
||||
notifyDataReady();
|
||||
requestSensorData();
|
||||
}
|
||||
}
|
||||
} // namespace create
|
||||
|
|
@ -1,98 +0,0 @@
|
|||
#include <iostream>
|
||||
#include <memory>
|
||||
|
||||
#include "create/serial_stream.h"
|
||||
#include "create/types.h"
|
||||
|
||||
namespace create {
|
||||
|
||||
SerialStream::SerialStream(std::shared_ptr<Data> d, const uint8_t& header, bool install_signal_handler) : Serial(d, install_signal_handler), readState(READ_HEADER), headerByte(header) {
|
||||
}
|
||||
|
||||
bool SerialStream::startSensorStream() {
|
||||
// Request from Create that we want a stream containing all packets
|
||||
const uint8_t numPackets = data->getNumPackets();
|
||||
std::vector<uint8_t> packetIDs = data->getPacketIDs();
|
||||
packetIDs.insert(packetIDs.begin(), numPackets);
|
||||
packetIDs.insert(packetIDs.begin(), OC_STREAM);
|
||||
|
||||
// Start streaming data
|
||||
send(packetIDs.data(), packetIDs.size());
|
||||
|
||||
expectedNumBytes = data->getTotalDataBytes() + numPackets;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SerialStream::processByte(uint8_t byteRead) {
|
||||
numBytesRead++;
|
||||
byteSum += byteRead;
|
||||
switch (readState) {
|
||||
case READ_HEADER:
|
||||
if (byteRead == headerByte) {
|
||||
readState = READ_NBYTES;
|
||||
byteSum = byteRead;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_NBYTES:
|
||||
if (byteRead == expectedNumBytes) {
|
||||
readState = READ_PACKET_ID;
|
||||
numBytesRead = 0;
|
||||
}
|
||||
else {
|
||||
//notifyDataReady();
|
||||
readState = READ_HEADER;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_PACKET_ID:
|
||||
packetID = byteRead;
|
||||
if (data->isValidPacketID(packetID)) {
|
||||
expectedNumDataBytes = data->getPacket(packetID)->nbytes;
|
||||
assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
|
||||
numDataBytesRead = 0;
|
||||
packetBytes = 0;
|
||||
readState = READ_PACKET_BYTES;
|
||||
}
|
||||
else {
|
||||
//notifyDataReady();
|
||||
readState = READ_HEADER;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_PACKET_BYTES:
|
||||
numDataBytesRead++;
|
||||
if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
|
||||
// High byte first
|
||||
packetBytes = (byteRead << 8) & 0xff00;
|
||||
}
|
||||
else {
|
||||
// Low byte
|
||||
packetBytes += byteRead;
|
||||
}
|
||||
if (numDataBytesRead >= expectedNumDataBytes) {
|
||||
data->getPacket(packetID)->setDataToValidate(packetBytes);
|
||||
if (numBytesRead >= expectedNumBytes)
|
||||
readState = READ_CHECKSUM;
|
||||
else
|
||||
readState = READ_PACKET_ID;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_CHECKSUM:
|
||||
if ((byteSum & 0xFF) == 0) {
|
||||
notifyDataReady();
|
||||
}
|
||||
else {
|
||||
// Corrupt data
|
||||
corruptPackets++;
|
||||
}
|
||||
totalPackets++;
|
||||
// Start again
|
||||
readState = READ_HEADER;
|
||||
break;
|
||||
} // end switch (readState)
|
||||
}
|
||||
|
||||
} // namespace create
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
#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);
|
||||
}
|
||||
|
|
@ -1,65 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file test_create.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/create.h"
|
||||
#include "create/types.h"
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(CreateTest, ConstructorSingleParam)
|
||||
{
|
||||
create::Create create_default;
|
||||
|
||||
create::Create create_1(create::RobotModel::CREATE_1);
|
||||
|
||||
create::Create create_2(create::RobotModel::CREATE_2);
|
||||
|
||||
create::Create create_roomba_400(create::RobotModel::ROOMBA_400);
|
||||
}
|
||||
|
||||
// TEST(CreateTest, ConstructorMultiParam)
|
||||
// {
|
||||
// TODO(jacobperron): Document exception thrown and consider defining custom exception
|
||||
// create::Create create(std::string("/dev/ttyUSB0"), 11520);
|
||||
// }
|
||||
|
||||
TEST(CreateTest, Connected)
|
||||
{
|
||||
create::Create create;
|
||||
// Nothing to be connected to
|
||||
EXPECT_FALSE(create.connected());
|
||||
}
|
||||
|
||||
TEST(CreateTest, Disconnect)
|
||||
{
|
||||
create::Create create;
|
||||
// Even though not connected, this should not crash
|
||||
create.disconnect();
|
||||
}
|
||||
|
|
@ -1,169 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file test_data.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/data.h"
|
||||
#include "create/packet.h"
|
||||
#include "create/types.h"
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
TEST(DataTest, Constructor)
|
||||
{
|
||||
create::Data data_default;
|
||||
|
||||
create::Data data_v_1(create::V_1);
|
||||
|
||||
create::Data data_v_2(create::V_2);
|
||||
|
||||
create::Data data_v_3(create::V_3);
|
||||
|
||||
create::Data data_v_all(create::V_ALL);
|
||||
}
|
||||
|
||||
// Number of packets for a given protocol are determined in the Data() constructor
|
||||
TEST(DataTest, GetNumPackets)
|
||||
{
|
||||
// Number of packets shared by all protocols is 17
|
||||
create::Data data_v_1(create::V_1);
|
||||
// Number exclusive to V_1 = 3
|
||||
// 17 + 3 = 20
|
||||
EXPECT_EQ(static_cast<int>(data_v_1.getNumPackets()), 20);
|
||||
|
||||
create::Data data_v_2(create::V_2);
|
||||
// Number exclusive to V_2 = 3
|
||||
// 17 + 3 = 20
|
||||
EXPECT_EQ(static_cast<int>(data_v_2.getNumPackets()), 20);
|
||||
|
||||
create::Data data_v_3(create::V_3);
|
||||
// Number exclusive to V_3 = 13
|
||||
// 17 + 17 = 34
|
||||
EXPECT_EQ(static_cast<int>(data_v_3.getNumPackets()), 34);
|
||||
|
||||
create::Data data_v_all(create::V_ALL);
|
||||
EXPECT_EQ(static_cast<int>(data_v_all.getNumPackets()), 37);
|
||||
}
|
||||
|
||||
TEST(DataTest, GetPacket)
|
||||
{
|
||||
// Get a packet exclusive to V_1
|
||||
create::Data data_v_1(create::V_1);
|
||||
std::shared_ptr<create::Packet> v_1_packet_ptr = data_v_1.getPacket(create::ID_OVERCURRENTS);
|
||||
EXPECT_NE(v_1_packet_ptr, std::shared_ptr<create::Packet>())
|
||||
<< "ID_OVERCURRENTS packet not found for protocol V_1";
|
||||
EXPECT_EQ(static_cast<int>(v_1_packet_ptr->nbytes), 1);
|
||||
EXPECT_EQ(v_1_packet_ptr->info, std::string("overcurrents"));
|
||||
|
||||
// Get a packet for V_2
|
||||
create::Data data_v_2(create::V_2);
|
||||
std::shared_ptr<create::Packet> v_2_packet_ptr = data_v_2.getPacket(create::ID_DISTANCE);
|
||||
EXPECT_NE(v_2_packet_ptr, std::shared_ptr<create::Packet>())
|
||||
<< "ID_DISTANCE packet not found for protocol V_2";
|
||||
EXPECT_EQ(static_cast<int>(v_2_packet_ptr->nbytes), 2);
|
||||
EXPECT_EQ(v_2_packet_ptr->info, std::string("distance"));
|
||||
|
||||
// Get a packet exclusive to V_3
|
||||
create::Data data_v_3(create::V_3);
|
||||
std::shared_ptr<create::Packet> v_3_packet_ptr = data_v_3.getPacket(create::ID_LIGHT_FRONT_RIGHT);
|
||||
EXPECT_NE(v_3_packet_ptr, std::shared_ptr<create::Packet>())
|
||||
<< "ID_LIGHT_FRONT_RIGHT packet not found for protocol V_3";
|
||||
EXPECT_EQ(static_cast<int>(v_3_packet_ptr->nbytes), 2);
|
||||
EXPECT_EQ(v_3_packet_ptr->info, std::string("light_bumper_front_right"));
|
||||
|
||||
// Get a non-existent packet
|
||||
std::shared_ptr<create::Packet> not_a_packet_ptr = data_v_3.getPacket(60);
|
||||
EXPECT_EQ(not_a_packet_ptr, std::shared_ptr<create::Packet>());
|
||||
}
|
||||
|
||||
TEST(DataTest, GetPacketIDs)
|
||||
{
|
||||
create::Data data_v_3(create::V_3);
|
||||
const std::vector<uint8_t> packet_ids = data_v_3.getPacketIDs();
|
||||
// Vector should have same length as reported by getNumPackets()
|
||||
ASSERT_EQ(static_cast<int>(packet_ids.size()), 34);
|
||||
|
||||
// Vector should contain ID_LEFT_ENC
|
||||
bool found = false;
|
||||
for (std::size_t i = 0; (i < packet_ids.size()) && !found; i++)
|
||||
{
|
||||
if (packet_ids[i] == create::ID_LEFT_ENC)
|
||||
{
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
EXPECT_TRUE(found) << "ID_LEFT_ENC packet ID not returned for protocol V_3";
|
||||
}
|
||||
|
||||
TEST(DataTest, GetTotalDataBytes)
|
||||
{
|
||||
// All protocols have 21 mutual data bytes
|
||||
// V_1 has an additional 5 bytes
|
||||
create::Data data_v_1(create::V_1);
|
||||
EXPECT_EQ(static_cast<int>(data_v_1.getTotalDataBytes()), 26);
|
||||
|
||||
// V_2 has an additional 5 bytes
|
||||
create::Data data_v_2(create::V_2);
|
||||
EXPECT_EQ(static_cast<int>(data_v_2.getTotalDataBytes()), 26);
|
||||
|
||||
// V_3 has an additional 29 bytes
|
||||
create::Data data_v_3(create::V_3);
|
||||
EXPECT_EQ(static_cast<int>(data_v_3.getTotalDataBytes()), 50);
|
||||
}
|
||||
|
||||
TEST(DataTest, IsValidPacketID)
|
||||
{
|
||||
create::Data data_v_1(create::V_1);
|
||||
EXPECT_TRUE(data_v_1.isValidPacketID(create::ID_DIRT_DETECT_RIGHT))
|
||||
<< "ID_DIRT_DETECT_RIGHT packet not found for protocol V_1";
|
||||
EXPECT_FALSE(data_v_1.isValidPacketID(create::ID_OI_MODE))
|
||||
<< "ID_OI_MODE packet should not exist for protocol V_1";
|
||||
|
||||
// V_2 has an additional 5 bytes
|
||||
create::Data data_v_2(create::V_2);
|
||||
EXPECT_TRUE(data_v_2.isValidPacketID(create::ID_ANGLE))
|
||||
<< "ID_ANGLE packet not found for protocol V_2";
|
||||
EXPECT_FALSE(data_v_2.isValidPacketID(create::ID_LIGHT))
|
||||
<< "ID_LIGHT packet should not exist for protocol V_2";
|
||||
|
||||
// V_3 has an additional 21 bytes
|
||||
create::Data data_v_3(create::V_3);
|
||||
EXPECT_TRUE(data_v_3.isValidPacketID(create::ID_STASIS))
|
||||
<< "ID_STATIS packet not found for protocol V_3";
|
||||
EXPECT_FALSE(data_v_3.isValidPacketID(create::ID_DISTANCE))
|
||||
<< "ID_DISTANCE packet should not exist for protocol V_3";
|
||||
}
|
||||
|
||||
TEST(DataTest, ValidateAll)
|
||||
{
|
||||
create::Data data_v_3(create::V_3);
|
||||
// Don't crash
|
||||
data_v_3.validateAll();
|
||||
}
|
||||
|
|
@ -1,73 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file test_packet.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/packet.h"
|
||||
#include "create/types.h"
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(PacketTest, Constructor)
|
||||
{
|
||||
create::Packet empty_packet(0, std::string(""));
|
||||
EXPECT_EQ(static_cast<int>(empty_packet.nbytes), 0);
|
||||
EXPECT_EQ(empty_packet.info, std::string(""));
|
||||
|
||||
create::Packet some_packet(2, std::string("test_packet"));
|
||||
EXPECT_EQ(static_cast<int>(some_packet.nbytes), 2);
|
||||
EXPECT_EQ(some_packet.info, std::string("test_packet"));
|
||||
}
|
||||
|
||||
TEST(PacketTest, SetValidateAndGetData)
|
||||
{
|
||||
create::Packet packet(2, std::string("test_packet"));
|
||||
|
||||
// Set some data and validate it
|
||||
const uint16_t some_data = 123;
|
||||
packet.setDataToValidate(some_data);
|
||||
packet.validate();
|
||||
// Confirm data was validated
|
||||
const uint16_t some_data_result = packet.getData();
|
||||
EXPECT_EQ(some_data_result, some_data);
|
||||
|
||||
// Set zero data and validate it
|
||||
const uint16_t zero_data = 0;
|
||||
packet.setDataToValidate(zero_data);
|
||||
packet.validate();
|
||||
// Confirm data was validated
|
||||
const uint16_t zero_data_result = packet.getData();
|
||||
EXPECT_EQ(zero_data_result, zero_data);
|
||||
|
||||
// Set some data but do not validate it
|
||||
const uint16_t do_not_validate_data = 321;
|
||||
packet.setDataToValidate(do_not_validate_data);
|
||||
// Confirm data was not validated
|
||||
const uint16_t unvalidated_data_result = packet.getData();
|
||||
EXPECT_NE(unvalidated_data_result, do_not_validate_data);
|
||||
}
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file test_robot_model.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/types.h"
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(RobotModelTest, Equality)
|
||||
{
|
||||
EXPECT_EQ(create::RobotModel::CREATE_1, create::RobotModel::CREATE_1);
|
||||
EXPECT_EQ(create::RobotModel::CREATE_2, create::RobotModel::CREATE_2);
|
||||
EXPECT_EQ(create::RobotModel::ROOMBA_400, create::RobotModel::ROOMBA_400);
|
||||
EXPECT_NE(create::RobotModel::CREATE_1, create::RobotModel::CREATE_2);
|
||||
EXPECT_NE(create::RobotModel::CREATE_1, create::RobotModel::ROOMBA_400);
|
||||
EXPECT_NE(create::RobotModel::CREATE_2, create::RobotModel::ROOMBA_400);
|
||||
}
|
||||
|
|
@ -1,90 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file test_serial_query.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/data.h"
|
||||
#include "create/serial_query.h"
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
TEST(SerialQueryTest, Constructor)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialQuery serial_query(data_ptr);
|
||||
}
|
||||
|
||||
TEST(SerialQueryTest, Connected)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialQuery serial_query(data_ptr);
|
||||
|
||||
// Did not call connect and nothing to connect to, so expect false
|
||||
EXPECT_FALSE(serial_query.connected());
|
||||
}
|
||||
|
||||
TEST(SerialQueryTest, Disconnect)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialQuery serial_query(data_ptr);
|
||||
|
||||
// Not connected, but should not fail
|
||||
serial_query.disconnect();
|
||||
}
|
||||
|
||||
TEST(SerialQueryTest, NumPackets)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialQuery serial_query(data_ptr);
|
||||
|
||||
// Not connected, so zero packets should have been received
|
||||
EXPECT_EQ(serial_query.getNumCorruptPackets(), 0);
|
||||
EXPECT_EQ(serial_query.getTotalPackets(), 0);
|
||||
}
|
||||
|
||||
TEST(SerialQueryTest, Send)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialQuery serial_query(data_ptr);
|
||||
|
||||
// Some bytes to send (to set date)
|
||||
uint8_t bytes[4] = { create::OC_DATE, 0, 1, 2 };
|
||||
// Not connected, so failure expected
|
||||
EXPECT_FALSE(serial_query.send(bytes, 4));
|
||||
}
|
||||
|
||||
TEST(SerialQueryTest, SendOpcode)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialQuery serial_query(data_ptr);
|
||||
|
||||
// Not connected, so failure expected
|
||||
EXPECT_FALSE(serial_query.sendOpcode(create::OC_POWER));
|
||||
}
|
||||
|
|
@ -1,90 +0,0 @@
|
|||
/**
|
||||
Software License Agreement (BSD)
|
||||
|
||||
\file test_serial_stream.cpp
|
||||
\authors Jacob Perron <jperron@sfu.ca>
|
||||
\copyright Copyright (c) 2018, Autonomy Lab (Simon Fraser University), All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of Autonomy Lab nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software without
|
||||
specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include "create/data.h"
|
||||
#include "create/serial_stream.h"
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
TEST(SerialStreamTest, Constructor)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialStream serial_stream(data_ptr);
|
||||
}
|
||||
|
||||
TEST(SerialStreamTest, Connected)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialStream serial_stream(data_ptr);
|
||||
|
||||
// Did not call connect and nothing to connect to, so expect false
|
||||
EXPECT_FALSE(serial_stream.connected());
|
||||
}
|
||||
|
||||
TEST(SerialStreamTest, Disconnect)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialStream serial_stream(data_ptr);
|
||||
|
||||
// Not connected, but should not fail
|
||||
serial_stream.disconnect();
|
||||
}
|
||||
|
||||
TEST(SerialStreamTest, NumPackets)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialStream serial_stream(data_ptr);
|
||||
|
||||
// Not connected, so zero packets should have been received
|
||||
EXPECT_EQ(serial_stream.getNumCorruptPackets(), 0);
|
||||
EXPECT_EQ(serial_stream.getTotalPackets(), 0);
|
||||
}
|
||||
|
||||
TEST(SerialStreamTest, Send)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialStream serial_stream(data_ptr);
|
||||
|
||||
// Some bytes to send (to set date)
|
||||
uint8_t bytes[4] = { create::OC_DATE, 0, 1, 2 };
|
||||
// Not connected, so failure expected
|
||||
EXPECT_FALSE(serial_stream.send(bytes, 4));
|
||||
}
|
||||
|
||||
TEST(SerialStreamTest, SendOpcode)
|
||||
{
|
||||
std::shared_ptr<create::Data> data_ptr = std::make_shared<create::Data>();
|
||||
create::SerialStream serial_stream(data_ptr);
|
||||
|
||||
// Not connected, so failure expected
|
||||
EXPECT_FALSE(serial_stream.sendOpcode(create::OC_POWER));
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue