diff --git a/README.md b/README.md index e8d3720c1..f156d5ac7 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ This package wraps the C++ library [libcreate][libcreate], which uses iRobot's [ ## Build Status -TravisCI (Ubuntu _Trusty_, ROS _Indigo_ and _Jade_) +TravisCI (Ubuntu _Trusty_, ROS _Indigo_ and _Jade_) ![Build Status](https://api.travis-ci.org/AutonomyLab/create_autonomy.svg?branch=indigo-devel) ## Supported Robots @@ -27,7 +27,7 @@ TravisCI (Ubuntu _Trusty_, ROS _Indigo_ and _Jade_) | Roomba 800 Series | Yes + | | Roomba 900 Series | No * | -_+ Verified by third-party. Please note [Odometry Issue #28](https://github.com/AutonomyLab/create_autonomy/issues/32)_ +_+ Verified by third-party. Please note [Odometry Issue #28](https://github.com/AutonomyLab/create_autonomy/issues/32)_ _* Not verified. Anyone who is able to verify that this driver works or not is encouraged to contact [Jacob](https://jacobperron.ca) with their findings or open an issue._ ## Features @@ -63,40 +63,35 @@ _* Not verified. Anyone who is able to verify that this driver works or not is e #### Prerequisites * Internet connection -* [ROS](http://wiki.ros.org/ROS/Installation) _Indigo_ or _Jade_ -* Ubuntu packages: `python-rosdep`, `python-catkin-tools` - -``` bash -$ sudo apt-get install python-rosdep python-catkin-tools -``` +* [ROS 2](https://index.ros.org/doc/ros2/Installation/) _Crystal_ #### Compiling -1. Create a catkin workspace +1. Create a colcon workspace ``` bash $ cd ~ - $ mkdir -p create_ws/src - $ cd create_ws - $ catkin init + $ mkdir -p create_ws/src + $ cd create_ws + $ colcon build ``` -2. Clone this repo +2. Clone this repo ``` bash $ cd ~/create_ws/src - $ git clone https://github.com/AutonomyLab/create_autonomy.git + $ git clone https://github.com/AutonomyLab/create_autonomy.git ``` - -3. Install dependencies + +3. Install dependencies ``` bash $ cd ~/create_ws - $ rosdep update - $ rosdep install --from-paths src -i + $ rosdep update + $ rosdep install --from-paths src -i ``` -4. Build +4. Build ``` bash $ cd ~/create_ws - $ catkin build + $ colcon build ``` #### USB Permissions 5. In order to connect to Create over USB, ensure your user is in the dialout group @@ -110,9 +105,9 @@ $ sudo apt-get install python-rosdep python-catkin-tools ### Setup -1. After compiling from source, don't forget to source your workspace: +1. After compiling from source, don't forget to source your workspace: ``` bash - $ source ~/create_ws/devel/setup.bash + $ source ~/create_ws/install/local_setup.bash ``` 2. Connect computer to Create's 7-pin serial port @@ -124,17 +119,17 @@ $ sudo apt-get install python-rosdep python-catkin-tools For Create 2 (Roomba 600/700 series): ``` bash -$ roslaunch ca_driver create_2.launch +$ ros2 launch ca_driver create_2.launch.py ``` For Create 1 (Roomba 500 series): ``` bash -$ roslaunch ca_driver create_1.launch +$ ros2 launch ca_driver create_1.launch.py ``` For Roomba 400 series: ``` bash -$ roslaunch ca_driver roomba_400.launch +$ ros2 launch ca_driver roomba_400.launch.py ``` #### Launch file arguments @@ -145,7 +140,7 @@ $ roslaunch ca_driver roomba_400.launch For example, if you would like to disable the robot description and provide a custom configuration file: ```bash -$ roslaunch ca_driver create_2.launch config:=/abs/path/to/config.yaml desc:=false +$ ros2 launch ca_driver create_2.launch.py config:=/abs/path/to/config.yaml desc:=false ``` ### Parameters @@ -157,7 +152,7 @@ $ roslaunch ca_driver create_2.launch config:=/abs/path/to/config.yaml desc:=fal `odom_frame` | The robot's odometry frame ID | `odom` `latch_cmd_duration` | If this many seconds passes without receiving a velocity command the robot stops | `0.2` `loop_hz` | Frequency of internal update loop | `10.0` -`publish_tf` | Publish the transform from `odom_frame` to `base_frame` | `true` +`publish_tf` | Publish the transform from `odom_frame` to `base_frame` | `true` `robot_model` | The type of robot being controlled (supported values: `ROOMBA_400`, `CREATE_1` and `CREATE_2`) | `CREATE_2` `baud` | Serial baud rate | Inferred based on robot model, but is overwritten upon providing a value @@ -237,7 +232,7 @@ Contributing to the development and maintenance of _create\_autonomy_ is encoura - Confirms driver works with Roomba 700 and 800 series. * [Clyde McQueen](https://github.com/clydemcqueen) - Added support for sound ([#37](https://github.com/AutonomyLab/create_autonomy/pull/37)). -* [Ben Wolsieffer](https://github.com/lopsided98) +* [Ben Wolsieffer](https://github.com/lopsided98) - Added JointState publisher for wheels ([#26](https://github.com/AutonomyLab/create_autonomy/pull/26)). - Added Create 1 description ([#27](https://github.com/AutonomyLab/create_autonomy/pull/27)). @@ -258,4 +253,3 @@ Contributing to the development and maintenance of _create\_autonomy_ is encoura [jointstate_msg]: http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html [definesong_msg]: https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/DefineSong.msg [playsong_msg]: https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/PlaySong.msg - diff --git a/ca_description/CMakeLists.txt b/ca_description/CMakeLists.txt index d35e2ceef..880e8adbf 100644 --- a/ca_description/CMakeLists.txt +++ b/ca_description/CMakeLists.txt @@ -1,25 +1,20 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ca_description) -find_package(catkin REQUIRED COMPONENTS - roslaunch -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) +find_package(urdf REQUIRED) +# find_package(xacro REQUIRED) -roslaunch_add_file_check(launch) +install(DIRECTORY urdf + DESTINATION urdf + ) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE -) +install(DIRECTORY meshes + DESTINATION meshes + ) -install(DIRECTORY meshes/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes - PATTERN ".svn" EXCLUDE -) - -install(DIRECTORY urdf/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf - PATTERN ".svn" EXCLUDE -) +ament_package() diff --git a/ca_description/package.xml b/ca_description/package.xml index 947b4ef94..b2d4907c4 100644 --- a/ca_description/package.xml +++ b/ca_description/package.xml @@ -1,10 +1,11 @@ - + ca_description - 1.3.0 + 2.0.0 Robot URDF descriptions for create_autonomy Jacob Perron + Yutaka Kondo BSD @@ -12,14 +13,13 @@ Jacob Perron - catkin - - roslaunch + ament_cmake + urdf + robot_state_publisher - urdf - xacro + ament_cmake diff --git a/ca_description/urdf/create_1.urdf b/ca_description/urdf/create_1.urdf new file mode 100644 index 000000000..92872d2c7 --- /dev/null +++ b/ca_description/urdf/create_1.urdf @@ -0,0 +1,410 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 40 + right_wheel_joint + left_wheel_joint + 0.26 + 0.072 + 1.0 + + cmd_vel + odom + odom + base_footprint + + + true + 100.0 + base_link + simple_controller/absolute_position + 0 + map + 0 0 0 + 0 0 0 + + + + + 1.0 + 1.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + 1.0 + 1.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + 0.0 + 0.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + 0.0 + 0.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.0160 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 30 + gyro_link + imu/data + imu/is_calibrated + 2.89e-06 + 0 0 0 + 0 0 0 + + + + diff --git a/ca_description/urdf/create_1_gazebo.urdf b/ca_description/urdf/create_1_gazebo.urdf new file mode 100644 index 000000000..63433ae67 --- /dev/null +++ b/ca_description/urdf/create_1_gazebo.urdf @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ca_description/urdf/create_2.urdf b/ca_description/urdf/create_2.urdf new file mode 100644 index 000000000..9579e0e73 --- /dev/null +++ b/ca_description/urdf/create_2.urdf @@ -0,0 +1,410 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 40 + right_wheel_joint + left_wheel_joint + 0.235 + 0.072 + 1.0 + + cmd_vel + odom + odom + base_footprint + + + true + 100.0 + base_link + simple_controller/absolute_position + 0 + map + 0 0 0 + 0 0 0 + + + + + 1.0 + 1.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + 1.0 + 1.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + 0.0 + 0.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + 0.0 + 0.0 + 1000000.0 + 100.0 + 0.001 + 1.0 + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.0160 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 20.0 + 0 0 0 0 0 0 + false + + + + 1 + 1 + 0 + 0 + + + + 0.01 + 0.04 + 0.1 + + + + + + + true + 30 + gyro_link + imu/data + imu/is_calibrated + 2.89e-06 + 0 0 0 + 0 0 0 + + + + diff --git a/ca_description/urdf/create_2_gazebo.urdf b/ca_description/urdf/create_2_gazebo.urdf new file mode 100644 index 000000000..fdada154b --- /dev/null +++ b/ca_description/urdf/create_2_gazebo.urdf @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ca_driver/CMakeLists.txt b/ca_driver/CMakeLists.txt index dcfc36bfc..acad34497 100644 --- a/ca_driver/CMakeLists.txt +++ b/ca_driver/CMakeLists.txt @@ -1,72 +1,62 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ca_driver) -find_package(libcreate REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - ca_msgs - diagnostic_msgs - diagnostic_updater - geometry_msgs - nav_msgs - roscpp - roslint - sensor_msgs - std_msgs - tf -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - ca_msgs - diagnostic_msgs - diagnostic_updater - geometry_msgs - nav_msgs - roscpp - sensor_msgs - std_msgs - tf - DEPENDS libcreate -) - -roslint_cpp() -roslint_add_test() +find_package(ament_cmake REQUIRED) +find_package(ca_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) +# find_package(diagnostic_updater REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(libcreate REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) include_directories( include + ${ca_msgs_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + # ${diagnostic_updater_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} ${libcreate_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) + ${nav_msgs_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ) add_executable(${PROJECT_NAME} src/create_driver.cpp) - -target_link_libraries(${PROJECT_NAME} - ${libcreate_LIBRARIES} - ${catkin_LIBRARIES} -) - -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - +ament_target_dependencies(${PROJECT_NAME} + "ca_msgs" + "diagnostic_msgs" + # "diagnostic_updater" + "geometry_msgs" + "libcreate" + "nav_msgs" + "rclcpp" + "sensor_msgs" + "std_msgs" + "tf2" + "tf2_ros" + ) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + DESTINATION lib/${PROJECT_NAME} + ) -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) + ) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE -) +install(DIRECTORY config + DESTINATION config + ) -install(DIRECTORY config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config - PATTERN ".svn" EXCLUDE -) +ament_package() diff --git a/ca_driver/include/create_driver/create_driver.h b/ca_driver/include/create_driver/create_driver.h index c7013e6b7..2e6bfdca5 100644 --- a/ca_driver/include/create_driver/create_driver.h +++ b/ca_driver/include/create_driver/create_driver.h @@ -1,56 +1,29 @@ -/** -Software License Agreement (BSD) -\file create_driver.h -\authors Jacob Perron -\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved. -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of Autonomy Lab nor the names of its contributors may - be used to endorse or promote products derived from this software without - specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. -*/ -#ifndef CREATE_DRIVER_CREATE_DRIVER_H -#define CREATE_DRIVER_CREATE_DRIVER_H -#include "ca_msgs/ChargingState.h" -#include "ca_msgs/Mode.h" -#include "ca_msgs/Bumper.h" -#include "ca_msgs/DefineSong.h" -#include "ca_msgs/PlaySong.h" - -#include "create/create.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#pragma once #include #include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "create/create.h" +#include "ca_msgs/msg/charging_state.hpp" +#include "ca_msgs/msg/mode.hpp" +#include "ca_msgs/msg/bumper.hpp" +#include "ca_msgs/msg/define_song.hpp" +#include "ca_msgs/msg/play_song.hpp" static const double MAX_DBL = std::numeric_limits::max(); static const double COVARIANCE[36] = {1e-5, 1e-5, 0.0, 0.0, 0.0, 1e-5, // NOLINT(whitespace/braces) @@ -60,25 +33,25 @@ static const double COVARIANCE[36] = {1e-5, 1e-5, 0.0, 0.0, 0.0, 1e- 0.0, 0.0, 0.0, 0.0, MAX_DBL, 0.0, 1e-5, 1e-5, 0.0, 0.0, 0.0, 1e-5}; -class CreateDriver +class CreateDriver : public rclcpp::Node { private: - create::Create* robot_; + std::unique_ptr robot_; create::RobotModel model_; - tf::TransformBroadcaster tf_broadcaster_; - diagnostic_updater::Updater diagnostics_; - ca_msgs::Mode mode_msg_; - ca_msgs::ChargingState charging_state_msg_; - ca_msgs::Bumper bumper_msg_; - nav_msgs::Odometry odom_msg_; - geometry_msgs::TransformStamped tf_odom_; - ros::Time last_cmd_vel_time_; - std_msgs::Empty empty_msg_; - std_msgs::Float32 float32_msg_; - std_msgs::UInt16 uint16_msg_; - std_msgs::Int16 int16_msg_; - sensor_msgs::JointState joint_state_msg_; - bool is_running_slowly_; + rclcpp::TimerBase::SharedPtr timer_; + std::shared_ptr tf_broadcaster_; + ca_msgs::msg::Mode mode_msg_; + ca_msgs::msg::ChargingState charging_state_msg_; + ca_msgs::msg::Bumper bumper_msg_; + nav_msgs::msg::Odometry odom_msg_; + geometry_msgs::msg::TransformStamped tf_odom_; + rclcpp::Clock ros_clock_; + rclcpp::Time last_cmd_vel_time_; + std_msgs::msg::Empty empty_msg_; + std_msgs::msg::Float32 float32_msg_; + std_msgs::msg::UInt16 uint16_msg_; + std_msgs::msg::Int16 int16_msg_; + sensor_msgs::msg::JointState joint_state_msg_; // ROS params std::string dev_; @@ -89,24 +62,19 @@ class CreateDriver bool publish_tf_; int baud_; - void cmdVelCallback(const geometry_msgs::TwistConstPtr& msg); - void debrisLEDCallback(const std_msgs::BoolConstPtr& msg); - void spotLEDCallback(const std_msgs::BoolConstPtr& msg); - void dockLEDCallback(const std_msgs::BoolConstPtr& msg); - void checkLEDCallback(const std_msgs::BoolConstPtr& msg); - void powerLEDCallback(const std_msgs::UInt8MultiArrayConstPtr& msg); - void setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr& msg); - void dockCallback(const std_msgs::EmptyConstPtr& msg); - void undockCallback(const std_msgs::EmptyConstPtr& msg); - void defineSongCallback(const ca_msgs::DefineSongConstPtr& msg); - void playSongCallback(const ca_msgs::PlaySongConstPtr& msg); + void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + void debrisLEDCallback(const std_msgs::msg::Bool::SharedPtr msg); + void spotLEDCallback(const std_msgs::msg::Bool::SharedPtr msg); + void dockLEDCallback(const std_msgs::msg::Bool::SharedPtr msg); + void checkLEDCallback(const std_msgs::msg::Bool::SharedPtr msg); + void powerLEDCallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void setASCIICallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void dockCallback(const std_msgs::msg::Empty::SharedPtr msg); + void undockCallback(const std_msgs::msg::Empty::SharedPtr msg); + void defineSongCallback(const ca_msgs::msg::DefineSong::SharedPtr msg); + void playSongCallback(const ca_msgs::msg::PlaySong::SharedPtr msg); - bool update(); - void updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateModeDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateDriverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); + void update(); void publishOdom(); void publishJointState(); void publishBatteryInfo(); @@ -117,45 +85,39 @@ class CreateDriver void publishWheeldrop(); protected: - ros::NodeHandle nh_; - ros::NodeHandle priv_nh_; - ros::Subscriber cmd_vel_sub_; - ros::Subscriber debris_led_sub_; - ros::Subscriber spot_led_sub_; - ros::Subscriber dock_led_sub_; - ros::Subscriber check_led_sub_; - ros::Subscriber power_led_sub_; - ros::Subscriber set_ascii_sub_; - ros::Subscriber dock_sub_; - ros::Subscriber undock_sub_; - ros::Subscriber define_song_sub_; - ros::Subscriber play_song_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + rclcpp::Subscription::SharedPtr debris_led_sub_; + rclcpp::Subscription::SharedPtr spot_led_sub_; + rclcpp::Subscription::SharedPtr dock_led_sub_; + rclcpp::Subscription::SharedPtr check_led_sub_; + rclcpp::Subscription::SharedPtr power_led_sub_; + rclcpp::Subscription::SharedPtr set_ascii_sub_; + rclcpp::Subscription::SharedPtr dock_sub_; + rclcpp::Subscription::SharedPtr undock_sub_; + rclcpp::Subscription::SharedPtr define_song_sub_; + rclcpp::Subscription::SharedPtr play_song_sub_; - ros::Publisher odom_pub_; - ros::Publisher clean_btn_pub_; - ros::Publisher day_btn_pub_; - ros::Publisher hour_btn_pub_; - ros::Publisher min_btn_pub_; - ros::Publisher dock_btn_pub_; - ros::Publisher spot_btn_pub_; - ros::Publisher voltage_pub_; - ros::Publisher current_pub_; - ros::Publisher charge_pub_; - ros::Publisher charge_ratio_pub_; - ros::Publisher capacity_pub_; - ros::Publisher temperature_pub_; - ros::Publisher charging_state_pub_; - ros::Publisher omni_char_pub_; - ros::Publisher mode_pub_; - ros::Publisher bumper_pub_; - ros::Publisher wheeldrop_pub_; - ros::Publisher wheel_joint_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr clean_btn_pub_; + rclcpp::Publisher::SharedPtr day_btn_pub_; + rclcpp::Publisher::SharedPtr hour_btn_pub_; + rclcpp::Publisher::SharedPtr min_btn_pub_; + rclcpp::Publisher::SharedPtr dock_btn_pub_; + rclcpp::Publisher::SharedPtr spot_btn_pub_; + rclcpp::Publisher::SharedPtr voltage_pub_; + rclcpp::Publisher::SharedPtr current_pub_; + rclcpp::Publisher::SharedPtr charge_pub_; + rclcpp::Publisher::SharedPtr charge_ratio_pub_; + rclcpp::Publisher::SharedPtr capacity_pub_; + rclcpp::Publisher::SharedPtr temperature_pub_; + rclcpp::Publisher::SharedPtr charging_state_pub_; + rclcpp::Publisher::SharedPtr omni_char_pub_; + rclcpp::Publisher::SharedPtr mode_pub_; + rclcpp::Publisher::SharedPtr bumper_pub_; + rclcpp::Publisher::SharedPtr wheeldrop_pub_; + rclcpp::Publisher::SharedPtr wheel_joint_pub_; public: - explicit CreateDriver(ros::NodeHandle& nh); + explicit CreateDriver(const std::string & name); ~CreateDriver(); - virtual void spin(); - virtual void spinOnce(); -}; // class CreateDriver - -#endif // CREATE_DRIVER_CREATE_DRIVER_H +}; diff --git a/ca_driver/package.xml b/ca_driver/package.xml index 456a0abf4..953e220d7 100644 --- a/ca_driver/package.xml +++ b/ca_driver/package.xml @@ -1,10 +1,11 @@ - + ca_driver - 1.3.0 - ROS driver for iRobot's Create and Roomba platforms, based on libcreate + 2.0.0 + ROS 2 driver for iRobot's Create and Roomba platforms, based on libcreate Jacob Perron + Yutaka Kondo BSD @@ -12,23 +13,23 @@ Jacob Perron - catkin - - roslint + ament_cmake ca_msgs diagnostic_msgs - diagnostic_updater + geometry_msgs libcreate nav_msgs - roscpp + rclcpp sensor_msgs std_msgs - tf + tf2 + tf2_ros ca_description + ament_cmake diff --git a/ca_driver/src/create_driver.cpp b/ca_driver/src/create_driver.cpp index 8f723e4c8..86f335334 100644 --- a/ca_driver/src/create_driver.cpp +++ b/ca_driver/src/create_driver.cpp @@ -1,52 +1,23 @@ -/** -Software License Agreement (BSD) -\file create_driver.cpp -\authors Jacob Perron -\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved. -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of Autonomy Lab nor the names of its contributors may - be used to endorse or promote products derived from this software without - specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. -*/ +#include +#include +#include #include "create_driver/create_driver.h" -#include - -#include - -CreateDriver::CreateDriver(ros::NodeHandle& nh) - : nh_(nh), - priv_nh_("~"), - diagnostics_(), +CreateDriver::CreateDriver(const std::string & name) + : Node(name), model_(create::RobotModel::CREATE_2), - is_running_slowly_(false) + ros_clock_(RCL_ROS_TIME) { - bool create_one; + using namespace std::chrono_literals; + std::string robot_model_name; - priv_nh_.param("dev", dev_, "/dev/ttyUSB0"); - priv_nh_.param("robot_model", robot_model_name, "CREATE_2"); - priv_nh_.param("base_frame", base_frame_, "base_footprint"); - priv_nh_.param("odom_frame", odom_frame_, "odom"); - priv_nh_.param("latch_cmd_duration", latch_duration_, 0.2); - priv_nh_.param("loop_hz", loop_hz_, 10.0); - priv_nh_.param("publish_tf", publish_tf_, true); + get_parameter_or("dev", dev_, "/dev/ttyUSB0"); + get_parameter_or("robot_model", robot_model_name, "CREATE_2"); + get_parameter_or("base_frame", base_frame_, "base_footprint"); + get_parameter_or("odom_frame", odom_frame_, "odom"); + get_parameter_or("latch_cmd_duration", latch_duration_, 0.2); + get_parameter_or("loop_hz", loop_hz_, 10.0); + get_parameter_or("publish_tf", publish_tf_, true); if (robot_model_name == "ROOMBA_400") { @@ -62,30 +33,34 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh) } else { - ROS_FATAL_STREAM("[CREATE] Robot model \"" + robot_model_name + "\" is not known."); - ros::shutdown(); + RCLCPP_FATAL(get_logger(), "[CREATE] Robot model \"%s\" is not known.", + robot_model_name.c_str()); + rclcpp::shutdown(); return; } - ROS_INFO_STREAM("[CREATE] \"" << robot_model_name << "\" selected"); + RCLCPP_INFO(get_logger(), "[CREATE] \"%s\" selected", + robot_model_name.c_str()); - priv_nh_.param("baud", baud_, model_.getBaud()); + get_parameter_or("baud", baud_, model_.getBaud()); - robot_ = new create::Create(model_); + robot_ = std::make_unique(model_); if (!robot_->connect(dev_, baud_)) { - ROS_FATAL("[CREATE] Failed to establish serial connection with Create."); - ros::shutdown(); + RCLCPP_FATAL(get_logger(), + "[CREATE] Failed to establish serial connection with Create."); + rclcpp::shutdown(); } - ROS_INFO("[CREATE] Connection established."); + RCLCPP_INFO(get_logger(), "[CREATE] Connection established."); // Start in full control mode robot_->setMode(create::MODE_FULL); // Show robot's battery level - ROS_INFO("[CREATE] Battery level %.2f %%", (robot_->getBatteryCharge() / robot_->getBatteryCapacity()) * 100.0); + RCLCPP_INFO(get_logger(), "[CREATE] Battery level %.2f %%", + (robot_->getBatteryCharge() / robot_->getBatteryCapacity()) * 100.0); // Set frame_id's mode_msg_.header.frame_id = base_frame_; @@ -110,89 +85,95 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh) } // Setup subscribers - cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &CreateDriver::cmdVelCallback, this); - debris_led_sub_ = nh.subscribe("debris_led", 10, &CreateDriver::debrisLEDCallback, this); - spot_led_sub_ = nh.subscribe("spot_led", 10, &CreateDriver::spotLEDCallback, this); - dock_led_sub_ = nh.subscribe("dock_led", 10, &CreateDriver::dockLEDCallback, this); - check_led_sub_ = nh.subscribe("check_led", 10, &CreateDriver::checkLEDCallback, this); - power_led_sub_ = nh.subscribe("power_led", 10, &CreateDriver::powerLEDCallback, this); - set_ascii_sub_ = nh.subscribe("set_ascii", 10, &CreateDriver::setASCIICallback, this); - dock_sub_ = nh.subscribe("dock", 10, &CreateDriver::dockCallback, this); - undock_sub_ = nh.subscribe("undock", 10, &CreateDriver::undockCallback, this); - define_song_sub_ = nh.subscribe("define_song", 10, &CreateDriver::defineSongCallback, this); - play_song_sub_ = nh.subscribe("play_song", 10, &CreateDriver::playSongCallback, this); + cmd_vel_sub_ = create_subscription( + "cmd_vel", std::bind(&CreateDriver::cmdVelCallback, this, std::placeholders::_1)); + debris_led_sub_ = create_subscription( + "debris_led", std::bind(&CreateDriver::debrisLEDCallback, this, std::placeholders::_1)); + spot_led_sub_ = create_subscription( + "spot_led", std::bind(&CreateDriver::spotLEDCallback, this, std::placeholders::_1)); + dock_led_sub_ = create_subscription( + "dock_led", std::bind(&CreateDriver::dockLEDCallback, this, std::placeholders::_1)); + check_led_sub_ = create_subscription( + "check_led", std::bind(&CreateDriver::checkLEDCallback, this, std::placeholders::_1)); + power_led_sub_ = create_subscription( + "power_led", std::bind(&CreateDriver::powerLEDCallback, this, std::placeholders::_1)); + set_ascii_sub_ = create_subscription( + "set_ascii", std::bind(&CreateDriver::setASCIICallback, this, std::placeholders::_1)); + dock_sub_ = create_subscription( + "dock", std::bind(&CreateDriver::dockCallback, this, std::placeholders::_1)); + undock_sub_ = create_subscription( + "undock", std::bind(&CreateDriver::undockCallback, this, std::placeholders::_1)); + define_song_sub_ = create_subscription( + "define_song", std::bind(&CreateDriver::defineSongCallback, this, std::placeholders::_1)); + play_song_sub_ = create_subscription( + "play_song", std::bind(&CreateDriver::playSongCallback, this, std::placeholders::_1)); // Setup publishers - odom_pub_ = nh.advertise("odom", 30); - clean_btn_pub_ = nh.advertise("clean_button", 30); - day_btn_pub_ = nh.advertise("day_button", 30); - hour_btn_pub_ = nh.advertise("hour_button", 30); - min_btn_pub_ = nh.advertise("minute_button", 30); - dock_btn_pub_ = nh.advertise("dock_button", 30); - spot_btn_pub_ = nh.advertise("spot_button", 30); - voltage_pub_ = nh.advertise("battery/voltage", 30); - current_pub_ = nh.advertise("battery/current", 30); - charge_pub_ = nh.advertise("battery/charge", 30); - charge_ratio_pub_ = nh.advertise("battery/charge_ratio", 30); - capacity_pub_ = nh.advertise("battery/capacity", 30); - temperature_pub_ = nh.advertise("battery/temperature", 30); - charging_state_pub_ = nh.advertise("battery/charging_state", 30); - omni_char_pub_ = nh.advertise("ir_omni", 30); - mode_pub_ = nh.advertise("mode", 30); - bumper_pub_ = nh.advertise("bumper", 30); - wheeldrop_pub_ = nh.advertise("wheeldrop", 30); - wheel_joint_pub_ = nh.advertise("joint_states", 10); - - // Setup diagnostics - diagnostics_.add("Battery Status", this, &CreateDriver::updateBatteryDiagnostics); - diagnostics_.add("Safety Status", this, &CreateDriver::updateSafetyDiagnostics); - diagnostics_.add("Serial Status", this, &CreateDriver::updateSerialDiagnostics); - diagnostics_.add("Base Mode", this, &CreateDriver::updateModeDiagnostics); - diagnostics_.add("Driver Status", this, &CreateDriver::updateDriverDiagnostics); - - diagnostics_.setHardwareID(robot_model_name); - - ROS_INFO("[CREATE] Ready."); + odom_pub_ = create_publisher("odom", 30); + clean_btn_pub_ = create_publisher("clean_button"); + day_btn_pub_ = create_publisher("day_button"); + hour_btn_pub_ = create_publisher("hour_button"); + min_btn_pub_ = create_publisher("minute_button"); + dock_btn_pub_ = create_publisher("dock_button"); + spot_btn_pub_ = create_publisher("spot_button"); + voltage_pub_ = create_publisher("battery/voltage"); + current_pub_ = create_publisher("battery/current"); + charge_pub_ = create_publisher("battery/charge"); + charge_ratio_pub_ = create_publisher("battery/charge_ratio"); + capacity_pub_ = create_publisher("battery/capacity"); + temperature_pub_ = create_publisher("battery/temperature"); + charging_state_pub_ = create_publisher("battery/charging_state"); + omni_char_pub_ = create_publisher("ir_omni"); + mode_pub_ = create_publisher("mode"); + bumper_pub_ = create_publisher("bumper"); + wheeldrop_pub_ = create_publisher("wheeldrop"); + wheel_joint_pub_ = create_publisher("joint_states"); + + timer_ = create_wall_timer(100ms, + [this]() -> void { + this->update(); + }); + + RCLCPP_INFO(get_logger(), "[CREATE] Ready."); } CreateDriver::~CreateDriver() { - ROS_INFO("[CREATE] Destruct sequence initiated."); + RCLCPP_INFO(get_logger(), "[CREATE] Destruct sequence initiated."); robot_->disconnect(); - delete robot_; } -void CreateDriver::cmdVelCallback(const geometry_msgs::TwistConstPtr& msg) +void CreateDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { robot_->drive(msg->linear.x, msg->angular.z); - last_cmd_vel_time_ = ros::Time::now(); + last_cmd_vel_time_ = ros_clock_.now(); } -void CreateDriver::debrisLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::debrisLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableDebrisLED(msg->data); } -void CreateDriver::spotLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::spotLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableSpotLED(msg->data); } -void CreateDriver::dockLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::dockLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableDockLED(msg->data); } -void CreateDriver::checkLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::checkLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableCheckRobotLED(msg->data); } -void CreateDriver::powerLEDCallback(const std_msgs::UInt8MultiArrayConstPtr& msg) +void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { - if (msg->data.size() < 1) + if (msg->data.empty()) { - ROS_ERROR("[CREATE] No values provided to set power LED"); + RCLCPP_ERROR(get_logger(), "[CREATE] No values provided to set power LED"); } else { @@ -207,12 +188,12 @@ void CreateDriver::powerLEDCallback(const std_msgs::UInt8MultiArrayConstPtr& msg } } -void CreateDriver::setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr& msg) +void CreateDriver::setASCIICallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { - bool result; - if (msg->data.size() < 1) + bool result = false; + if (msg->data.empty()) { - ROS_ERROR("[CREATE] No ASCII digits provided"); + RCLCPP_ERROR(get_logger(), "[CREATE] No ASCII digits provided"); } else if (msg->data.size() < 2) { @@ -233,11 +214,11 @@ void CreateDriver::setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr& msg if (!result) { - ROS_ERROR("[CREATE] ASCII character out of range [32, 126]"); + RCLCPP_ERROR(get_logger(), "[CREATE] ASCII character out of range [32, 126]"); } } -void CreateDriver::dockCallback(const std_msgs::EmptyConstPtr& msg) +void CreateDriver::dockCallback(const std_msgs::msg::Empty::SharedPtr msg) { robot_->setMode(create::MODE_PASSIVE); @@ -248,29 +229,30 @@ void CreateDriver::dockCallback(const std_msgs::EmptyConstPtr& msg) robot_->dock(); } -void CreateDriver::undockCallback(const std_msgs::EmptyConstPtr& msg) +void CreateDriver::undockCallback(const std_msgs::msg::Empty::SharedPtr msg) { // Switch robot back to FULL mode robot_->setMode(create::MODE_FULL); } -void CreateDriver::defineSongCallback(const ca_msgs::DefineSongConstPtr& msg) +void CreateDriver::defineSongCallback(const ca_msgs::msg::DefineSong::SharedPtr msg) { if (!robot_->defineSong(msg->song, msg->length, &(msg->notes.front()), &(msg->durations.front()))) { - ROS_ERROR_STREAM("[CREATE] Failed to define song " << msg->song << " of length " << msg->length); + RCLCPP_ERROR(get_logger(), "[CREATE] Failed to define song %d of length %d", + msg->song, msg->length); } } -void CreateDriver::playSongCallback(const ca_msgs::PlaySongConstPtr& msg) +void CreateDriver::playSongCallback(const ca_msgs::msg::PlaySong::SharedPtr msg) { if (!robot_->playSong(msg->song)) { - ROS_ERROR_STREAM("[CREATE] Failed to play song " << msg->song); + RCLCPP_ERROR(get_logger(), "[CREATE] Failed to play song %d", msg->song); } } -bool CreateDriver::update() +void CreateDriver::update() { publishOdom(); publishJointState(); @@ -282,145 +264,10 @@ bool CreateDriver::update() publishWheeldrop(); // If last velocity command was sent longer than latch duration, stop robot - if (ros::Time::now() - last_cmd_vel_time_ >= ros::Duration(latch_duration_)) + if (ros_clock_.now() - last_cmd_vel_time_ >= rclcpp::Duration(latch_duration_)) { robot_->drive(0, 0); } - - return true; -} - -void CreateDriver::updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - const float charge = robot_->getBatteryCharge(); - const float capacity = robot_->getBatteryCapacity(); - const create::ChargingState charging_state = robot_->getChargingState(); - const float charge_ratio = charge / capacity; - - if (charging_state == create::CHARGE_FAULT) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Charging fault reported by base"); - } - else if (charge_ratio == 0) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Battery reports no charge"); - } - else if (charge_ratio < 0.1) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery reports less than 10% charge"); - } - else - { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Battery is OK"); - } - - stat.add("Charge (Ah)", charge); - stat.add("Capacity (Ah)", capacity); - stat.add("Temperature (Celsius)", robot_->getTemperature()); - stat.add("Current (A)", robot_->getCurrent()); - stat.add("Voltage (V)", robot_->getVoltage()); - - switch (charging_state) - { - case create::CHARGE_NONE: - stat.add("Charging state", "Not charging"); - break; - case create::CHARGE_RECONDITION: - stat.add("Charging state", "Reconditioning"); - break; - case create::CHARGE_FULL: - stat.add("Charging state", "Full charge"); - break; - case create::CHARGE_TRICKLE: - stat.add("Charging state", "Trickle charging"); - break; - case create::CHARGE_WAITING: - stat.add("Charging state", "Waiting"); - break; - case create::CHARGE_FAULT: - stat.add("Charging state", "Fault"); - break; - } -} - -void CreateDriver::updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - const bool is_wheeldrop = robot_->isWheeldrop(); - const bool is_cliff = robot_->isCliff(); - if (is_wheeldrop) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wheeldrop detected"); - } - else if (is_cliff) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Cliff detected"); - } - else - { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "No safety issues detected"); - } - - stat.add("Wheeldrop", is_wheeldrop); - stat.add("Cliff", is_cliff); -} - -void CreateDriver::updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - const bool is_connected = robot_->connected(); - const uint64_t corrupt_packets = robot_->getNumCorruptPackets(); - const uint64_t total_packets = robot_->getTotalPackets(); - - if (!is_connected) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Serial port to base not open"); - } - else if (corrupt_packets) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, - "Corrupt packets detected. If the number of corrupt packets is increasing, data may be unreliable"); - } - else - { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Serial connection is good"); - } - - stat.add("Corrupt packets", corrupt_packets); - stat.add("Total packets", total_packets); -} - -void CreateDriver::updateModeDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - const create::CreateMode mode = robot_->getMode(); - switch (mode) - { - case create::MODE_UNAVAILABLE: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Unknown mode of operation"); - break; - case create::MODE_OFF: - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Mode is set to OFF"); - break; - case create::MODE_PASSIVE: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to PASSIVE"); - break; - case create::MODE_SAFE: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to SAFE"); - break; - case create::MODE_FULL: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to FULL"); - break; - } -} - -void CreateDriver::updateDriverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - if (is_running_slowly_) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Internal loop running slowly"); - } - else - { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Maintaining loop frequency"); - } } void CreateDriver::publishOdom() @@ -429,11 +276,15 @@ void CreateDriver::publishOdom() create::Vel vel = robot_->getVel(); // Populate position info - geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromRollPitchYaw(0, 0, pose.yaw); - odom_msg_.header.stamp = ros::Time::now(); + tf2::Quaternion quat; + quat.setRPY(0, 0, pose.yaw); + odom_msg_.header.stamp = ros_clock_.now(); odom_msg_.pose.pose.position.x = pose.x; odom_msg_.pose.pose.position.y = pose.y; - odom_msg_.pose.pose.orientation = quat; + odom_msg_.pose.pose.orientation.x = quat.x(); + odom_msg_.pose.pose.orientation.y = quat.y(); + odom_msg_.pose.pose.orientation.z = quat.z(); + odom_msg_.pose.pose.orientation.w = quat.w(); // Populate velocity info odom_msg_.twist.twist.linear.x = vel.x; @@ -462,99 +313,102 @@ void CreateDriver::publishOdom() if (publish_tf_) { - tf_odom_.header.stamp = ros::Time::now(); + tf_odom_.header.stamp = ros_clock_.now(); tf_odom_.transform.translation.x = pose.x; tf_odom_.transform.translation.y = pose.y; - tf_odom_.transform.rotation = quat; - tf_broadcaster_.sendTransform(tf_odom_); + tf_odom_.transform.rotation.x = quat.x(); + tf_odom_.transform.rotation.y = quat.y(); + tf_odom_.transform.rotation.z = quat.z(); + tf_odom_.transform.rotation.w = quat.w(); + tf_broadcaster_->sendTransform(tf_odom_); } - odom_pub_.publish(odom_msg_); + odom_pub_->publish(odom_msg_); } void CreateDriver::publishJointState() { // Publish joint states - float wheelRadius = model_.getWheelDiameter() / 2.0; + double wheelRadius = model_.getWheelDiameter() / 2.0; - joint_state_msg_.header.stamp = ros::Time::now(); + joint_state_msg_.header.stamp = ros_clock_.now(); joint_state_msg_.position[0] = robot_->getLeftWheelDistance() / wheelRadius; joint_state_msg_.position[1] = robot_->getRightWheelDistance() / wheelRadius; joint_state_msg_.velocity[0] = robot_->getRequestedLeftWheelVel() / wheelRadius; joint_state_msg_.velocity[1] = robot_->getRequestedRightWheelVel() / wheelRadius; - wheel_joint_pub_.publish(joint_state_msg_); + wheel_joint_pub_->publish(joint_state_msg_); } void CreateDriver::publishBatteryInfo() { float32_msg_.data = robot_->getVoltage(); - voltage_pub_.publish(float32_msg_); + voltage_pub_->publish(float32_msg_); float32_msg_.data = robot_->getCurrent(); - current_pub_.publish(float32_msg_); + current_pub_->publish(float32_msg_); float32_msg_.data = robot_->getBatteryCharge(); - charge_pub_.publish(float32_msg_); + charge_pub_->publish(float32_msg_); float32_msg_.data = robot_->getBatteryCapacity(); - capacity_pub_.publish(float32_msg_); + capacity_pub_->publish(float32_msg_); int16_msg_.data = robot_->getTemperature(); - temperature_pub_.publish(int16_msg_); + temperature_pub_->publish(int16_msg_); float32_msg_.data = robot_->getBatteryCharge() / robot_->getBatteryCapacity(); - charge_ratio_pub_.publish(float32_msg_); + charge_ratio_pub_->publish(float32_msg_); const create::ChargingState charging_state = robot_->getChargingState(); - charging_state_msg_.header.stamp = ros::Time::now(); + charging_state_msg_.header.stamp = ros_clock_.now(); switch (charging_state) { case create::CHARGE_NONE: - charging_state_msg_.state = charging_state_msg_.CHARGE_NONE; + charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_NONE; break; case create::CHARGE_RECONDITION: - charging_state_msg_.state = charging_state_msg_.CHARGE_RECONDITION; + charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_RECONDITION; break; case create::CHARGE_FULL: - charging_state_msg_.state = charging_state_msg_.CHARGE_FULL; + charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_FULL; break; case create::CHARGE_TRICKLE: - charging_state_msg_.state = charging_state_msg_.CHARGE_TRICKLE; + charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_TRICKLE; break; case create::CHARGE_WAITING: - charging_state_msg_.state = charging_state_msg_.CHARGE_WAITING; + charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_WAITING; break; case create::CHARGE_FAULT: - charging_state_msg_.state = charging_state_msg_.CHARGE_FAULT; + charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_FAULT; break; } - charging_state_pub_.publish(charging_state_msg_); + charging_state_pub_->publish(charging_state_msg_); } void CreateDriver::publishButtonPresses() const { if (robot_->isCleanButtonPressed()) { - clean_btn_pub_.publish(empty_msg_); + clean_btn_pub_->publish(empty_msg_); } if (robot_->isDayButtonPressed()) { - day_btn_pub_.publish(empty_msg_); + day_btn_pub_->publish(empty_msg_); } if (robot_->isHourButtonPressed()) { - hour_btn_pub_.publish(empty_msg_); + hour_btn_pub_->publish(empty_msg_); } if (robot_->isMinButtonPressed()) { - min_btn_pub_.publish(empty_msg_); + min_btn_pub_->publish(empty_msg_); } if (robot_->isDockButtonPressed()) { - dock_btn_pub_.publish(empty_msg_); + dock_btn_pub_->publish(empty_msg_); } if (robot_->isSpotButtonPressed()) { - spot_btn_pub_.publish(empty_msg_); + spot_btn_pub_->publish(empty_msg_); } } @@ -562,14 +416,14 @@ void CreateDriver::publishOmniChar() { uint8_t ir_char = robot_->getIROmni(); uint16_msg_.data = ir_char; - omni_char_pub_.publish(uint16_msg_); + omni_char_pub_->publish(uint16_msg_); // TODO(jacobperron): Publish info based on character, such as dock in sight } void CreateDriver::publishMode() { const create::CreateMode mode = robot_->getMode(); - mode_msg_.header.stamp = ros::Time::now(); + mode_msg_.header.stamp = ros_clock_.now(); switch (mode) { case create::MODE_OFF: @@ -585,15 +439,15 @@ void CreateDriver::publishMode() mode_msg_.mode = mode_msg_.MODE_FULL; break; default: - ROS_ERROR("[CREATE] Unknown mode detected"); + RCLCPP_ERROR(get_logger(), "[CREATE] Unknown mode detected"); break; } - mode_pub_.publish(mode_msg_); + mode_pub_->publish(mode_msg_); } void CreateDriver::publishBumperInfo() { - bumper_msg_.header.stamp = ros::Time::now(); + bumper_msg_.header.stamp = ros_clock_.now(); bumper_msg_.is_left_pressed = robot_->isLeftBumper(); bumper_msg_.is_right_pressed = robot_->isRightBumper(); @@ -614,52 +468,23 @@ void CreateDriver::publishBumperInfo() bumper_msg_.light_signal_center_right = robot_->getLightSignalCenterRight(); } - bumper_pub_.publish(bumper_msg_); + bumper_pub_->publish(bumper_msg_); } void CreateDriver::publishWheeldrop() { if (robot_->isWheeldrop()) - wheeldrop_pub_.publish(empty_msg_); -} - -void CreateDriver::spinOnce() -{ - update(); - diagnostics_.update(); - ros::spinOnce(); -} - -void CreateDriver::spin() -{ - ros::Rate rate(loop_hz_); - while (ros::ok()) - { - spinOnce(); - - is_running_slowly_ = !rate.sleep(); - if (is_running_slowly_) - { - ROS_WARN("[CREATE] Loop running slowly."); - } - } + wheeldrop_pub_->publish(empty_msg_); } int main(int argc, char** argv) { - ros::init(argc, argv, "ca_driver"); - ros::NodeHandle nh; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); - CreateDriver create_driver(nh); + auto node = std::make_shared("ca_driver"); + rclcpp::spin(node); + rclcpp::shutdown(); - try - { - create_driver.spin(); - } - catch (std::runtime_error& ex) - { - ROS_FATAL_STREAM("[CREATE] Runtime error: " << ex.what()); - return 1; - } return 0; } diff --git a/ca_msgs/CMakeLists.txt b/ca_msgs/CMakeLists.txt index 055bcb357..f57ebc9a1 100644 --- a/ca_msgs/CMakeLists.txt +++ b/ca_msgs/CMakeLists.txt @@ -1,25 +1,23 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ca_msgs) -find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -add_message_files( - FILES - Bumper.msg - ChargingState.msg - DefineSong.msg - Mode.msg - PlaySong.msg -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(common_interfaces REQUIRED) -generate_messages( - DEPENDENCIES - std_msgs -) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Bumper.msg" + "msg/ChargingState.msg" + "msg/DefineSong.msg" + "msg/Mode.msg" + "msg/PlaySong.msg" + DEPENDENCIES std_msgs + ) -catkin_package( - CATKIN_DEPENDS message_runtime std_msgs -) +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/ca_msgs/msg/Bumper.msg b/ca_msgs/msg/Bumper.msg index fda36a58a..d6532140a 100644 --- a/ca_msgs/msg/Bumper.msg +++ b/ca_msgs/msg/Bumper.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header # Contact sensors bool is_left_pressed diff --git a/ca_msgs/msg/ChargingState.msg b/ca_msgs/msg/ChargingState.msg index aa4559a13..06117ce57 100644 --- a/ca_msgs/msg/ChargingState.msg +++ b/ca_msgs/msg/ChargingState.msg @@ -5,5 +5,5 @@ uint8 CHARGE_TRICKLE=3 uint8 CHARGE_WAITING=4 uint8 CHARGE_FAULT=5 -Header header +std_msgs/Header header uint8 state diff --git a/ca_msgs/msg/Mode.msg b/ca_msgs/msg/Mode.msg index a0adb2b6f..9412b3ec9 100644 --- a/ca_msgs/msg/Mode.msg +++ b/ca_msgs/msg/Mode.msg @@ -3,5 +3,5 @@ uint8 MODE_PASSIVE=1 uint8 MODE_SAFE=2 uint8 MODE_FULL=3 -Header header +std_msgs/Header header uint8 mode diff --git a/ca_msgs/package.xml b/ca_msgs/package.xml index fd9c12372..357cb550a 100644 --- a/ca_msgs/package.xml +++ b/ca_msgs/package.xml @@ -1,10 +1,11 @@ - + ca_msgs - 1.3.0 + 2.0.0 Common message definitions for create_autonomy Jacob Perron + Yutaka Kondo BSD @@ -12,14 +13,15 @@ Jacob Perron - catkin - - message_generation + ament_cmake + rosidl_default_generators std_msgs - - message_runtime + common_interfaces + rosidl_default_runtime + rosidl_interface_packages + ament_cmake diff --git a/ca_tools/CMakeLists.txt b/ca_tools/CMakeLists.txt index 1be0a5d8f..367095e57 100644 --- a/ca_tools/CMakeLists.txt +++ b/ca_tools/CMakeLists.txt @@ -1,11 +1,16 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ca_tools) -find_package(catkin REQUIRED) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -catkin_package() +find_package(ament_cmake REQUIRED) +find_package(joy REQUIRED) +find_package(teleop_twist_joy REQUIRED) -install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN ".svn" EXCLUDE -) +install(DIRECTORY config + DESTINATION config + ) + +ament_package() diff --git a/ca_tools/package.xml b/ca_tools/package.xml index ae90b288d..6e082f77a 100644 --- a/ca_tools/package.xml +++ b/ca_tools/package.xml @@ -1,10 +1,11 @@ - + ca_tools - 1.3.0 + 2.0.0 Launch and configuration files for common accessories when working with Create/Roomba platforms. Jacob Perron + Yutaka Kondo BSD @@ -12,11 +13,12 @@ Jacob Perron - catkin + ament_cmake joy - joy_teleop + teleop_twist_joy + ament_cmake diff --git a/create_autonomy/CMakeLists.txt b/create_autonomy/CMakeLists.txt index 686329989..3b71d6dac 100644 --- a/create_autonomy/CMakeLists.txt +++ b/create_autonomy/CMakeLists.txt @@ -1,4 +1,14 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(create_autonomy) -find_package(catkin REQUIRED) -catkin_metapackage() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(ament_cmake REQUIRED) +# find_package(ca_description EQUIRED) +# find_package(ca_driver REQUIRED) +find_package(ca_msgs REQUIRED) +# find_package(ca_tools REQUIRED) + +ament_package() diff --git a/create_autonomy/package.xml b/create_autonomy/package.xml index 67457642e..dd8984df5 100644 --- a/create_autonomy/package.xml +++ b/create_autonomy/package.xml @@ -1,10 +1,11 @@ - + create_autonomy - 1.3.0 - ROS driver for iRobot's Create 1 and 2, based on the libcreate C++ library + 2.0.0 + ROS 2 driver for iRobot's Create 1 and 2, based on the libcreate C++ library Jacob Perron + Yutaka Kondo BSD @@ -13,14 +14,14 @@ Jacob Perron - catkin + ament_cmake - ca_description - ca_driver - ca_msgs - ca_tools + ca_description + ca_driver + ca_msgs + ca_tools - + ament_cmake