From 4127147af18ce719bdf8b1c7f30c20fbb3adaa92 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 18:08:19 +0900 Subject: [PATCH 01/27] update ca_msgs to ros2 --- ca_msgs/CMakeLists.txt | 38 +++++++++++++++++------------------ ca_msgs/msg/Bumper.msg | 2 +- ca_msgs/msg/ChargingState.msg | 2 +- ca_msgs/msg/Mode.msg | 2 +- ca_msgs/package.xml | 16 ++++++++------- 5 files changed, 30 insertions(+), 30 deletions(-) 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 From ff528f6290fe026327d22ef32110cdef68fe452e Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 18:09:00 +0900 Subject: [PATCH 02/27] update create_autonomy to ros2 --- create_autonomy/CMakeLists.txt | 16 +++++++++++++--- create_autonomy/package.xml | 19 ++++++++++--------- 2 files changed, 23 insertions(+), 12 deletions(-) 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..b1565b045 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_msgs + - + ament_cmake From 54d09b463c0abb01867550b5b95ae4ed37c7f3fd Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 18:36:01 +0900 Subject: [PATCH 03/27] convert *.xacro.urdf to *.urdf --- ca_description/urdf/create_1.urdf | 410 +++++++++++++++++++++++ ca_description/urdf/create_1_gazebo.urdf | 8 + ca_description/urdf/create_2.urdf | 410 +++++++++++++++++++++++ ca_description/urdf/create_2_gazebo.urdf | 8 + 4 files changed, 836 insertions(+) create mode 100644 ca_description/urdf/create_1.urdf create mode 100644 ca_description/urdf/create_1_gazebo.urdf create mode 100644 ca_description/urdf/create_2.urdf create mode 100644 ca_description/urdf/create_2_gazebo.urdf 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 @@ + + + + + + + + From 408b19f9e51d2394894414481bc7be4880922aa9 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 18:51:07 +0900 Subject: [PATCH 04/27] update ca_description to ros2 --- ca_description/CMakeLists.txt | 33 ++++++++++++++------------------- ca_description/package.xml | 14 +++++++------- 2 files changed, 21 insertions(+), 26 deletions(-) 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 From e56c355753adc8294fc0d23c2d041eccf33d5484 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 18:54:43 +0900 Subject: [PATCH 05/27] update ca_tools to ros2 --- ca_tools/CMakeLists.txt | 18 +++++++++++------- ca_tools/package.xml | 8 +++++--- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/ca_tools/CMakeLists.txt b/ca_tools/CMakeLists.txt index 1be0a5d8f..5c128c4d6 100644 --- a/ca_tools/CMakeLists.txt +++ b/ca_tools/CMakeLists.txt @@ -1,11 +1,15 @@ -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(urdf 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..c6642486c 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 + ament_cmake From 3f526ae5412329305666636e9182d2dc0050f19e Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 19:17:27 +0900 Subject: [PATCH 06/27] ignore ca_driver --- ca_driver/AMENT_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 ca_driver/AMENT_IGNORE diff --git a/ca_driver/AMENT_IGNORE b/ca_driver/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb From d16bd048e1b960ad3600106abe27eb894faa2db0 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 20:04:52 +0900 Subject: [PATCH 07/27] add ca_driver2 --- ca_driver2/CMakeLists.txt | 60 ++ ca_driver2/README.md | 3 + ca_driver2/config/default.yaml | 20 + .../include/create_driver/create_driver.h | 161 +++++ ca_driver2/package.xml | 33 + ca_driver2/src/create_driver.cpp | 665 ++++++++++++++++++ 6 files changed, 942 insertions(+) create mode 100644 ca_driver2/CMakeLists.txt create mode 100644 ca_driver2/README.md create mode 100644 ca_driver2/config/default.yaml create mode 100644 ca_driver2/include/create_driver/create_driver.h create mode 100644 ca_driver2/package.xml create mode 100644 ca_driver2/src/create_driver.cpp diff --git a/ca_driver2/CMakeLists.txt b/ca_driver2/CMakeLists.txt new file mode 100644 index 000000000..546f3fa57 --- /dev/null +++ b/ca_driver2/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.5) +project(ca_driver2) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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) + +include_directories( + include + ${ca_msgs_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + # ${diagnostic_updater_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${libcreate_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) +ament_target_dependencies(${PROJECT_NAME} + "ca_msgs" + "diagnostic_msgs" + # "diagnostic_updater" + "geometry_msgs" + "libcreate" + "nav_msgs" + "rclcpp" + "sensor_msgs" + "std_msgs" + "tf2" + ) +install(TARGETS ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" + ) + +install(DIRECTORY config + DESTINATION config + ) + +ament_package() diff --git a/ca_driver2/README.md b/ca_driver2/README.md new file mode 100644 index 000000000..f841415b4 --- /dev/null +++ b/ca_driver2/README.md @@ -0,0 +1,3 @@ +# ca_driver + +ROS driver for iRobot's Create 1 and 2. diff --git a/ca_driver2/config/default.yaml b/ca_driver2/config/default.yaml new file mode 100644 index 000000000..79817e569 --- /dev/null +++ b/ca_driver2/config/default.yaml @@ -0,0 +1,20 @@ +# The device path for the robot +dev: "/dev/ttyUSB0" + +# Baud rate. Passing this parameter overwrites the inferred value based on the robot_model +# baud: 115200 + +# Base frame ID +base_frame: "base_footprint" + +# Odometry frame ID +odom_frame: "odom" + +# Time (s) without receiving a velocity command before stopping the robot +latch_cmd_duration: 0.2 + +# Internal loop update rate (Hz) +loop_hz: 10.0 + +# Whether to publish the transform between odom_frame and base_frame +publish_tf: true diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h new file mode 100644 index 000000000..c7013e6b7 --- /dev/null +++ b/ca_driver2/include/create_driver/create_driver.h @@ -0,0 +1,161 @@ +/** +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 + +#include +#include + +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) + 1e-5, 1e-5, 0.0, 0.0, 0.0, 1e-5, + 0.0, 0.0, MAX_DBL, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, MAX_DBL, 0.0, 0.0, + 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 +{ +private: + create::Create* 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_; + + // ROS params + std::string dev_; + std::string base_frame_; + std::string odom_frame_; + double latch_duration_; + double loop_hz_; + 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); + + 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 publishOdom(); + void publishJointState(); + void publishBatteryInfo(); + void publishButtonPresses() const; + void publishOmniChar(); + void publishMode(); + void publishBumperInfo(); + 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_; + + 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_; + +public: + explicit CreateDriver(ros::NodeHandle& nh); + ~CreateDriver(); + virtual void spin(); + virtual void spinOnce(); +}; // class CreateDriver + +#endif // CREATE_DRIVER_CREATE_DRIVER_H diff --git a/ca_driver2/package.xml b/ca_driver2/package.xml new file mode 100644 index 000000000..06f475dcb --- /dev/null +++ b/ca_driver2/package.xml @@ -0,0 +1,33 @@ + + + ca_driver2 + 2.0.0 + ROS 2 driver for iRobot's Create and Roomba platforms, based on libcreate + + Yutaka Kondo + + Apache License 2.0 + + http://wiki.ros.org/ca_driver + + Yutaka Kondo + + ament_cmake + + ca_msgs + diagnostic_msgs + + geometry_msgs + libcreate + nav_msgs + rclcpp + sensor_msgs + std_msgs + tf2 + + ca_description + + + ament_cmake + + diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp new file mode 100644 index 000000000..8f723e4c8 --- /dev/null +++ b/ca_driver2/src/create_driver.cpp @@ -0,0 +1,665 @@ +/** +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 "create_driver/create_driver.h" + +#include + +#include + +CreateDriver::CreateDriver(ros::NodeHandle& nh) + : nh_(nh), + priv_nh_("~"), + diagnostics_(), + model_(create::RobotModel::CREATE_2), + is_running_slowly_(false) +{ + bool create_one; + 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); + + if (robot_model_name == "ROOMBA_400") + { + model_ = create::RobotModel::ROOMBA_400; + } + else if (robot_model_name == "CREATE_1") + { + model_ = create::RobotModel::CREATE_1; + } + else if (robot_model_name == "CREATE_2") + { + model_ = create::RobotModel::CREATE_2; + } + else + { + ROS_FATAL_STREAM("[CREATE] Robot model \"" + robot_model_name + "\" is not known."); + ros::shutdown(); + return; + } + + ROS_INFO_STREAM("[CREATE] \"" << robot_model_name << "\" selected"); + + priv_nh_.param("baud", baud_, model_.getBaud()); + + robot_ = new create::Create(model_); + + if (!robot_->connect(dev_, baud_)) + { + ROS_FATAL("[CREATE] Failed to establish serial connection with Create."); + ros::shutdown(); + } + + ROS_INFO("[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); + + // Set frame_id's + mode_msg_.header.frame_id = base_frame_; + bumper_msg_.header.frame_id = base_frame_; + charging_state_msg_.header.frame_id = base_frame_; + tf_odom_.header.frame_id = odom_frame_; + tf_odom_.child_frame_id = base_frame_; + odom_msg_.header.frame_id = odom_frame_; + odom_msg_.child_frame_id = base_frame_; + joint_state_msg_.name.resize(2); + joint_state_msg_.position.resize(2); + joint_state_msg_.velocity.resize(2); + joint_state_msg_.effort.resize(2); + joint_state_msg_.name[0] = "left_wheel_joint"; + joint_state_msg_.name[1] = "right_wheel_joint"; + + // Populate intial covariances + for (int i = 0; i < 36; i++) + { + odom_msg_.pose.covariance[i] = COVARIANCE[i]; + odom_msg_.twist.covariance[i] = COVARIANCE[i]; + } + + // 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); + + // 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."); +} + +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, msg->angular.z); + last_cmd_vel_time_ = ros::Time::now(); +} + +void CreateDriver::debrisLEDCallback(const std_msgs::BoolConstPtr& msg) +{ + robot_->enableDebrisLED(msg->data); +} + +void CreateDriver::spotLEDCallback(const std_msgs::BoolConstPtr& msg) +{ + robot_->enableSpotLED(msg->data); +} + +void CreateDriver::dockLEDCallback(const std_msgs::BoolConstPtr& msg) +{ + robot_->enableDockLED(msg->data); +} + +void CreateDriver::checkLEDCallback(const std_msgs::BoolConstPtr& msg) +{ + robot_->enableCheckRobotLED(msg->data); +} + +void CreateDriver::powerLEDCallback(const std_msgs::UInt8MultiArrayConstPtr& msg) +{ + if (msg->data.size() < 1) + { + ROS_ERROR("[CREATE] No values provided to set power LED"); + } + else + { + if (msg->data.size() < 2) + { + robot_->setPowerLED(msg->data[0]); + } + else + { + robot_->setPowerLED(msg->data[0], msg->data[1]); + } + } +} + +void CreateDriver::setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr& msg) +{ + bool result; + if (msg->data.size() < 1) + { + ROS_ERROR("[CREATE] No ASCII digits provided"); + } + else if (msg->data.size() < 2) + { + result = robot_->setDigitsASCII(msg->data[0], ' ', ' ', ' '); + } + else if (msg->data.size() < 3) + { + result = robot_->setDigitsASCII(msg->data[0], msg->data[1], ' ', ' '); + } + else if (msg->data.size() < 4) + { + result = robot_->setDigitsASCII(msg->data[0], msg->data[1], msg->data[2], ' '); + } + else + { + result = robot_->setDigitsASCII(msg->data[0], msg->data[1], msg->data[2], msg->data[3]); + } + + if (!result) + { + ROS_ERROR("[CREATE] ASCII character out of range [32, 126]"); + } +} + +void CreateDriver::dockCallback(const std_msgs::EmptyConstPtr& msg) +{ + robot_->setMode(create::MODE_PASSIVE); + + if (model_.getVersion() <= create::V_2) + usleep(1000000); // Create 1 requires a delay (1 sec) + + // Call docking behaviour + robot_->dock(); +} + +void CreateDriver::undockCallback(const std_msgs::EmptyConstPtr& msg) +{ + // Switch robot back to FULL mode + robot_->setMode(create::MODE_FULL); +} + +void CreateDriver::defineSongCallback(const ca_msgs::DefineSongConstPtr& 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); + } +} + +void CreateDriver::playSongCallback(const ca_msgs::PlaySongConstPtr& msg) +{ + if (!robot_->playSong(msg->song)) + { + ROS_ERROR_STREAM("[CREATE] Failed to play song " << msg->song); + } +} + +bool CreateDriver::update() +{ + publishOdom(); + publishJointState(); + publishBatteryInfo(); + publishButtonPresses(); + publishOmniChar(); + publishMode(); + publishBumperInfo(); + 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_)) + { + 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() +{ + create::Pose pose = robot_->getPose(); + 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(); + odom_msg_.pose.pose.position.x = pose.x; + odom_msg_.pose.pose.position.y = pose.y; + odom_msg_.pose.pose.orientation = quat; + + // Populate velocity info + odom_msg_.twist.twist.linear.x = vel.x; + odom_msg_.twist.twist.linear.y = vel.y; + odom_msg_.twist.twist.angular.z = vel.yaw; + + // Update covariances + odom_msg_.pose.covariance[0] = static_cast(pose.covariance[0]); + odom_msg_.pose.covariance[1] = pose.covariance[1]; + odom_msg_.pose.covariance[5] = pose.covariance[2]; + odom_msg_.pose.covariance[6] = pose.covariance[3]; + odom_msg_.pose.covariance[7] = pose.covariance[4]; + odom_msg_.pose.covariance[11] = pose.covariance[5]; + odom_msg_.pose.covariance[30] = pose.covariance[6]; + odom_msg_.pose.covariance[31] = pose.covariance[7]; + odom_msg_.pose.covariance[35] = pose.covariance[8]; + odom_msg_.twist.covariance[0] = vel.covariance[0]; + odom_msg_.twist.covariance[1] = vel.covariance[1]; + odom_msg_.twist.covariance[5] = vel.covariance[2]; + odom_msg_.twist.covariance[6] = vel.covariance[3]; + odom_msg_.twist.covariance[7] = vel.covariance[4]; + odom_msg_.twist.covariance[11] = vel.covariance[5]; + odom_msg_.twist.covariance[30] = vel.covariance[6]; + odom_msg_.twist.covariance[31] = vel.covariance[7]; + odom_msg_.twist.covariance[35] = vel.covariance[8]; + + if (publish_tf_) + { + tf_odom_.header.stamp = ros::Time::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_); + } + + odom_pub_.publish(odom_msg_); +} + +void CreateDriver::publishJointState() +{ + // Publish joint states + float wheelRadius = model_.getWheelDiameter() / 2.0; + + joint_state_msg_.header.stamp = ros::Time::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_); +} + +void CreateDriver::publishBatteryInfo() +{ + float32_msg_.data = robot_->getVoltage(); + voltage_pub_.publish(float32_msg_); + float32_msg_.data = robot_->getCurrent(); + current_pub_.publish(float32_msg_); + float32_msg_.data = robot_->getBatteryCharge(); + charge_pub_.publish(float32_msg_); + float32_msg_.data = robot_->getBatteryCapacity(); + capacity_pub_.publish(float32_msg_); + int16_msg_.data = robot_->getTemperature(); + temperature_pub_.publish(int16_msg_); + float32_msg_.data = robot_->getBatteryCharge() / robot_->getBatteryCapacity(); + charge_ratio_pub_.publish(float32_msg_); + + const create::ChargingState charging_state = robot_->getChargingState(); + charging_state_msg_.header.stamp = ros::Time::now(); + switch (charging_state) + { + case create::CHARGE_NONE: + charging_state_msg_.state = charging_state_msg_.CHARGE_NONE; + break; + case create::CHARGE_RECONDITION: + charging_state_msg_.state = charging_state_msg_.CHARGE_RECONDITION; + break; + + case create::CHARGE_FULL: + charging_state_msg_.state = charging_state_msg_.CHARGE_FULL; + break; + + case create::CHARGE_TRICKLE: + charging_state_msg_.state = charging_state_msg_.CHARGE_TRICKLE; + break; + + case create::CHARGE_WAITING: + charging_state_msg_.state = charging_state_msg_.CHARGE_WAITING; + break; + + case create::CHARGE_FAULT: + charging_state_msg_.state = charging_state_msg_.CHARGE_FAULT; + break; + } + charging_state_pub_.publish(charging_state_msg_); +} + +void CreateDriver::publishButtonPresses() const +{ + if (robot_->isCleanButtonPressed()) + { + clean_btn_pub_.publish(empty_msg_); + } + if (robot_->isDayButtonPressed()) + { + day_btn_pub_.publish(empty_msg_); + } + if (robot_->isHourButtonPressed()) + { + hour_btn_pub_.publish(empty_msg_); + } + if (robot_->isMinButtonPressed()) + { + min_btn_pub_.publish(empty_msg_); + } + if (robot_->isDockButtonPressed()) + { + dock_btn_pub_.publish(empty_msg_); + } + if (robot_->isSpotButtonPressed()) + { + spot_btn_pub_.publish(empty_msg_); + } +} + +void CreateDriver::publishOmniChar() +{ + uint8_t ir_char = robot_->getIROmni(); + uint16_msg_.data = ir_char; + 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(); + switch (mode) + { + case create::MODE_OFF: + mode_msg_.mode = mode_msg_.MODE_OFF; + break; + case create::MODE_PASSIVE: + mode_msg_.mode = mode_msg_.MODE_PASSIVE; + break; + case create::MODE_SAFE: + mode_msg_.mode = mode_msg_.MODE_SAFE; + break; + case create::MODE_FULL: + mode_msg_.mode = mode_msg_.MODE_FULL; + break; + default: + ROS_ERROR("[CREATE] Unknown mode detected"); + break; + } + mode_pub_.publish(mode_msg_); +} + +void CreateDriver::publishBumperInfo() +{ + bumper_msg_.header.stamp = ros::Time::now(); + bumper_msg_.is_left_pressed = robot_->isLeftBumper(); + bumper_msg_.is_right_pressed = robot_->isRightBumper(); + + if (model_.getVersion() >= create::V_3) + { + bumper_msg_.is_light_left = robot_->isLightBumperLeft(); + bumper_msg_.is_light_front_left = robot_->isLightBumperFrontLeft(); + bumper_msg_.is_light_center_left = robot_->isLightBumperCenterLeft(); + bumper_msg_.is_light_right = robot_->isLightBumperRight(); + bumper_msg_.is_light_front_right = robot_->isLightBumperFrontRight(); + bumper_msg_.is_light_center_right = robot_->isLightBumperCenterRight(); + + bumper_msg_.light_signal_left = robot_->getLightSignalLeft(); + bumper_msg_.light_signal_front_left = robot_->getLightSignalFrontLeft(); + bumper_msg_.light_signal_center_left = robot_->getLightSignalCenterLeft(); + bumper_msg_.light_signal_right = robot_->getLightSignalRight(); + bumper_msg_.light_signal_front_right = robot_->getLightSignalFrontRight(); + bumper_msg_.light_signal_center_right = robot_->getLightSignalCenterRight(); + } + + 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."); + } + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "ca_driver"); + ros::NodeHandle nh; + + CreateDriver create_driver(nh); + + try + { + create_driver.spin(); + } + catch (std::runtime_error& ex) + { + ROS_FATAL_STREAM("[CREATE] Runtime error: " << ex.what()); + return 1; + } + return 0; +} From b1479946b653ba6f990a1aec190ae1972292d154 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 20:42:44 +0900 Subject: [PATCH 08/27] use rclcpp --- .../include/create_driver/create_driver.h | 204 ++++++++---------- ca_driver2/src/create_driver.cpp | 27 --- 2 files changed, 86 insertions(+), 145 deletions(-) diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h index c7013e6b7..99563c413 100644 --- a/ca_driver2/include/create_driver/create_driver.h +++ b/ca_driver2/include/create_driver/create_driver.h @@ -1,56 +1,30 @@ -/** -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 + +#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,24 +34,24 @@ 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_; + tf2_ros::TransformBroadcaster tf_broadcaster_; +// diagnostic_updater::Updater diagnostics_; + 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::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_; bool is_running_slowly_; // ROS params @@ -89,24 +63,24 @@ 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::ConstSharedPtr & msg); + void debrisLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); + void spotLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); + void dockLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); + void checkLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); + void powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr & msg); + void setASCIICallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr & msg); + void dockCallback(const std_msgs::msg::Empty::ConstSharedPtr & msg); + void undockCallback(const std_msgs::msg::Empty::ConstSharedPtr & msg); + void defineSongCallback(const ca_msgs::msg::DefineSong::ConstSharedPtr & msg); + void playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr & 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 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 publishOdom(); void publishJointState(); void publishBatteryInfo(); @@ -117,45 +91,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_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 8f723e4c8..59a80b3aa 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -1,30 +1,3 @@ -/** -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 "create_driver/create_driver.h" #include From e2b636e5680066b5b9ef5b7ac4140ca9f252595a Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sun, 24 Feb 2019 20:56:00 +0900 Subject: [PATCH 09/27] comment out diagnostics --- .../include/create_driver/create_driver.h | 2 +- ca_driver2/src/create_driver.cpp | 276 +++++++++--------- 2 files changed, 137 insertions(+), 141 deletions(-) diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h index 99563c413..c3f6462e7 100644 --- a/ca_driver2/include/create_driver/create_driver.h +++ b/ca_driver2/include/create_driver/create_driver.h @@ -39,7 +39,7 @@ class CreateDriver : public rclcpp::Node private: std::unique_ptr robot_; create::RobotModel model_; - tf2_ros::TransformBroadcaster tf_broadcaster_; + std::shared_ptr tf_broadcaster_; // diagnostic_updater::Updater diagnostics_; ca_msgs::msg::Mode mode_msg_; ca_msgs::msg::ChargingState charging_state_msg_; diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 59a80b3aa..404db42c8 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -1,13 +1,9 @@ +#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), +// diagnostics_(), model_(create::RobotModel::CREATE_2), is_running_slowly_(false) { @@ -263,138 +259,138 @@ bool CreateDriver::update() 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::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() { From 5424b54d0d6bed53ce4ef3b166b2d5bc9bf9c1a3 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Mon, 25 Feb 2019 06:40:22 +0900 Subject: [PATCH 10/27] remove diagnostics --- .../include/create_driver/create_driver.h | 7 - ca_driver2/src/create_driver.cpp | 144 ------------------ 2 files changed, 151 deletions(-) diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h index c3f6462e7..ce75bcfe0 100644 --- a/ca_driver2/include/create_driver/create_driver.h +++ b/ca_driver2/include/create_driver/create_driver.h @@ -6,7 +6,6 @@ #include #include -//#include #include #include #include @@ -40,7 +39,6 @@ class CreateDriver : public rclcpp::Node std::unique_ptr robot_; create::RobotModel model_; std::shared_ptr tf_broadcaster_; -// diagnostic_updater::Updater diagnostics_; ca_msgs::msg::Mode mode_msg_; ca_msgs::msg::ChargingState charging_state_msg_; ca_msgs::msg::Bumper bumper_msg_; @@ -76,11 +74,6 @@ class CreateDriver : public rclcpp::Node void playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr & 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 publishOdom(); void publishJointState(); void publishBatteryInfo(); diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 404db42c8..9d42f3d76 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -3,7 +3,6 @@ CreateDriver::CreateDriver(const std::string & name) : Node(name), -// diagnostics_(), model_(create::RobotModel::CREATE_2), is_running_slowly_(false) { @@ -112,15 +111,6 @@ CreateDriver::CreateDriver(const std::string & name) 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."); } @@ -259,139 +249,6 @@ bool CreateDriver::update() 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() { create::Pose pose = robot_->getPose(); @@ -595,7 +452,6 @@ void CreateDriver::publishWheeldrop() void CreateDriver::spinOnce() { update(); - diagnostics_.update(); ros::spinOnce(); } From 9ffdbd8ca182accc9ff74822777b9019f6351950 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Mon, 25 Feb 2019 07:48:16 +0900 Subject: [PATCH 11/27] updating cpp --- ca_driver2/src/create_driver.cpp | 165 +++++++++++++++++-------------- 1 file changed, 90 insertions(+), 75 deletions(-) diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 9d42f3d76..08ea6512f 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -8,13 +8,13 @@ CreateDriver::CreateDriver(const std::string & name) { bool create_one; 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") { @@ -30,30 +30,34 @@ CreateDriver::CreateDriver(const std::string & name) } 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_; @@ -78,80 +82,90 @@ CreateDriver::CreateDriver(const std::string & name) } // 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, _1)); + debris_led_sub_ = create_subscription( + "debris_led", std::bind(&CreateDriver::debrisLEDCallback, this, _1)); + spot_led_sub_ = create_subscription( + "spot_led", std::bind(&CreateDriver::spotLEDCallback, this, _1)); + dock_led_sub_ = create_subscription( + "dock_led", std::bind(&CreateDriver::dockLEDCallback, this, _1)); + check_led_sub_ = create_subscription( + "check_led", std::bind(&CreateDriver::checkLEDCallback, this, _1)); + power_led_sub_ = create_subscription( + "power_led", std::bind(&CreateDriver::powerLEDCallback, this, _1)); + set_ascii_sub_ = create_subscription( + "set_ascii", std::bind(&CreateDriver::setASCIICallback, this, _1)); + dock_sub_ = create_subscription( + "dock", std::bind(&CreateDriver::dockCallback, this, _1)); + undock_sub_ = create_subscription( + "undock", std::bind(&CreateDriver::undockCallback, this, _1)); + define_song_sub_ = create_subscription( + "define_song", std::bind(&CreateDriver::defineSongCallback, this, _1)); + play_song_sub_ = create_subscription( + "play_song", std::bind(&CreateDriver::playSongCallback, this, _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); - - 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"); + + 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::ConstSharedPtr& msg) { robot_->drive(msg->linear.x, msg->angular.z); - last_cmd_vel_time_ = ros::Time::now(); + last_cmd_vel_time_ = rclcpp::Clock()::now(); } -void CreateDriver::debrisLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::debrisLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) { robot_->enableDebrisLED(msg->data); } -void CreateDriver::spotLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::spotLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) { robot_->enableSpotLED(msg->data); } -void CreateDriver::dockLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::dockLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) { robot_->enableDockLED(msg->data); } -void CreateDriver::checkLEDCallback(const std_msgs::BoolConstPtr& msg) +void CreateDriver::checkLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) { robot_->enableCheckRobotLED(msg->data); } -void CreateDriver::powerLEDCallback(const std_msgs::UInt8MultiArrayConstPtr& msg) +void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr& msg) { if (msg->data.size() < 1) { - ROS_ERROR("[CREATE] No values provided to set power LED"); + RCLCPP_ERROR(get_logger(), "[CREATE] No values provided to set power LED"); } else { @@ -166,12 +180,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::ConstSharedPtr& msg) { bool result; if (msg->data.size() < 1) { - ROS_ERROR("[CREATE] No ASCII digits provided"); + RCLCPP_ERROR(get_logger(), "[CREATE] No ASCII digits provided"); } else if (msg->data.size() < 2) { @@ -192,11 +206,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::ConstSharedPtr& msg) { robot_->setMode(create::MODE_PASSIVE); @@ -207,13 +221,13 @@ 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::ConstSharedPtr& 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::ConstSharedPtr& msg) { if (!robot_->defineSong(msg->song, msg->length, &(msg->notes.front()), &(msg->durations.front()))) { @@ -221,7 +235,7 @@ void CreateDriver::defineSongCallback(const ca_msgs::DefineSongConstPtr& msg) } } -void CreateDriver::playSongCallback(const ca_msgs::PlaySongConstPtr& msg) +void CreateDriver::playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr& msg) { if (!robot_->playSong(msg->song)) { @@ -241,7 +255,7 @@ 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 (rclcpp::Clock()::now() - last_cmd_vel_time_ >= ros::Duration(latch_duration_)) { robot_->drive(0, 0); } @@ -255,8 +269,9 @@ 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(); + geometry_msgs::msg::Quaternion quat = + tf2::createQuaternionMsgFromRollPitchYaw(0, 0, pose.yaw); + odom_msg_.header.stamp = rclcpp::Clock()::now(); odom_msg_.pose.pose.position.x = pose.x; odom_msg_.pose.pose.position.y = pose.y; odom_msg_.pose.pose.orientation = quat; @@ -288,7 +303,7 @@ void CreateDriver::publishOdom() if (publish_tf_) { - tf_odom_.header.stamp = ros::Time::now(); + tf_odom_.header.stamp = rclcpp::Clock()::now(); tf_odom_.transform.translation.x = pose.x; tf_odom_.transform.translation.y = pose.y; tf_odom_.transform.rotation = quat; @@ -303,7 +318,7 @@ void CreateDriver::publishJointState() // Publish joint states float wheelRadius = model_.getWheelDiameter() / 2.0; - joint_state_msg_.header.stamp = ros::Time::now(); + joint_state_msg_.header.stamp = rclcpp::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; @@ -327,7 +342,7 @@ void CreateDriver::publishBatteryInfo() 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 = rclcpp::Clock()::now(); switch (charging_state) { case create::CHARGE_NONE: @@ -395,7 +410,7 @@ void CreateDriver::publishOmniChar() void CreateDriver::publishMode() { const create::CreateMode mode = robot_->getMode(); - mode_msg_.header.stamp = ros::Time::now(); + mode_msg_.header.stamp = rclcpp::Clock()::now(); switch (mode) { case create::MODE_OFF: @@ -411,7 +426,7 @@ 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_); @@ -419,7 +434,7 @@ void CreateDriver::publishMode() void CreateDriver::publishBumperInfo() { - bumper_msg_.header.stamp = ros::Time::now(); + bumper_msg_.header.stamp = rclcpp::Clock()::now(); bumper_msg_.is_left_pressed = robot_->isLeftBumper(); bumper_msg_.is_right_pressed = robot_->isRightBumper(); @@ -465,7 +480,7 @@ void CreateDriver::spin() is_running_slowly_ = !rate.sleep(); if (is_running_slowly_) { - ROS_WARN("[CREATE] Loop running slowly."); + RCLCPP_WARN(get_logger(), "[CREATE] Loop running slowly."); } } } From 0d70c012eba5b4147dd3dcd7c2148f4815d22577 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Thu, 28 Feb 2019 07:48:52 +0900 Subject: [PATCH 12/27] solve clang-tidy --- ca_driver2/src/create_driver.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 08ea6512f..7252296fd 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -6,7 +6,6 @@ CreateDriver::CreateDriver(const std::string & name) model_(create::RobotModel::CREATE_2), is_running_slowly_(false) { - bool create_one; std::string robot_model_name; get_parameter_or("dev", dev_, "/dev/ttyUSB0"); get_parameter_or("robot_model", robot_model_name, "CREATE_2"); @@ -163,7 +162,7 @@ void CreateDriver::checkLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& m void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr& msg) { - if (msg->data.size() < 1) + if (msg->data.empty()) { RCLCPP_ERROR(get_logger(), "[CREATE] No values provided to set power LED"); } @@ -183,7 +182,7 @@ void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstS void CreateDriver::setASCIICallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr& msg) { bool result; - if (msg->data.size() < 1) + if (msg->data.empty()) { RCLCPP_ERROR(get_logger(), "[CREATE] No ASCII digits provided"); } @@ -316,7 +315,7 @@ void CreateDriver::publishOdom() void CreateDriver::publishJointState() { // Publish joint states - float wheelRadius = model_.getWheelDiameter() / 2.0; + double wheelRadius = model_.getWheelDiameter() / 2.0; joint_state_msg_.header.stamp = rclcpp::Clock()::now(); joint_state_msg_.position[0] = robot_->getLeftWheelDistance() / wheelRadius; @@ -346,26 +345,26 @@ void CreateDriver::publishBatteryInfo() 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_); From 2f8472ebf5c99c216d59d4a1bd8f0f60b2bff9bf Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Thu, 28 Feb 2019 07:58:59 +0900 Subject: [PATCH 13/27] use ros_clock_.now() --- .../include/create_driver/create_driver.h | 1 + ca_driver2/src/create_driver.cpp | 17 +++++++++-------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h index ce75bcfe0..6214cacbf 100644 --- a/ca_driver2/include/create_driver/create_driver.h +++ b/ca_driver2/include/create_driver/create_driver.h @@ -44,6 +44,7 @@ class CreateDriver : public rclcpp::Node 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_; diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 7252296fd..9db774930 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -4,6 +4,7 @@ CreateDriver::CreateDriver(const std::string & name) : Node(name), model_(create::RobotModel::CREATE_2), + ros_clock_(RCL_ROS_TIME), is_running_slowly_(false) { std::string robot_model_name; @@ -137,7 +138,7 @@ CreateDriver::~CreateDriver() void CreateDriver::cmdVelCallback(const geometry_msgs::msg::Twist::ConstSharedPtr& msg) { robot_->drive(msg->linear.x, msg->angular.z); - last_cmd_vel_time_ = rclcpp::Clock()::now(); + last_cmd_vel_time_ = ros_clock_.now(); } void CreateDriver::debrisLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) @@ -254,7 +255,7 @@ bool CreateDriver::update() publishWheeldrop(); // If last velocity command was sent longer than latch duration, stop robot - if (rclcpp::Clock()::now() - last_cmd_vel_time_ >= ros::Duration(latch_duration_)) + if (ros_clock_.now() - last_cmd_vel_time_ >= ros::Duration(latch_duration_)) { robot_->drive(0, 0); } @@ -270,7 +271,7 @@ void CreateDriver::publishOdom() // Populate position info geometry_msgs::msg::Quaternion quat = tf2::createQuaternionMsgFromRollPitchYaw(0, 0, pose.yaw); - odom_msg_.header.stamp = rclcpp::Clock()::now(); + 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; @@ -302,7 +303,7 @@ void CreateDriver::publishOdom() if (publish_tf_) { - tf_odom_.header.stamp = rclcpp::Clock()::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; @@ -317,7 +318,7 @@ void CreateDriver::publishJointState() // Publish joint states double wheelRadius = model_.getWheelDiameter() / 2.0; - joint_state_msg_.header.stamp = rclcpp::Clock()::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; @@ -341,7 +342,7 @@ void CreateDriver::publishBatteryInfo() charge_ratio_pub_.publish(float32_msg_); const create::ChargingState charging_state = robot_->getChargingState(); - charging_state_msg_.header.stamp = rclcpp::Clock()::now(); + charging_state_msg_.header.stamp = ros_clock_.now(); switch (charging_state) { case create::CHARGE_NONE: @@ -409,7 +410,7 @@ void CreateDriver::publishOmniChar() void CreateDriver::publishMode() { const create::CreateMode mode = robot_->getMode(); - mode_msg_.header.stamp = rclcpp::Clock()::now(); + mode_msg_.header.stamp = ros_clock_.now(); switch (mode) { case create::MODE_OFF: @@ -433,7 +434,7 @@ void CreateDriver::publishMode() void CreateDriver::publishBumperInfo() { - bumper_msg_.header.stamp = rclcpp::Clock()::now(); + bumper_msg_.header.stamp = ros_clock_.now(); bumper_msg_.is_left_pressed = robot_->isLeftBumper(); bumper_msg_.is_right_pressed = robot_->isRightBumper(); From 3527790e4668ffb5025d5f1e190626f89d827a2c Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Thu, 28 Feb 2019 08:00:58 +0900 Subject: [PATCH 14/27] import .gitignore --- .gitignore | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..9a604a7a1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,50 @@ + +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib +build + +# Executables +*.exe +*.out +*.app + +# org-mode +*.html +*.aux +*.bbl +*.blg +*.dvi +*.log +*.out +*.pdf +*.tex +*.toc +ltximg + +.idea +.DS_Store From 990448e93429185348f54d42f6bed7f66c3106ac Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sat, 2 Mar 2019 15:54:30 +0900 Subject: [PATCH 15/27] update spin --- .../include/create_driver/create_driver.h | 6 +-- ca_driver2/src/create_driver.cpp | 38 +++++-------------- 2 files changed, 13 insertions(+), 31 deletions(-) diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h index 6214cacbf..0599bdb97 100644 --- a/ca_driver2/include/create_driver/create_driver.h +++ b/ca_driver2/include/create_driver/create_driver.h @@ -38,6 +38,7 @@ class CreateDriver : public rclcpp::Node private: std::unique_ptr robot_; create::RobotModel model_; + rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr tf_broadcaster_; ca_msgs::msg::Mode mode_msg_; ca_msgs::msg::ChargingState charging_state_msg_; @@ -51,7 +52,6 @@ class CreateDriver : public rclcpp::Node std_msgs::msg::UInt16 uint16_msg_; std_msgs::msg::Int16 int16_msg_; sensor_msgs::msg::JointState joint_state_msg_; - bool is_running_slowly_; // ROS params std::string dev_; @@ -74,7 +74,7 @@ class CreateDriver : public rclcpp::Node void defineSongCallback(const ca_msgs::msg::DefineSong::ConstSharedPtr & msg); void playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr & msg); - bool update(); + void update(); void publishOdom(); void publishJointState(); void publishBatteryInfo(); @@ -120,4 +120,4 @@ class CreateDriver : public rclcpp::Node public: explicit CreateDriver(const std::string & name); ~CreateDriver(); -}; // class CreateDriver +}; diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 9db774930..c2703eba1 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -1,11 +1,11 @@ +#include #include #include "create_driver/create_driver.h" CreateDriver::CreateDriver(const std::string & name) : Node(name), model_(create::RobotModel::CREATE_2), - ros_clock_(RCL_ROS_TIME), - is_running_slowly_(false) + ros_clock_(RCL_ROS_TIME) { std::string robot_model_name; get_parameter_or("dev", dev_, "/dev/ttyUSB0"); @@ -126,6 +126,11 @@ CreateDriver::CreateDriver(const std::string & name) wheeldrop_pub_ = create_publisher("wheeldrop"); wheel_joint_pub_ = create_publisher("joint_states"); + timer_ = create_wall_timer(1.0 / loop_hz_, + std::bind(&CreateDriver::update, [this]() -> void { + this->update(); + })); + RCLCPP_INFO(get_logger(), "[CREATE] Ready."); } @@ -182,7 +187,7 @@ void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstS void CreateDriver::setASCIICallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr& msg) { - bool result; + bool result = false; if (msg->data.empty()) { RCLCPP_ERROR(get_logger(), "[CREATE] No ASCII digits provided"); @@ -243,7 +248,7 @@ void CreateDriver::playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr } } -bool CreateDriver::update() +void CreateDriver::update() { publishOdom(); publishJointState(); @@ -255,12 +260,10 @@ bool CreateDriver::update() publishWheeldrop(); // If last velocity command was sent longer than latch duration, stop robot - if (ros_clock_.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::publishOdom() @@ -464,27 +467,6 @@ void CreateDriver::publishWheeldrop() wheeldrop_pub_.publish(empty_msg_); } -void CreateDriver::spinOnce() -{ - update(); - ros::spinOnce(); -} - -void CreateDriver::spin() -{ - ros::Rate rate(loop_hz_); - while (ros::ok()) - { - spinOnce(); - - is_running_slowly_ = !rate.sleep(); - if (is_running_slowly_) - { - RCLCPP_WARN(get_logger(), "[CREATE] Loop running slowly."); - } - } -} - int main(int argc, char** argv) { ros::init(argc, argv, "ca_driver"); From ff55b5474f47487f5d93f171f3c809ed0c1263b0 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sat, 2 Mar 2019 16:02:48 +0900 Subject: [PATCH 16/27] fix main --- ca_driver2/src/create_driver.cpp | 22 ++++++++-------------- 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index c2703eba1..6a4d355e2 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -236,7 +236,8 @@ void CreateDriver::defineSongCallback(const ca_msgs::msg::DefineSong::ConstShare { 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); } } @@ -244,7 +245,7 @@ void CreateDriver::playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr { 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); } } @@ -469,19 +470,12 @@ void CreateDriver::publishWheeldrop() 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; } From 566731d9e14afd291224c373679e7bd1ef74cea2 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sat, 2 Mar 2019 16:45:22 +0900 Subject: [PATCH 17/27] succeed in buils --- .../include/create_driver/create_driver.h | 22 ++-- ca_driver2/src/create_driver.cpp | 107 ++++++++++-------- 2 files changed, 69 insertions(+), 60 deletions(-) diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h index 0599bdb97..2e6bfdca5 100644 --- a/ca_driver2/include/create_driver/create_driver.h +++ b/ca_driver2/include/create_driver/create_driver.h @@ -62,17 +62,17 @@ class CreateDriver : public rclcpp::Node bool publish_tf_; int baud_; - void cmdVelCallback(const geometry_msgs::msg::Twist::ConstSharedPtr & msg); - void debrisLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); - void spotLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); - void dockLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); - void checkLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr & msg); - void powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr & msg); - void setASCIICallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr & msg); - void dockCallback(const std_msgs::msg::Empty::ConstSharedPtr & msg); - void undockCallback(const std_msgs::msg::Empty::ConstSharedPtr & msg); - void defineSongCallback(const ca_msgs::msg::DefineSong::ConstSharedPtr & msg); - void playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr & 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); void update(); void publishOdom(); diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp index 6a4d355e2..86f335334 100644 --- a/ca_driver2/src/create_driver.cpp +++ b/ca_driver2/src/create_driver.cpp @@ -1,4 +1,5 @@ #include +#include #include #include "create_driver/create_driver.h" @@ -7,6 +8,8 @@ CreateDriver::CreateDriver(const std::string & name) model_(create::RobotModel::CREATE_2), ros_clock_(RCL_ROS_TIME) { + using namespace std::chrono_literals; + std::string robot_model_name; get_parameter_or("dev", dev_, "/dev/ttyUSB0"); get_parameter_or("robot_model", robot_model_name, "CREATE_2"); @@ -83,27 +86,27 @@ CreateDriver::CreateDriver(const std::string & name) // Setup subscribers cmd_vel_sub_ = create_subscription( - "cmd_vel", std::bind(&CreateDriver::cmdVelCallback, this, _1)); + "cmd_vel", std::bind(&CreateDriver::cmdVelCallback, this, std::placeholders::_1)); debris_led_sub_ = create_subscription( - "debris_led", std::bind(&CreateDriver::debrisLEDCallback, this, _1)); + "debris_led", std::bind(&CreateDriver::debrisLEDCallback, this, std::placeholders::_1)); spot_led_sub_ = create_subscription( - "spot_led", std::bind(&CreateDriver::spotLEDCallback, this, _1)); + "spot_led", std::bind(&CreateDriver::spotLEDCallback, this, std::placeholders::_1)); dock_led_sub_ = create_subscription( - "dock_led", std::bind(&CreateDriver::dockLEDCallback, this, _1)); + "dock_led", std::bind(&CreateDriver::dockLEDCallback, this, std::placeholders::_1)); check_led_sub_ = create_subscription( - "check_led", std::bind(&CreateDriver::checkLEDCallback, this, _1)); + "check_led", std::bind(&CreateDriver::checkLEDCallback, this, std::placeholders::_1)); power_led_sub_ = create_subscription( - "power_led", std::bind(&CreateDriver::powerLEDCallback, this, _1)); + "power_led", std::bind(&CreateDriver::powerLEDCallback, this, std::placeholders::_1)); set_ascii_sub_ = create_subscription( - "set_ascii", std::bind(&CreateDriver::setASCIICallback, this, _1)); + "set_ascii", std::bind(&CreateDriver::setASCIICallback, this, std::placeholders::_1)); dock_sub_ = create_subscription( - "dock", std::bind(&CreateDriver::dockCallback, this, _1)); + "dock", std::bind(&CreateDriver::dockCallback, this, std::placeholders::_1)); undock_sub_ = create_subscription( - "undock", std::bind(&CreateDriver::undockCallback, this, _1)); + "undock", std::bind(&CreateDriver::undockCallback, this, std::placeholders::_1)); define_song_sub_ = create_subscription( - "define_song", std::bind(&CreateDriver::defineSongCallback, this, _1)); + "define_song", std::bind(&CreateDriver::defineSongCallback, this, std::placeholders::_1)); play_song_sub_ = create_subscription( - "play_song", std::bind(&CreateDriver::playSongCallback, this, _1)); + "play_song", std::bind(&CreateDriver::playSongCallback, this, std::placeholders::_1)); // Setup publishers odom_pub_ = create_publisher("odom", 30); @@ -126,10 +129,10 @@ CreateDriver::CreateDriver(const std::string & name) wheeldrop_pub_ = create_publisher("wheeldrop"); wheel_joint_pub_ = create_publisher("joint_states"); - timer_ = create_wall_timer(1.0 / loop_hz_, - std::bind(&CreateDriver::update, [this]() -> void { + timer_ = create_wall_timer(100ms, + [this]() -> void { this->update(); - })); + }); RCLCPP_INFO(get_logger(), "[CREATE] Ready."); } @@ -140,33 +143,33 @@ CreateDriver::~CreateDriver() robot_->disconnect(); } -void CreateDriver::cmdVelCallback(const geometry_msgs::msg::Twist::ConstSharedPtr& msg) +void CreateDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { robot_->drive(msg->linear.x, msg->angular.z); last_cmd_vel_time_ = ros_clock_.now(); } -void CreateDriver::debrisLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) +void CreateDriver::debrisLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableDebrisLED(msg->data); } -void CreateDriver::spotLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) +void CreateDriver::spotLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableSpotLED(msg->data); } -void CreateDriver::dockLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) +void CreateDriver::dockLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableDockLED(msg->data); } -void CreateDriver::checkLEDCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) +void CreateDriver::checkLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) { robot_->enableCheckRobotLED(msg->data); } -void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr& msg) +void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { if (msg->data.empty()) { @@ -185,7 +188,7 @@ void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::ConstS } } -void CreateDriver::setASCIICallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr& msg) +void CreateDriver::setASCIICallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { bool result = false; if (msg->data.empty()) @@ -215,7 +218,7 @@ void CreateDriver::setASCIICallback(const std_msgs::msg::UInt8MultiArray::ConstS } } -void CreateDriver::dockCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg) +void CreateDriver::dockCallback(const std_msgs::msg::Empty::SharedPtr msg) { robot_->setMode(create::MODE_PASSIVE); @@ -226,13 +229,13 @@ void CreateDriver::dockCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg) robot_->dock(); } -void CreateDriver::undockCallback(const std_msgs::msg::Empty::ConstSharedPtr& 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::msg::DefineSong::ConstSharedPtr& msg) +void CreateDriver::defineSongCallback(const ca_msgs::msg::DefineSong::SharedPtr msg) { if (!robot_->defineSong(msg->song, msg->length, &(msg->notes.front()), &(msg->durations.front()))) { @@ -241,7 +244,7 @@ void CreateDriver::defineSongCallback(const ca_msgs::msg::DefineSong::ConstShare } } -void CreateDriver::playSongCallback(const ca_msgs::msg::PlaySong::ConstSharedPtr& msg) +void CreateDriver::playSongCallback(const ca_msgs::msg::PlaySong::SharedPtr msg) { if (!robot_->playSong(msg->song)) { @@ -273,12 +276,15 @@ void CreateDriver::publishOdom() create::Vel vel = robot_->getVel(); // Populate position info - geometry_msgs::msg::Quaternion quat = - tf2::createQuaternionMsgFromRollPitchYaw(0, 0, pose.yaw); + 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; @@ -310,11 +316,14 @@ void CreateDriver::publishOdom() 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() @@ -327,23 +336,23 @@ void CreateDriver::publishJointState() 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_clock_.now(); @@ -372,34 +381,34 @@ void CreateDriver::publishBatteryInfo() 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_); } } @@ -407,7 +416,7 @@ 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 } @@ -433,7 +442,7 @@ void CreateDriver::publishMode() RCLCPP_ERROR(get_logger(), "[CREATE] Unknown mode detected"); break; } - mode_pub_.publish(mode_msg_); + mode_pub_->publish(mode_msg_); } void CreateDriver::publishBumperInfo() @@ -459,13 +468,13 @@ 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_); + wheeldrop_pub_->publish(empty_msg_); } int main(int argc, char** argv) From 98b87d9474d13cd9f936f52ab144cd7d9a947e57 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sat, 2 Mar 2019 16:45:34 +0900 Subject: [PATCH 18/27] depend tf2_ros --- ca_driver2/CMakeLists.txt | 1 + ca_driver2/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/ca_driver2/CMakeLists.txt b/ca_driver2/CMakeLists.txt index 546f3fa57..cb5696872 100644 --- a/ca_driver2/CMakeLists.txt +++ b/ca_driver2/CMakeLists.txt @@ -16,6 +16,7 @@ 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 diff --git a/ca_driver2/package.xml b/ca_driver2/package.xml index 06f475dcb..7318c0211 100644 --- a/ca_driver2/package.xml +++ b/ca_driver2/package.xml @@ -24,6 +24,7 @@ sensor_msgs std_msgs tf2 + tf2_ros ca_description From 3a905f4d4aac7f8690f5334743d61b1ffa8f951d Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sat, 2 Mar 2019 19:11:05 +0900 Subject: [PATCH 19/27] fix cmake --- ca_driver2/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/ca_driver2/CMakeLists.txt b/ca_driver2/CMakeLists.txt index cb5696872..ddabe506b 100644 --- a/ca_driver2/CMakeLists.txt +++ b/ca_driver2/CMakeLists.txt @@ -44,6 +44,7 @@ ament_target_dependencies(${PROJECT_NAME} "sensor_msgs" "std_msgs" "tf2" + "tf2_ros" ) install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} From 96a6bc7446c9119c97472f4b48614ff5417264e8 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sat, 2 Mar 2019 19:20:25 +0900 Subject: [PATCH 20/27] update README --- README.md | 52 +++++++++++++++++++++++----------------------------- 1 file changed, 23 insertions(+), 29 deletions(-) diff --git a/README.md b/README.md index e8d3720c1..b7a46ab6f 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_driver2 create_2.launch.py ``` For Create 1 (Roomba 500 series): ``` bash -$ roslaunch ca_driver create_1.launch +$ ros2 launch ca_driver2 create_1.launch.py ``` For Roomba 400 series: ``` bash -$ roslaunch ca_driver roomba_400.launch +$ ros2 launch ca_driver2 roomba_400.launch.ph ``` #### 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_driver2 create_2.launch 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 - From 4baf4393396f261d2e99ceb07186d1a8e5ef680c Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Sat, 2 Mar 2019 19:51:06 +0900 Subject: [PATCH 21/27] depend teleop_twist_joy instead of joy_teleop --- ca_tools/CMakeLists.txt | 3 ++- ca_tools/package.xml | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ca_tools/CMakeLists.txt b/ca_tools/CMakeLists.txt index 5c128c4d6..367095e57 100644 --- a/ca_tools/CMakeLists.txt +++ b/ca_tools/CMakeLists.txt @@ -6,7 +6,8 @@ if(NOT CMAKE_CXX_STANDARD) endif() find_package(ament_cmake REQUIRED) -find_package(urdf REQUIRED) +find_package(joy REQUIRED) +find_package(teleop_twist_joy REQUIRED) install(DIRECTORY config DESTINATION config diff --git a/ca_tools/package.xml b/ca_tools/package.xml index c6642486c..6e082f77a 100644 --- a/ca_tools/package.xml +++ b/ca_tools/package.xml @@ -16,7 +16,7 @@ ament_cmake joy - joy_teleop + teleop_twist_joy ament_cmake From 30b412eb1b7079f2cbe09926c553064c8accebb6 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 12 Mar 2019 08:54:41 +0900 Subject: [PATCH 22/27] remove ca_driver2 --- ca_driver2/CMakeLists.txt | 62 --- ca_driver2/README.md | 3 - ca_driver2/config/default.yaml | 20 - .../include/create_driver/create_driver.h | 123 ----- ca_driver2/package.xml | 34 -- ca_driver2/src/create_driver.cpp | 490 ------------------ 6 files changed, 732 deletions(-) delete mode 100644 ca_driver2/CMakeLists.txt delete mode 100644 ca_driver2/README.md delete mode 100644 ca_driver2/config/default.yaml delete mode 100644 ca_driver2/include/create_driver/create_driver.h delete mode 100644 ca_driver2/package.xml delete mode 100644 ca_driver2/src/create_driver.cpp diff --git a/ca_driver2/CMakeLists.txt b/ca_driver2/CMakeLists.txt deleted file mode 100644 index ddabe506b..000000000 --- a/ca_driver2/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(ca_driver2) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -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} - ${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) -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} - DESTINATION lib/${PROJECT_NAME} - ) - -install(DIRECTORY include/${PROJECT_NAME} - DESTINATION include/${PROJECT_NAME} - FILES_MATCHING PATTERN "*.h" - ) - -install(DIRECTORY config - DESTINATION config - ) - -ament_package() diff --git a/ca_driver2/README.md b/ca_driver2/README.md deleted file mode 100644 index f841415b4..000000000 --- a/ca_driver2/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# ca_driver - -ROS driver for iRobot's Create 1 and 2. diff --git a/ca_driver2/config/default.yaml b/ca_driver2/config/default.yaml deleted file mode 100644 index 79817e569..000000000 --- a/ca_driver2/config/default.yaml +++ /dev/null @@ -1,20 +0,0 @@ -# The device path for the robot -dev: "/dev/ttyUSB0" - -# Baud rate. Passing this parameter overwrites the inferred value based on the robot_model -# baud: 115200 - -# Base frame ID -base_frame: "base_footprint" - -# Odometry frame ID -odom_frame: "odom" - -# Time (s) without receiving a velocity command before stopping the robot -latch_cmd_duration: 0.2 - -# Internal loop update rate (Hz) -loop_hz: 10.0 - -# Whether to publish the transform between odom_frame and base_frame -publish_tf: true diff --git a/ca_driver2/include/create_driver/create_driver.h b/ca_driver2/include/create_driver/create_driver.h deleted file mode 100644 index 2e6bfdca5..000000000 --- a/ca_driver2/include/create_driver/create_driver.h +++ /dev/null @@ -1,123 +0,0 @@ -#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) - 1e-5, 1e-5, 0.0, 0.0, 0.0, 1e-5, - 0.0, 0.0, MAX_DBL, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, MAX_DBL, 0.0, 0.0, - 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 : public rclcpp::Node -{ -private: - std::unique_ptr robot_; - create::RobotModel model_; - 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_; - std::string base_frame_; - std::string odom_frame_; - double latch_duration_; - double loop_hz_; - bool publish_tf_; - int baud_; - - 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); - - void update(); - void publishOdom(); - void publishJointState(); - void publishBatteryInfo(); - void publishButtonPresses() const; - void publishOmniChar(); - void publishMode(); - void publishBumperInfo(); - void publishWheeldrop(); - -protected: - 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_; - - 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(const std::string & name); - ~CreateDriver(); -}; diff --git a/ca_driver2/package.xml b/ca_driver2/package.xml deleted file mode 100644 index 7318c0211..000000000 --- a/ca_driver2/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - ca_driver2 - 2.0.0 - ROS 2 driver for iRobot's Create and Roomba platforms, based on libcreate - - Yutaka Kondo - - Apache License 2.0 - - http://wiki.ros.org/ca_driver - - Yutaka Kondo - - ament_cmake - - ca_msgs - diagnostic_msgs - - geometry_msgs - libcreate - nav_msgs - rclcpp - sensor_msgs - std_msgs - tf2 - tf2_ros - - ca_description - - - ament_cmake - - diff --git a/ca_driver2/src/create_driver.cpp b/ca_driver2/src/create_driver.cpp deleted file mode 100644 index 86f335334..000000000 --- a/ca_driver2/src/create_driver.cpp +++ /dev/null @@ -1,490 +0,0 @@ -#include -#include -#include -#include "create_driver/create_driver.h" - -CreateDriver::CreateDriver(const std::string & name) - : Node(name), - model_(create::RobotModel::CREATE_2), - ros_clock_(RCL_ROS_TIME) -{ - using namespace std::chrono_literals; - - std::string robot_model_name; - 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") - { - model_ = create::RobotModel::ROOMBA_400; - } - else if (robot_model_name == "CREATE_1") - { - model_ = create::RobotModel::CREATE_1; - } - else if (robot_model_name == "CREATE_2") - { - model_ = create::RobotModel::CREATE_2; - } - else - { - RCLCPP_FATAL(get_logger(), "[CREATE] Robot model \"%s\" is not known.", - robot_model_name.c_str()); - rclcpp::shutdown(); - return; - } - - RCLCPP_INFO(get_logger(), "[CREATE] \"%s\" selected", - robot_model_name.c_str()); - - get_parameter_or("baud", baud_, model_.getBaud()); - - robot_ = std::make_unique(model_); - - if (!robot_->connect(dev_, baud_)) - { - RCLCPP_FATAL(get_logger(), - "[CREATE] Failed to establish serial connection with Create."); - rclcpp::shutdown(); - } - - RCLCPP_INFO(get_logger(), "[CREATE] Connection established."); - - // Start in full control mode - robot_->setMode(create::MODE_FULL); - - // Show robot's battery level - 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_; - bumper_msg_.header.frame_id = base_frame_; - charging_state_msg_.header.frame_id = base_frame_; - tf_odom_.header.frame_id = odom_frame_; - tf_odom_.child_frame_id = base_frame_; - odom_msg_.header.frame_id = odom_frame_; - odom_msg_.child_frame_id = base_frame_; - joint_state_msg_.name.resize(2); - joint_state_msg_.position.resize(2); - joint_state_msg_.velocity.resize(2); - joint_state_msg_.effort.resize(2); - joint_state_msg_.name[0] = "left_wheel_joint"; - joint_state_msg_.name[1] = "right_wheel_joint"; - - // Populate intial covariances - for (int i = 0; i < 36; i++) - { - odom_msg_.pose.covariance[i] = COVARIANCE[i]; - odom_msg_.twist.covariance[i] = COVARIANCE[i]; - } - - // Setup subscribers - 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_ = 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() -{ - RCLCPP_INFO(get_logger(), "[CREATE] Destruct sequence initiated."); - robot_->disconnect(); -} - -void CreateDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) -{ - robot_->drive(msg->linear.x, msg->angular.z); - last_cmd_vel_time_ = ros_clock_.now(); -} - -void CreateDriver::debrisLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) -{ - robot_->enableDebrisLED(msg->data); -} - -void CreateDriver::spotLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) -{ - robot_->enableSpotLED(msg->data); -} - -void CreateDriver::dockLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) -{ - robot_->enableDockLED(msg->data); -} - -void CreateDriver::checkLEDCallback(const std_msgs::msg::Bool::SharedPtr msg) -{ - robot_->enableCheckRobotLED(msg->data); -} - -void CreateDriver::powerLEDCallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) -{ - if (msg->data.empty()) - { - RCLCPP_ERROR(get_logger(), "[CREATE] No values provided to set power LED"); - } - else - { - if (msg->data.size() < 2) - { - robot_->setPowerLED(msg->data[0]); - } - else - { - robot_->setPowerLED(msg->data[0], msg->data[1]); - } - } -} - -void CreateDriver::setASCIICallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) -{ - bool result = false; - if (msg->data.empty()) - { - RCLCPP_ERROR(get_logger(), "[CREATE] No ASCII digits provided"); - } - else if (msg->data.size() < 2) - { - result = robot_->setDigitsASCII(msg->data[0], ' ', ' ', ' '); - } - else if (msg->data.size() < 3) - { - result = robot_->setDigitsASCII(msg->data[0], msg->data[1], ' ', ' '); - } - else if (msg->data.size() < 4) - { - result = robot_->setDigitsASCII(msg->data[0], msg->data[1], msg->data[2], ' '); - } - else - { - result = robot_->setDigitsASCII(msg->data[0], msg->data[1], msg->data[2], msg->data[3]); - } - - if (!result) - { - RCLCPP_ERROR(get_logger(), "[CREATE] ASCII character out of range [32, 126]"); - } -} - -void CreateDriver::dockCallback(const std_msgs::msg::Empty::SharedPtr msg) -{ - robot_->setMode(create::MODE_PASSIVE); - - if (model_.getVersion() <= create::V_2) - usleep(1000000); // Create 1 requires a delay (1 sec) - - // Call docking behaviour - robot_->dock(); -} - -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::msg::DefineSong::SharedPtr msg) -{ - if (!robot_->defineSong(msg->song, msg->length, &(msg->notes.front()), &(msg->durations.front()))) - { - RCLCPP_ERROR(get_logger(), "[CREATE] Failed to define song %d of length %d", - msg->song, msg->length); - } -} - -void CreateDriver::playSongCallback(const ca_msgs::msg::PlaySong::SharedPtr msg) -{ - if (!robot_->playSong(msg->song)) - { - RCLCPP_ERROR(get_logger(), "[CREATE] Failed to play song %d", msg->song); - } -} - -void CreateDriver::update() -{ - publishOdom(); - publishJointState(); - publishBatteryInfo(); - publishButtonPresses(); - publishOmniChar(); - publishMode(); - publishBumperInfo(); - publishWheeldrop(); - - // If last velocity command was sent longer than latch duration, stop robot - if (ros_clock_.now() - last_cmd_vel_time_ >= rclcpp::Duration(latch_duration_)) - { - robot_->drive(0, 0); - } -} - -void CreateDriver::publishOdom() -{ - create::Pose pose = robot_->getPose(); - create::Vel vel = robot_->getVel(); - - // Populate position info - 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.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; - odom_msg_.twist.twist.linear.y = vel.y; - odom_msg_.twist.twist.angular.z = vel.yaw; - - // Update covariances - odom_msg_.pose.covariance[0] = static_cast(pose.covariance[0]); - odom_msg_.pose.covariance[1] = pose.covariance[1]; - odom_msg_.pose.covariance[5] = pose.covariance[2]; - odom_msg_.pose.covariance[6] = pose.covariance[3]; - odom_msg_.pose.covariance[7] = pose.covariance[4]; - odom_msg_.pose.covariance[11] = pose.covariance[5]; - odom_msg_.pose.covariance[30] = pose.covariance[6]; - odom_msg_.pose.covariance[31] = pose.covariance[7]; - odom_msg_.pose.covariance[35] = pose.covariance[8]; - odom_msg_.twist.covariance[0] = vel.covariance[0]; - odom_msg_.twist.covariance[1] = vel.covariance[1]; - odom_msg_.twist.covariance[5] = vel.covariance[2]; - odom_msg_.twist.covariance[6] = vel.covariance[3]; - odom_msg_.twist.covariance[7] = vel.covariance[4]; - odom_msg_.twist.covariance[11] = vel.covariance[5]; - odom_msg_.twist.covariance[30] = vel.covariance[6]; - odom_msg_.twist.covariance[31] = vel.covariance[7]; - odom_msg_.twist.covariance[35] = vel.covariance[8]; - - if (publish_tf_) - { - 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.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_); -} - -void CreateDriver::publishJointState() -{ - // Publish joint states - double wheelRadius = model_.getWheelDiameter() / 2.0; - - 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_); -} - -void CreateDriver::publishBatteryInfo() -{ - float32_msg_.data = robot_->getVoltage(); - voltage_pub_->publish(float32_msg_); - float32_msg_.data = robot_->getCurrent(); - current_pub_->publish(float32_msg_); - float32_msg_.data = robot_->getBatteryCharge(); - charge_pub_->publish(float32_msg_); - float32_msg_.data = robot_->getBatteryCapacity(); - capacity_pub_->publish(float32_msg_); - int16_msg_.data = robot_->getTemperature(); - temperature_pub_->publish(int16_msg_); - float32_msg_.data = robot_->getBatteryCharge() / robot_->getBatteryCapacity(); - charge_ratio_pub_->publish(float32_msg_); - - const create::ChargingState charging_state = robot_->getChargingState(); - charging_state_msg_.header.stamp = ros_clock_.now(); - switch (charging_state) - { - case create::CHARGE_NONE: - charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_NONE; - break; - case create::CHARGE_RECONDITION: - charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_RECONDITION; - break; - - case create::CHARGE_FULL: - charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_FULL; - break; - - case create::CHARGE_TRICKLE: - charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_TRICKLE; - break; - - case create::CHARGE_WAITING: - charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_WAITING; - break; - - case create::CHARGE_FAULT: - charging_state_msg_.state = ca_msgs::msg::ChargingState::CHARGE_FAULT; - break; - } - charging_state_pub_->publish(charging_state_msg_); -} - -void CreateDriver::publishButtonPresses() const -{ - if (robot_->isCleanButtonPressed()) - { - clean_btn_pub_->publish(empty_msg_); - } - if (robot_->isDayButtonPressed()) - { - day_btn_pub_->publish(empty_msg_); - } - if (robot_->isHourButtonPressed()) - { - hour_btn_pub_->publish(empty_msg_); - } - if (robot_->isMinButtonPressed()) - { - min_btn_pub_->publish(empty_msg_); - } - if (robot_->isDockButtonPressed()) - { - dock_btn_pub_->publish(empty_msg_); - } - if (robot_->isSpotButtonPressed()) - { - spot_btn_pub_->publish(empty_msg_); - } -} - -void CreateDriver::publishOmniChar() -{ - uint8_t ir_char = robot_->getIROmni(); - uint16_msg_.data = ir_char; - 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_clock_.now(); - switch (mode) - { - case create::MODE_OFF: - mode_msg_.mode = mode_msg_.MODE_OFF; - break; - case create::MODE_PASSIVE: - mode_msg_.mode = mode_msg_.MODE_PASSIVE; - break; - case create::MODE_SAFE: - mode_msg_.mode = mode_msg_.MODE_SAFE; - break; - case create::MODE_FULL: - mode_msg_.mode = mode_msg_.MODE_FULL; - break; - default: - RCLCPP_ERROR(get_logger(), "[CREATE] Unknown mode detected"); - break; - } - mode_pub_->publish(mode_msg_); -} - -void CreateDriver::publishBumperInfo() -{ - bumper_msg_.header.stamp = ros_clock_.now(); - bumper_msg_.is_left_pressed = robot_->isLeftBumper(); - bumper_msg_.is_right_pressed = robot_->isRightBumper(); - - if (model_.getVersion() >= create::V_3) - { - bumper_msg_.is_light_left = robot_->isLightBumperLeft(); - bumper_msg_.is_light_front_left = robot_->isLightBumperFrontLeft(); - bumper_msg_.is_light_center_left = robot_->isLightBumperCenterLeft(); - bumper_msg_.is_light_right = robot_->isLightBumperRight(); - bumper_msg_.is_light_front_right = robot_->isLightBumperFrontRight(); - bumper_msg_.is_light_center_right = robot_->isLightBumperCenterRight(); - - bumper_msg_.light_signal_left = robot_->getLightSignalLeft(); - bumper_msg_.light_signal_front_left = robot_->getLightSignalFrontLeft(); - bumper_msg_.light_signal_center_left = robot_->getLightSignalCenterLeft(); - bumper_msg_.light_signal_right = robot_->getLightSignalRight(); - bumper_msg_.light_signal_front_right = robot_->getLightSignalFrontRight(); - bumper_msg_.light_signal_center_right = robot_->getLightSignalCenterRight(); - } - - bumper_pub_->publish(bumper_msg_); -} - -void CreateDriver::publishWheeldrop() -{ - if (robot_->isWheeldrop()) - wheeldrop_pub_->publish(empty_msg_); -} - -int main(int argc, char** argv) -{ - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - rclcpp::init(argc, argv); - - auto node = std::make_shared("ca_driver"); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} From 41376640d853e949a373c9b1d489efe64207a1d2 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 12 Mar 2019 08:58:27 +0900 Subject: [PATCH 23/27] import ca_driver2 code to ca_driver --- ca_driver/AMENT_IGNORE | 0 ca_driver/CMakeLists.txt | 106 ++-- .../include/create_driver/create_driver.h | 204 +++----- ca_driver/package.xml | 24 +- ca_driver/src/create_driver.cpp | 473 ++++++------------ 5 files changed, 292 insertions(+), 515 deletions(-) delete mode 100644 ca_driver/AMENT_IGNORE diff --git a/ca_driver/AMENT_IGNORE b/ca_driver/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 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..3f4321398 100644 --- a/ca_driver/package.xml +++ b/ca_driver/package.xml @@ -1,34 +1,34 @@ - + 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 + Apache License 2.0 http://wiki.ros.org/ca_driver - Jacob Perron + Yutaka Kondo - 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; } From 2618607045248909d55c09b7dca54f64b6bd50f8 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 12 Mar 2019 09:02:34 +0900 Subject: [PATCH 24/27] update package.xml --- ca_driver/package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ca_driver/package.xml b/ca_driver/package.xml index 3f4321398..953e220d7 100644 --- a/ca_driver/package.xml +++ b/ca_driver/package.xml @@ -4,13 +4,14 @@ 2.0.0 ROS 2 driver for iRobot's Create and Roomba platforms, based on libcreate + Jacob Perron Yutaka Kondo - Apache License 2.0 + BSD http://wiki.ros.org/ca_driver - Yutaka Kondo + Jacob Perron ament_cmake From 130c75814dcefb9576be79fbdc0641a414048768 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 12 Mar 2019 09:07:22 +0900 Subject: [PATCH 25/27] fix readme --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index b7a46ab6f..f156d5ac7 100644 --- a/README.md +++ b/README.md @@ -119,17 +119,17 @@ _* Not verified. Anyone who is able to verify that this driver works or not is e For Create 2 (Roomba 600/700 series): ``` bash -$ ros2 launch ca_driver2 create_2.launch.py +$ ros2 launch ca_driver create_2.launch.py ``` For Create 1 (Roomba 500 series): ``` bash -$ ros2 launch ca_driver2 create_1.launch.py +$ ros2 launch ca_driver create_1.launch.py ``` For Roomba 400 series: ``` bash -$ ros2 launch ca_driver2 roomba_400.launch.ph +$ ros2 launch ca_driver roomba_400.launch.py ``` #### Launch file arguments @@ -140,7 +140,7 @@ $ ros2 launch ca_driver2 roomba_400.launch.ph For example, if you would like to disable the robot description and provide a custom configuration file: ```bash -$ ros2 launch ca_driver2 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 From 1f560bbb3b701d66434d5a371b2d22711b4e3592 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Fri, 15 Mar 2019 15:49:32 +0900 Subject: [PATCH 26/27] Delete .gitignore --- .gitignore | 50 -------------------------------------------------- 1 file changed, 50 deletions(-) delete mode 100644 .gitignore diff --git a/.gitignore b/.gitignore deleted file mode 100644 index 9a604a7a1..000000000 --- a/.gitignore +++ /dev/null @@ -1,50 +0,0 @@ - -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib -build - -# Executables -*.exe -*.out -*.app - -# org-mode -*.html -*.aux -*.bbl -*.blg -*.dvi -*.log -*.out -*.pdf -*.tex -*.toc -ltximg - -.idea -.DS_Store From ee91d7684ea0b3a758859c4db0799a8bd90bc844 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Fri, 15 Mar 2019 15:56:10 +0900 Subject: [PATCH 27/27] comment-in depends --- create_autonomy/package.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/create_autonomy/package.xml b/create_autonomy/package.xml index b1565b045..dd8984df5 100644 --- a/create_autonomy/package.xml +++ b/create_autonomy/package.xml @@ -16,10 +16,10 @@ ament_cmake - - + ca_description + ca_driver ca_msgs - + ca_tools ament_cmake