From 67000e27ec6e1e8d8eee54aef2c9b9b055c85c7c Mon Sep 17 00:00:00 2001 From: jacobperron Date: Fri, 11 Dec 2015 13:57:33 -0800 Subject: [PATCH] Initial commit. --- LICENSE | 25 +++ README.md | 8 + create_autonomy/CMakeLists.txt | 4 + create_autonomy/package.xml | 23 ++ create_driver/CMakeLists.txt | 212 ++++++++++++++++++ .../include/create_driver/create_driver.h | 35 +++ create_driver/launch/create.launch | 6 + create_driver/package.xml | 60 +++++ create_driver/src/create_driver.cpp | 98 ++++++++ 9 files changed, 471 insertions(+) create mode 100644 LICENSE create mode 100644 README.md create mode 100644 create_autonomy/CMakeLists.txt create mode 100644 create_autonomy/package.xml create mode 100644 create_driver/CMakeLists.txt create mode 100644 create_driver/include/create_driver/create_driver.h create mode 100644 create_driver/launch/create.launch create mode 100644 create_driver/package.xml create mode 100644 create_driver/src/create_driver.cpp diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..b0ce6207 --- /dev/null +++ b/LICENSE @@ -0,0 +1,25 @@ +Copyright (c) 2015, Jacob Perron (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. diff --git a/README.md b/README.md new file mode 100644 index 00000000..6dbeebd3 --- /dev/null +++ b/README.md @@ -0,0 +1,8 @@ +# create_autonomy + +[ROS](http://ros.org) driver for [iRobot Create](). + +* Documentation: TODO +* ROS wiki page: http://wiki.ros.org/create_autonomy +* Code API: TODO +* Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca)) diff --git a/create_autonomy/CMakeLists.txt b/create_autonomy/CMakeLists.txt new file mode 100644 index 00000000..68632998 --- /dev/null +++ b/create_autonomy/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(create_autonomy) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/create_autonomy/package.xml b/create_autonomy/package.xml new file mode 100644 index 00000000..e8f679b9 --- /dev/null +++ b/create_autonomy/package.xml @@ -0,0 +1,23 @@ + + + create_autonomy + 0.0.1 + create_autonomy is a ROS driver for iRobot's Create, based on the libcreate C++ library + + Jacob Perron + BSD + + + http://github.com/AutonomyLab/create_autonomy + + Jacob Perron + + catkin + + create_driver + + + + + + diff --git a/create_driver/CMakeLists.txt b/create_driver/CMakeLists.txt new file mode 100644 index 00000000..c74ff593 --- /dev/null +++ b/create_driver/CMakeLists.txt @@ -0,0 +1,212 @@ +cmake_minimum_required(VERSION 2.8.3) +project(create_driver) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_msgs + roscpp + std_msgs + tf +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED system thread) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs# nav_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES libcreate create_driver + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp std_msgs tf + DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +include(ExternalProject) +ExternalProject_Add(libcreate + GIT_REPOSITORY https://github.com/AutonomyLab/libcreate.git + GIT_TAG master + PREFIX ${CATKIN_DEVEL_PREFIX} + CONFIGURE_COMMAND cmake . + BUILD_COMMAND make + INSTALL_COMMAND echo "No install" + #make install INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/lib/create + BUILD_IN_SOURCE 1 +) + +set(libcreate_PATH "${CATKIN_DEVEL_PREFIX}/src/libcreate/") + +include_directories( + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${libcreate_PATH}/include + include +) + +link_directories(${libcreate_PATH}) + +add_executable(create_driver src/create_driver.cpp) + +target_link_libraries(create_driver + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + -lboost_system # Why need these two lines? + -lboost_thread + create +) +add_dependencies(create_driver libcreate) +## Declare a C++ library +# add_library(create_driver +# src/${PROJECT_NAME}/create_driver.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(create_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(create_driver_node src/create_driver_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(create_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(create_driver_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS create_driver create_driver_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_create_driver.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/create_driver/include/create_driver/create_driver.h b/create_driver/include/create_driver/create_driver.h new file mode 100644 index 00000000..bfdeb421 --- /dev/null +++ b/create_driver/include/create_driver/create_driver.h @@ -0,0 +1,35 @@ +#ifndef CREATE_AUTONOMY_CREATE_DRIVER_H +#define CREATE_AUTONOMY_CREATE_DRIVER_H + +#include +#include +#include +#include "create/create.h" + +class CreateDriver { + private: + create::Create* robot; + nav_msgs::Odometry odom; + double loopHz; + std::string dev; + int baud; + static const double maxVelX = 0.5; + static const double maxAngularVel = 4.25; + + void cmdVelCallback(const geometry_msgs::TwistConstPtr& msg); + bool update(); + void publishOdom(); + + protected: + ros::NodeHandle nh; + ros::Subscriber cmdVelSub; + ros::Publisher odomPub; + + public: + CreateDriver(ros::NodeHandle& nh_); + ~CreateDriver(); + virtual void spin(); + virtual void spinOnce(); +}; // class CreateDriver + +#endif // CREATE_AUTONOMY_CREATE_DRIVER_H diff --git a/create_driver/launch/create.launch b/create_driver/launch/create.launch new file mode 100644 index 00000000..681db82e --- /dev/null +++ b/create_driver/launch/create.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/create_driver/package.xml b/create_driver/package.xml new file mode 100644 index 00000000..760d137c --- /dev/null +++ b/create_driver/package.xml @@ -0,0 +1,60 @@ + + + create_driver + 0.0.0 + The create_driver package + + + + + ank + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + nav_msgs + roscpp + std_msgs + tf + geometry_msgs + nav_msgs + roscpp + std_msgs + tf + + + + + + + + diff --git a/create_driver/src/create_driver.cpp b/create_driver/src/create_driver.cpp new file mode 100644 index 00000000..3615a727 --- /dev/null +++ b/create_driver/src/create_driver.cpp @@ -0,0 +1,98 @@ +#include +#include "create_driver/create_driver.h" + +CreateDriver::CreateDriver(ros::NodeHandle& nh_) : nh(nh_) { + nh.param("loop_hz", loopHz, 10); + nh.param("dev", dev, "/dev/ttyUSB0"); + nh.param("baud", baud, 115200); + + // TODO get these from Create class + //maxVelX = 0.5; + //maxAngularVel = 4.25; + + robot = new create::Create(); + + if (!robot->connect(dev, baud)) { + ROS_FATAL("[CREATE] Failed to establish serial connection with Create."); + ros::shutdown(); + } + + ROS_INFO("[CREATE] Connection established."); + + // Put into full control mode + //TODO: Make option to run in safe mode as parameter + robot->setMode(create::MODE_FULL); + + // Show robot's battery level + ROS_INFO("[CREATE] Battery level %.2f %%", (robot->getBatteryCharge() / (float)robot->getBatteryCapacity()) * 100.0); + + // Not sure if these are correct + odom.header.frame_id = "base_link"; + odom.child_frame_id = "odom"; + + cmdVelSub = nh.subscribe(std::string("cmd_vel"), 1, &CreateDriver::cmdVelCallback, this); + + odomPub = nh.advertise(std::string("odom"), 10); +} + +CreateDriver::~CreateDriver() { + ROS_INFO("[CREATE] Destruct sequence initiated."); + robot->disconnect(); + delete robot; +} + +void CreateDriver::cmdVelCallback(const geometry_msgs::TwistConstPtr& msg) { + robot->drive(msg->linear.x * maxVelX, msg->angular.z * maxAngularVel); +} + +bool CreateDriver::update() { + publishOdom(); + return true; +} + +void CreateDriver::publishOdom() { + create::Pose pose = robot->getPose(); + odom.pose.pose.position.x = pose.x; + odom.pose.pose.position.y = pose.y; + odom.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, pose.yaw); + + // TODO: populate pose covariance + //odom.pose.covariance = ? + + // TODO: populate twist data + //odom.twist.twist.linear.x = ? + //odom.twist.twist.angular.z = ? + //odom.twist.covariance = ? + odomPub.publish(odom); +} + +void CreateDriver::spinOnce() { + update(); + ros::spinOnce(); +} + +void CreateDriver::spin() { + ros::Rate rate(loopHz); + while (ros::ok()) { + spinOnce(); + if (!rate.sleep()) { + ROS_WARN("[CREATE] Loop running slowly."); + } + } +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "create_driver"); + ros::NodeHandle nh("~"); + + CreateDriver createDriver(nh); + + try { + createDriver.spin(); + } + catch (std::runtime_error& ex) { + ROS_FATAL_STREAM("[CREATE] Runtime error: " << ex.what()); + return 1; + } + return 0; +}