├── create3_teleop ├── resource │ └── create3_teleop ├── setup.cfg ├── CHANGELOG.rst ├── package.xml ├── setup.py ├── README.md └── launch │ └── teleop_joystick_launch.py ├── create3_examples_py ├── resource │ └── create3_examples_py ├── create3_examples_py │ ├── __init__.py │ └── dance │ │ ├── __init__.py │ │ ├── create3_dance.py │ │ └── dance_choreograph.py ├── setup.cfg ├── README.md ├── setup.py ├── test │ ├── test_copyright.py │ ├── test_pep257.py │ └── test_flake8.py ├── package.xml └── CHANGELOG.rst ├── create3_lidar_slam ├── TF_Tree.pdf ├── config │ ├── rplidar_node.yaml │ └── mapper_params_online_async.yaml ├── CMakeLists.txt ├── package.xml ├── CHANGELOG.rst ├── launch │ ├── rviz_launch.py │ ├── sensors_launch.py │ └── slam_toolbox_launch.py ├── README.md └── rviz │ └── create3_lidar_slam.rviz ├── create3_coverage ├── include │ └── create3_coverage │ │ ├── state.hpp │ │ ├── behaviors │ │ ├── behavior.hpp │ │ ├── dock-behavior.hpp │ │ ├── undock-behavior.hpp │ │ ├── drive-straight-behavior.hpp │ │ ├── spiral-behavior.hpp │ │ ├── reflex-behavior.hpp │ │ └── rotate-behavior.hpp │ │ ├── coverage_state_machine.hpp │ │ └── create3_coverage_node.hpp ├── src │ ├── main.cpp │ ├── behaviors │ │ ├── utils.hpp │ │ ├── utils.cpp │ │ ├── spiral-behavior.cpp │ │ ├── drive-straight-behavior.cpp │ │ ├── reflex-behavior.cpp │ │ ├── dock-behavior.cpp │ │ ├── undock-behavior.cpp │ │ └── rotate-behavior.cpp │ ├── coverage_state_machine.cpp │ └── create3_coverage_node.cpp ├── package.xml ├── README.md ├── CMakeLists.txt └── CHANGELOG.rst ├── create3_examples_msgs ├── README.md ├── action │ └── Coverage.action ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── create3_republisher ├── dds-config │ ├── fastdds-passive-unicast.xml │ ├── fastdds-active-unicast.xml │ └── fastdds-robot-passive-unicast.xml ├── package.xml ├── CMakeLists.txt ├── CHANGELOG.rst ├── bringup │ ├── create3_republisher_launch.py │ └── params.yaml ├── README.md └── republisher.cpp ├── README.md └── LICENSE.txt /create3_teleop/resource/create3_teleop: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /create3_examples_py/resource/create3_examples_py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /create3_examples_py/create3_examples_py/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /create3_examples_py/create3_examples_py/dance/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /create3_lidar_slam/TF_Tree.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iRobotEducation/create3_examples/HEAD/create3_lidar_slam/TF_Tree.pdf -------------------------------------------------------------------------------- /create3_teleop/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/create3_teleop 3 | [install] 4 | install_scripts=$base/lib/create3_teleop 5 | -------------------------------------------------------------------------------- /create3_examples_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/create3_examples_py 3 | [install] 4 | install_scripts=$base/lib/create3_examples_py 5 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | namespace create3_coverage { 6 | 7 | enum class State { 8 | RUNNING, 9 | FAILURE, 10 | SUCCESS 11 | }; 12 | 13 | } // namespace create3_coverage 14 | -------------------------------------------------------------------------------- /create3_examples_msgs/README.md: -------------------------------------------------------------------------------- 1 | # iRobot® Create® 3 Examples messages 2 | 3 | ROS 2 action, message, and service definitions used by the iRobot® Create® 3 Examples packages. 4 | 5 | ## Actions (.action) 6 | * [Coverage](action/Coverage.action): Command the robot perform a non-systematic coverage behavior in the environment. 7 | -------------------------------------------------------------------------------- /create3_coverage/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include 4 | 5 | #include "create3_coverage/create3_coverage_node.hpp" 6 | #include "rclcpp/rclcpp.hpp" 7 | 8 | int main(int argc, char ** argv) 9 | { 10 | rclcpp::init(argc, argv); 11 | auto node = std::make_shared(); 12 | rclcpp::spin(node); 13 | rclcpp::shutdown(); 14 | 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /create3_examples_py/README.md: -------------------------------------------------------------------------------- 1 | # iRobot® Create® 3 Examples Python 2 | 3 | This package contains Python script that demonstrate how to interact with the Create® 3 robot. 4 | 5 | ## iRobot® Create® 3 Dance 6 | 7 | **NOTES:** 8 | - You must undock the robot before starting this example 9 | 10 | This executable will show how to drive the robot around while changing its LED colors. 11 | Use it to create patterns and follow the tempo of your favourite songs! 12 | To run it: 13 | 14 | ```sh 15 | ros2 run create3_examples_py create3_dance 16 | ``` 17 | -------------------------------------------------------------------------------- /create3_lidar_slam/config/rplidar_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | rplidar_node: 3 | ros__parameters: 4 | angle_compensate: true 5 | channel_type: serial 6 | flip_x_axis: false 7 | frame_id: laser_frame 8 | inverted: false 9 | scan_mode: '' 10 | serial_baudrate: 115200 11 | serial_port: /dev/ttyUSB0 12 | topic_name: scan 13 | use_sim_time: false 14 | 15 | # True turns off lidar motor when scan topic is not subscribed to. 16 | # False runs lidar motor constantly. 17 | auto_standby: true -------------------------------------------------------------------------------- /create3_lidar_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.5) 3 | project(create3_lidar_slam) 4 | 5 | # Default to C++14 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 14) 8 | endif() 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC) 12 | endif() 13 | 14 | find_package(ament_cmake REQUIRED) 15 | 16 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 17 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 18 | install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /create3_teleop/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package create3_teleop 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2024-09-28) 6 | ------------------ 7 | 8 | 0.0.5 (2024-06-27) 9 | ------------------ 10 | 11 | 0.0.4 (2024-06-27) 12 | ------------------ 13 | 14 | 0.0.3 (2024-06-27) 15 | ------------------ 16 | 17 | 0.0.2 (2024-06-05) 18 | ------------------ 19 | * typo 20 | * Merge pull request `#18 `_ from iRobotEducation/asoragna/teleop 21 | add teleop launch scripts 22 | * add teleop launch scripts 23 | * Contributors: Alberto Soragna, Steven Shamlian 24 | -------------------------------------------------------------------------------- /create3_examples_msgs/action/Coverage.action: -------------------------------------------------------------------------------- 1 | # Request 2 | 3 | # The robot will explore the environment for `explore_duration` time. 4 | # After that, it will try to dock as soon as it sees the docking station. 5 | # The `max_runtime` duration defines when the behavior should end regardless of whether 6 | # the dock has been found or not. 7 | 8 | builtin_interfaces/Duration explore_duration 9 | builtin_interfaces/Duration max_runtime 10 | --- 11 | # Result 12 | bool success 13 | bool is_docked 14 | builtin_interfaces/Duration duration 15 | --- 16 | # Feedback 17 | 18 | int32 DOCK = 0 19 | int32 DRIVE_STRAIGHT = 1 20 | int32 ROTATE = 2 21 | int32 SPIRAL = 3 22 | int32 UNDOCK = 4 23 | 24 | int32 current_behavior 25 | -------------------------------------------------------------------------------- /create3_republisher/dds-config/fastdds-passive-unicast.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 |
127.0.0.1
13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /create3_examples_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(create3_examples_msgs) 4 | 5 | # Default to C++14 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 14) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | 16 | find_package(action_msgs REQUIRED) 17 | find_package(builtin_interfaces REQUIRED) 18 | 19 | set(action_files 20 | "action/Coverage.action" 21 | ) 22 | 23 | rosidl_generate_interfaces(${PROJECT_NAME} 24 | ${action_files} 25 | DEPENDENCIES action_msgs builtin_interfaces 26 | ADD_LINTER_TESTS 27 | ) 28 | 29 | ament_export_dependencies(rosidl_default_runtime) 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /create3_republisher/dds-config/fastdds-active-unicast.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 |
127.0.0.1
13 |
192.168.186.2
14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /create3_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | create3_teleop 5 | 1.0.0 6 | Example launch files for teleoperating the iRobot(R) Create(R) 3 Educational Robot. 7 | Alberto Soragna 8 | BSD 9 | 10 | joy 11 | launch_ros 12 | teleop_twist_keyboard 13 | teleop_twist_joy 14 | 15 | ament_cmake_flake8 16 | python3-pytest 17 | 18 | 19 | ament_python 20 | 21 | 22 | -------------------------------------------------------------------------------- /create3_examples_py/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from setuptools import find_packages 3 | 4 | package_name = 'create3_examples_py' 5 | 6 | setup( 7 | name=package_name, 8 | version='0.0.5', 9 | packages=find_packages(exclude=['test']), 10 | data_files=[ 11 | ('share/ament_index/resource_index/packages', 12 | ['resource/' + package_name]), 13 | ('share/' + package_name, ['package.xml']), 14 | ], 15 | install_requires=['setuptools'], 16 | zip_safe=True, 17 | maintainer='jkearns', 18 | maintainer_email='jkearns@irobot.com', 19 | description='Example ROS 2 Python code to use iRobot® Create® 3', 20 | license='BSD-3', 21 | tests_require=['pytest'], 22 | entry_points={ 23 | 'console_scripts': [ 24 | 'create3_dance = create3_examples_py.dance.create3_dance:main' 25 | ], 26 | }, 27 | ) 28 | -------------------------------------------------------------------------------- /create3_examples_py/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /create3_teleop/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from glob import glob 3 | from setuptools import setup 4 | 5 | package_name = 'create3_teleop' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.5', 10 | packages=[], 11 | data_files=[ 12 | (os.path.join('share','ament_index', 'resource_index', 'packages'), 13 | [os.path.join('resource', package_name)]), 14 | (os.path.join('share', package_name), 15 | ['package.xml']), 16 | (os.path.join('share', package_name, "launch"), 17 | glob(os.path.join('launch', '*launch.py'))), 18 | ], 19 | install_requires=['setuptools'], 20 | zip_safe=True, 21 | maintainer='Alberto Soragna', 22 | maintainer_email='asoragna@irobot.com', 23 | description='Example launch files for teleoperating the iRobot(R) Create(R) 3 Educational Robot.', 24 | license='BSD-3', 25 | tests_require=['pytest'], 26 | ) 27 | -------------------------------------------------------------------------------- /create3_examples_py/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /create3_examples_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | create3_examples_py 5 | 1.0.0 6 | Python examples for interacting with the iRobot(R) Create(R) 3 Educational Robot 7 | jkearns 8 | BSD 9 | Justin Kearns 10 | 11 | rclpy 12 | geometry_msgs 13 | irobot_create_msgs 14 | 15 | ament_copyright 16 | ament_flake8 17 | ament_pep257 18 | python3-pytest 19 | 20 | 21 | ament_python 22 | 23 | 24 | -------------------------------------------------------------------------------- /create3_lidar_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | create3_lidar_slam 5 | 1.0.0 6 | Example for using an RPLIDAR A1 with a Create 3 7 | Alberto Soragna 8 | Justin Kearns 9 | Steven Shamlian 10 | 11 | BSD 12 | 13 | irobot_create_msgs 14 | 15 | ament_cmake 16 | launch_ros 17 | 18 | launch_ros 19 | rplidar_ros 20 | slam_toolbox 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "geometry_msgs/msg/point.hpp" 6 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 7 | #include "irobot_create_msgs/msg/ir_opcode.hpp" 8 | 9 | namespace create3_coverage { 10 | 11 | /** 12 | * @brief Computes Euclidean distance between two points 13 | */ 14 | double get_distance(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); 15 | 16 | /** 17 | * @brief Inspects received opcodes to determine if robot is driving 18 | * towards the dock 19 | */ 20 | bool is_driving_towards_dock(const std::vector& opcodes); 21 | 22 | /** 23 | * @brief Inspects hazard detections to determine if any hazard is active. 24 | * This excludes BACKUP_LIMIT hazards. 25 | */ 26 | bool is_front_hazard_active(const irobot_create_msgs::msg::HazardDetectionVector& hazards); 27 | 28 | } // namespace create3_coverage 29 | -------------------------------------------------------------------------------- /create3_republisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | create3_republisher 5 | 1.0.0 6 | C++ action server exposing a non-systematic coverage behavior 7 | Alberto Soragna 8 | BSD 9 | Alberto Soragna 10 | 11 | ament_cmake 12 | 13 | geometry_msgs 14 | sensor_msgs 15 | nav_msgs 16 | irobot_create_msgs 17 | rclcpp 18 | rclcpp_action 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | -------------------------------------------------------------------------------- /create3_examples_py/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/behaviors/behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | #include "create3_coverage/state.hpp" 8 | #include "geometry_msgs/msg/pose.hpp" 9 | #include "irobot_create_msgs/msg/dock_status.hpp" 10 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 11 | #include "irobot_create_msgs/msg/ir_opcode.hpp" 12 | 13 | namespace create3_coverage { 14 | 15 | class Behavior 16 | { 17 | public: 18 | struct Data { 19 | geometry_msgs::msg::Pose pose; 20 | irobot_create_msgs::msg::HazardDetectionVector hazards; 21 | irobot_create_msgs::msg::DockStatus dock; 22 | std::vector opcodes; 23 | }; 24 | 25 | Behavior() = default; 26 | 27 | virtual ~Behavior() = default; 28 | 29 | virtual State execute(const Data & data) = 0; 30 | 31 | virtual int32_t get_id() const = 0; 32 | 33 | virtual void cleanup() {} 34 | }; 35 | 36 | } // namespace create3_coverage 37 | -------------------------------------------------------------------------------- /create3_teleop/README.md: -------------------------------------------------------------------------------- 1 | # Create3 Teleoperation 2 | 3 | This package contains scripts and instructions for teleoperating the the Create® 3 robot using keyboard or joystick. 4 | 5 | ### Keyboard Teleoperation 6 | 7 | ```sh 8 | ros2 run teleop_twist_keyboard teleop_twist_keyboard 9 | ``` 10 | 11 | ### Joystick Teleoperation 12 | 13 | ```sh 14 | ros2 launch create3_teleop teleop_joystick_launch.py 15 | ``` 16 | 17 | This will default to an xbox 360 controller, but can be easily overriden using the `joy_config` launchfile argument for any of the supported platforms. As of time of writing, these are: 18 | - Logitech Attack3 (`atk3`) 19 | - Logitech Extreme 3D Pro (`xd3`) 20 | - PS3 (`ps3` or `ps3-holonomic`) 21 | - Xbox 360 (`xbox`) 22 | 23 | Example for a PS3 controller: 24 | 25 | ```sh 26 | ros2 launch create3_teleop teleop_joystick_launch.py joy_config:=ps3 27 | ``` 28 | 29 | Also, it's possible to select the specific device to use with the `joy_dev` argument. It can be used as follows: 30 | 31 | ```sh 32 | ros2 launch create3_teleop teleop_joystick_launch.py joy_dev:=/dev/input/js1 33 | ``` 34 | -------------------------------------------------------------------------------- /create3_republisher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(create3_republisher) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # find dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(irobot_create_msgs REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rclcpp_action REQUIRED) 18 | 19 | add_executable(create3_republisher) 20 | target_sources(create3_republisher PRIVATE republisher.cpp) 21 | target_link_libraries( 22 | create3_republisher 23 | PRIVATE 24 | rclcpp::rclcpp 25 | rclcpp_action::rclcpp_action 26 | ${irobot_create_msgs_TARGETS} 27 | ) 28 | 29 | install(TARGETS create3_republisher 30 | RUNTIME DESTINATION lib/${PROJECT_NAME} 31 | ) 32 | 33 | install(DIRECTORY bringup DESTINATION share/${PROJECT_NAME}) 34 | install(DIRECTORY dds-config DESTINATION share/${PROJECT_NAME}) 35 | 36 | ament_package() 37 | -------------------------------------------------------------------------------- /create3_coverage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | create3_coverage 5 | 1.0.0 6 | C++ action server exposing a non-systematic coverage behavior 7 | Alberto Soragna 8 | Justin Kearns 9 | BSD 10 | Alberto Soragna 11 | 12 | ament_cmake 13 | 14 | create3_examples_msgs 15 | geometry_msgs 16 | irobot_create_msgs 17 | nav_msgs 18 | rclcpp 19 | rclcpp_action 20 | tf2_geometry_msgs 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /create3_lidar_slam/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package create3_lidar_slam 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2024-09-28) 6 | ------------------ 7 | 8 | 0.0.5 (2024-06-27) 9 | ------------------ 10 | 11 | 0.0.4 (2024-06-27) 12 | ------------------ 13 | 14 | 0.0.3 (2024-06-27) 15 | ------------------ 16 | 17 | 0.0.2 (2024-06-05) 18 | ------------------ 19 | * Merge pull request `#51 `_ from iRobotEducation/shamlian/fix-50 20 | Remove name for rplidar_node in LIDAR SLAM example. 21 | * Remove name for rplidar_node 22 | The node is named by the configuration; it should not be renamed here. 23 | * Merge pull request `#48 `_ from brianabouchard/humble-mapping-namespace 24 | Add namespace functionality to Humble mapping example 25 | * Remove rviz2 install instructions 26 | - confirmed rviz2 installs when ros-humble-desktop is installed and create docs site provides desktop installation command for humble. 27 | * Changed package name to create3_lidar_slam 28 | * Contributors: Briana Bouchard, Steven Shamlian 29 | -------------------------------------------------------------------------------- /create3_examples_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package create3_examples_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2024-09-28) 6 | ------------------ 7 | 8 | 0.0.5 (2024-06-27) 9 | ------------------ 10 | 11 | 0.0.4 (2024-06-27) 12 | ------------------ 13 | * add missing buildtool dependency in create3_examples_msgs 14 | * Contributors: Alberto Soragna 15 | 16 | 0.0.3 (2024-06-27) 17 | ------------------ 18 | 19 | 0.0.2 (2024-06-05) 20 | ------------------ 21 | * Merge pull request `#17 `_ from iRobotEducation/asoragna/rename-duration 22 | rename coverage action max_duration into max_runtime 23 | * rename coverage action max_duration into max_runtime 24 | * Documentation updates 25 | Typos, trademark symbols, and grammar fixes. 26 | * add READMEs to the repo 27 | * update license to BSD-3 28 | * Merge pull request `#2 `_ from iRobotEducation/asoragna/create3-coverage 29 | Add create3 coverage example 30 | * add explore_time argument and cleanup of logs 31 | * Add create3 coverage example 32 | * Contributors: Alberto Soragna, Steven Shamlian 33 | -------------------------------------------------------------------------------- /create3_examples_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | create3_examples_msgs 5 | 1.0.0 6 | Package containing action, message and service definitions used by the iRobot(R) Create(R) 3 examples 7 | Alberto Soragna 8 | Justin Kearns 9 | BSD 10 | 11 | Alberto Soragna 12 | Justin Kearns 13 | 14 | ament_cmake 15 | rosidl_default_generators 16 | 17 | action_msgs 18 | builtin_interfaces 19 | 20 | action_msgs 21 | builtin_interfaces 22 | rosidl_default_runtime 23 | 24 | ament_lint_common 25 | 26 | rosidl_interface_packages 27 | 28 | 29 | ament_cmake 30 | 31 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/behaviors/dock-behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "create3_coverage/behaviors/behavior.hpp" 6 | #include "create3_examples_msgs/action/coverage.hpp" 7 | #include "irobot_create_msgs/action/dock.hpp" 8 | #include "rclcpp_action/rclcpp_action.hpp" 9 | 10 | namespace create3_coverage { 11 | 12 | class DockBehavior : public Behavior 13 | { 14 | public: 15 | using DockAction = irobot_create_msgs::action::Dock; 16 | using GoalHandleDock = rclcpp_action::ClientGoalHandle; 17 | 18 | DockBehavior( 19 | rclcpp_action::Client::SharedPtr dock_action_client, 20 | rclcpp::Logger logger); 21 | 22 | ~DockBehavior() = default; 23 | 24 | State execute(const Data & data) override; 25 | 26 | int32_t get_id() const override { return create3_examples_msgs::action::Coverage::Feedback::DOCK; } 27 | 28 | void cleanup() override; 29 | 30 | private: 31 | bool m_dock_action_sent {false}; 32 | GoalHandleDock::SharedPtr m_dock_goal_handle; 33 | bool m_dock_goal_handle_ready {false}; 34 | GoalHandleDock::WrappedResult m_dock_result; 35 | bool m_dock_result_ready {false}; 36 | 37 | rclcpp_action::Client::SharedPtr m_dock_action_client; 38 | rclcpp::Logger m_logger; 39 | }; 40 | 41 | } // namespace create3_coverage 42 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # create3_examples 2 | 3 | Example nodes to drive the iRobot® Create® 3 Educational Robot. 4 | 5 | ### Dependencies 6 | 7 | Make sure that ROS 2 Humble is already installed in your system. 8 | You can follow the [official instructions](https://docs.ros.org/en/jazzy/Installation.html). 9 | 10 | ### Build instructions 11 | 12 | First, source your ROS 2 workspaces with all the required dependencies. 13 | Then, you are ready to clone and build this repository. 14 | You should only have to do this once per install. 15 | 16 | ```sh 17 | mkdir -p create3_examples_ws/src 18 | cd create3_examples_ws/src 19 | git clone https://github.com/iRobotEducation/create3_examples.git --branch jazzy 20 | cd .. 21 | rosdep install --from-path src --ignore-src -yi 22 | colcon build 23 | ``` 24 | 25 | ### Initialization instructions 26 | 27 | You will have to do this in every new session in which you wish to use these examples: 28 | 29 | ```sh 30 | source ~/create3_examples_ws/install/local_setup.sh 31 | ``` 32 | 33 | ### Run the examples 34 | 35 | Refer to the individual examples README.md for instructions on how to run them. 36 | 37 | ### Potential pitfalls 38 | 39 | If you are unable to automatically install dependencies with rosdep (perhaps due to [this issue](https://github.com/ros-infrastructure/rosdep/issues/733)), please do be sure to manually install the dependencies for your particular example of interest, contained in its package.xml file. 40 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/behaviors/undock-behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "create3_coverage/behaviors/behavior.hpp" 6 | #include "create3_examples_msgs/action/coverage.hpp" 7 | #include "irobot_create_msgs/action/undock.hpp" 8 | #include "rclcpp_action/rclcpp_action.hpp" 9 | 10 | namespace create3_coverage { 11 | 12 | class UndockBehavior : public Behavior 13 | { 14 | public: 15 | using UndockAction = irobot_create_msgs::action::Undock; 16 | using GoalHandleUndock = rclcpp_action::ClientGoalHandle; 17 | 18 | UndockBehavior( 19 | rclcpp_action::Client::SharedPtr undock_action_client, 20 | rclcpp::Logger logger); 21 | 22 | ~UndockBehavior() = default; 23 | 24 | State execute(const Data & data) override; 25 | 26 | int32_t get_id() const override { return create3_examples_msgs::action::Coverage::Feedback::UNDOCK; } 27 | 28 | void cleanup() override; 29 | 30 | private: 31 | bool m_undock_action_sent {false}; 32 | GoalHandleUndock::SharedPtr m_undock_goal_handle; 33 | bool m_undock_goal_handle_ready {false}; 34 | GoalHandleUndock::WrappedResult m_undock_result; 35 | bool m_undock_result_ready {false}; 36 | 37 | rclcpp_action::Client::SharedPtr m_undock_action_client; 38 | rclcpp::Logger m_logger; 39 | }; 40 | 41 | } // namespace create3_coverage 42 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2021 iRobot Corporation. 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 2. 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. 7 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | 9 | 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. 10 | 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. 11 | -------------------------------------------------------------------------------- /create3_coverage/README.md: -------------------------------------------------------------------------------- 1 | # iRobot® Create® 3 Coverage 2 | 3 | This example creates a ROS 2 action server that runs a simple non-systematic coverage algorithm on your Create® 3. 4 | 5 | The purpose of this example is to show how to command your robot and react to its hazard information. 6 | 7 | ### How to use 8 | 9 | Build this and the `create3_examples_msgs` packages. 10 | Source the setup shell scripts. 11 | 12 | Start the coverage action server 13 | 14 | ```bash 15 | ros2 run create3_coverage create3_coverage 16 | ``` 17 | 18 | In a separate terminal command a coverage action 19 | 20 | ```bash 21 | ros2 action send_goal /coverage create3_examples_msgs/action/Coverage "{explore_duration:{sec: 500, nanosec: 0}, max_runtime:{sec: 1000,nanosec: 0}}" 22 | ``` 23 | 24 | ### Robot initial configuration 25 | 26 | **NOTES:** 27 | - Do not start the behavior with the robot undocked, but very close to the dock. The behavior may fail or it may cause the robot to run over its dock. 28 | It's safe to start with the robot still docked. 29 | - Do not start the behavior with the robot in contact with obstacles or cliffs. 30 | 31 | 32 | ### Troubleshooting 33 | 34 | ##### `Waiting for an action server to become available...` 35 | 36 | If users notice that they are unable to communicate with the coverage action server when using Fast-DDS RMW, this is due to the following bug https://github.com/ros2/rmw_fastrtps/issues/563. 37 | A simple fix consists in updating the `rwm_fastrtps_cpp` library to a version >= 5.0.1 38 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/behaviors/drive-straight-behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "create3_coverage/behaviors/behavior.hpp" 6 | #include "create3_examples_msgs/action/coverage.hpp" 7 | #include "geometry_msgs/msg/point.hpp" 8 | #include "geometry_msgs/msg/twist.hpp" 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 11 | 12 | namespace create3_coverage { 13 | 14 | class DriveStraightBehavior : public Behavior 15 | { 16 | public: 17 | using OpCodeMsg = irobot_create_msgs::msg::IrOpcode; 18 | using TwistMsg = geometry_msgs::msg::Twist; 19 | 20 | struct Config 21 | { 22 | double max_distance {5}; 23 | double min_distance {0.25}; 24 | double linear_vel {0.25}; 25 | }; 26 | 27 | DriveStraightBehavior( 28 | Config config, 29 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 30 | rclcpp::Logger logger, 31 | rclcpp::Clock::SharedPtr clock); 32 | 33 | ~DriveStraightBehavior() = default; 34 | 35 | State execute(const Data & data) override; 36 | 37 | int32_t get_id() const override { return create3_examples_msgs::action::Coverage::Feedback::DRIVE_STRAIGHT; } 38 | 39 | private: 40 | Config m_config; 41 | 42 | bool m_first_run; 43 | rclcpp::Time m_start_time; 44 | geometry_msgs::msg::Point m_initial_position; 45 | 46 | rclcpp::Publisher::SharedPtr m_cmd_vel_publisher; 47 | rclcpp::Logger m_logger; 48 | rclcpp::Clock::SharedPtr m_clock; 49 | }; 50 | 51 | } // namespace create3_coverage 52 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/behaviors/spiral-behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "create3_coverage/behaviors/behavior.hpp" 6 | #include "create3_examples_msgs/action/coverage.hpp" 7 | #include "geometry_msgs/msg/twist.hpp" 8 | #include "rclcpp/rclcpp.hpp" 9 | 10 | namespace create3_coverage { 11 | 12 | class SpiralBehavior : public Behavior 13 | { 14 | public: 15 | using OpCodeMsg = irobot_create_msgs::msg::IrOpcode; 16 | using TwistMsg = geometry_msgs::msg::Twist; 17 | 18 | struct Config 19 | { 20 | rclcpp::Duration spiral_duration {rclcpp::Duration(std::chrono::seconds(30))}; 21 | double linear_vel {0.4}; 22 | double initial_radius {0.25}; 23 | double radius_increment {0.25}; 24 | rclcpp::Duration radius_increment_interval {rclcpp::Duration(std::chrono::seconds(5))}; 25 | }; 26 | 27 | SpiralBehavior( 28 | Config config, 29 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 30 | rclcpp::Logger logger, 31 | rclcpp::Clock::SharedPtr clock); 32 | 33 | ~SpiralBehavior() = default; 34 | 35 | State execute(const Data & data) override; 36 | 37 | int32_t get_id() const override { return create3_examples_msgs::action::Coverage::Feedback::SPIRAL; } 38 | 39 | private: 40 | rclcpp::Time m_start_time; 41 | rclcpp::Time m_last_radius_update_time; 42 | double m_radius; 43 | 44 | Config m_config; 45 | 46 | rclcpp::Publisher::SharedPtr m_cmd_vel_publisher; 47 | rclcpp::Logger m_logger; 48 | rclcpp::Clock::SharedPtr m_clock; 49 | }; 50 | 51 | } // namespace create3_coverage 52 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/behaviors/reflex-behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "create3_coverage/behaviors/behavior.hpp" 6 | #include "create3_examples_msgs/action/coverage.hpp" 7 | #include "geometry_msgs/msg/point.hpp" 8 | #include "geometry_msgs/msg/twist.hpp" 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 11 | 12 | namespace create3_coverage { 13 | 14 | class ReflexBehavior : public Behavior 15 | { 16 | public: 17 | using TwistMsg = geometry_msgs::msg::Twist; 18 | 19 | struct Config 20 | { 21 | double backup_distance {0.05}; 22 | double linear_vel {0.1}; 23 | rclcpp::Duration clear_hazard_time {rclcpp::Duration(std::chrono::seconds(2))}; 24 | }; 25 | 26 | ReflexBehavior( 27 | Config config, 28 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 29 | rclcpp::Logger logger, 30 | rclcpp::Clock::SharedPtr clock); 31 | 32 | ~ReflexBehavior() = default; 33 | 34 | State execute(const Data & data) override; 35 | 36 | int32_t get_id() const override { return create3_examples_msgs::action::Coverage::Feedback::DRIVE_STRAIGHT; } 37 | 38 | private: 39 | bool backup_limit_reached(const irobot_create_msgs::msg::HazardDetectionVector& hazards); 40 | 41 | Config m_config; 42 | 43 | bool m_first_run; 44 | rclcpp::Time m_start_time; 45 | geometry_msgs::msg::Point m_initial_position; 46 | 47 | rclcpp::Publisher::SharedPtr m_cmd_vel_publisher; 48 | rclcpp::Logger m_logger; 49 | rclcpp::Clock::SharedPtr m_clock; 50 | }; 51 | 52 | } // namespace create3_coverage 53 | -------------------------------------------------------------------------------- /create3_republisher/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package create3_republisher 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2024-09-28) 6 | ------------------ 7 | * Merge pull request `#58 `_ from civerachb-cpr/patch-1 8 | Adds the new `/cmd_vel_stamped` topic to the robot subscriptions. 9 | * Merge pull request `#56 `_ from hilary-luo/hluo/rel-namespace-fix 10 | Relative Namespace Fix 11 | * Remove manual handling of namespace not being fully qualified 12 | * Contributors: Alberto Soragna, Chris Iverach-Brereton, Hilary Luo 13 | 14 | 0.0.5 (2024-06-27) 15 | ------------------ 16 | 17 | 0.0.4 (2024-06-27) 18 | ------------------ 19 | 20 | 0.0.3 (2024-06-27) 21 | ------------------ 22 | * add debug statistics 23 | * Contributors: Alberto Soragna 24 | 25 | 0.0.2 (2024-06-05) 26 | ------------------ 27 | * update republisher 28 | * Merge pull request `#54 `_ from hilary-luo/hluo/turtlebot4-republisher 29 | Turtlebot4 Republisher 30 | * Enable topics that Turtlebot4 needs by default 31 | * Accept a relative robot namespace 32 | * Merge pull request `#53 `_ from iRobotEducation/asoragna/create3-republisher 33 | add create 3 republisher package 34 | * add missing reset-pose service and cleanup 35 | * fix bug with undock action and update docs 36 | * update docks 37 | * add logs and improve documentation 38 | * remove hardcoded periods and timeouts 39 | * allow use of / namespace for republisher 40 | * add create 3 republisher package 41 | * Contributors: Alberto Soragna, Hilary Luo 42 | -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include 4 | 5 | #include "utils.hpp" 6 | 7 | #include 8 | namespace create3_coverage { 9 | 10 | double get_distance(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) 11 | { 12 | double dx = p1.x - p2.x; 13 | double dy = p1.y - p2.y; 14 | return sqrt(dx*dx + dy*dy); 15 | } 16 | 17 | bool is_driving_towards_dock(const std::vector& opcodes) 18 | { 19 | using irobot_create_msgs::msg::IrOpcode; 20 | 21 | // We consider the robot to be driving towards the dock if both IR receivers are detecting it. 22 | // The directional sensor will tell us that we are pointing towards it. 23 | // The omni sensors will tell us that we are close to it. 24 | 25 | bool dir_dock_detection = false; 26 | bool omni_dock_detection = false; 27 | 28 | for (const IrOpcode& msg : opcodes) { 29 | if (msg.sensor == IrOpcode::SENSOR_DIRECTIONAL_FRONT && msg.opcode != IrOpcode::CODE_IR_VIRTUAL_WALL) { 30 | dir_dock_detection = true; 31 | continue; 32 | } 33 | if (msg.sensor == IrOpcode::SENSOR_OMNI && msg.opcode != IrOpcode::CODE_IR_VIRTUAL_WALL) { 34 | omni_dock_detection = true; 35 | continue; 36 | } 37 | } 38 | 39 | return dir_dock_detection && omni_dock_detection; 40 | } 41 | 42 | bool is_front_hazard_active(const irobot_create_msgs::msg::HazardDetectionVector& hazards) 43 | { 44 | using irobot_create_msgs::msg::HazardDetection; 45 | 46 | for (const HazardDetection& detection : hazards.detections) { 47 | if (detection.type != HazardDetection::BACKUP_LIMIT) { 48 | return true; 49 | } 50 | } 51 | 52 | return false; 53 | } 54 | 55 | } // namespace create3_coverage 56 | -------------------------------------------------------------------------------- /create3_examples_py/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package create3_examples_py 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2024-09-28) 6 | ------------------ 7 | 8 | 0.0.5 (2024-06-27) 9 | ------------------ 10 | 11 | 0.0.4 (2024-06-27) 12 | ------------------ 13 | 14 | 0.0.3 (2024-06-27) 15 | ------------------ 16 | 17 | 0.0.2 (2024-06-05) 18 | ------------------ 19 | * Merge pull request `#15 `_ from iRobotEducation/justinIRBT/dance_clean_shutdown 20 | Dance: Handle exceptions for cleaner exit 21 | * add sys dependency 22 | * Handle exceptions for cleaner exit 23 | * Merge pull request `#9 `_ from iRobotEducation/asoragna/ready-to-start 24 | check if all robot topics/servers are available before accepting a goal 25 | * check if all robot topics/servers are available before accepting a goal 26 | * Merge pull request `#6 `_ from iRobotEducation/justinIRBT/send_params 27 | Set safety_override parameter to backup_only on startup to allow dance to drive backwards 28 | * Set safety_override parameter to backup_only to allow dance to drive backwards 29 | * Documentation updates 30 | Typos, trademark symbols, and grammar fixes. 31 | * add READMEs to the repo 32 | * update license to BSD-3 33 | * Merge pull request `#4 `_ from iRobotEducation/asoragna/license 34 | change LICENSE to apache 2.0 35 | * change LICENSE to apache 2.0 36 | * Merge pull request `#3 `_ from iRobotEducation/asoragna/rename-dir 37 | rename create_examples_py into create3_examples_py 38 | * rename create_examples_py into create3_examples_py 39 | * Contributors: Alberto Soragna, Justin Kearns, Steven Shamlian 40 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/behaviors/rotate-behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "create3_coverage/behaviors/behavior.hpp" 6 | #include "create3_coverage/behaviors/reflex-behavior.hpp" 7 | #include "create3_examples_msgs/action/coverage.hpp" 8 | #include "geometry_msgs/msg/twist.hpp" 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 11 | 12 | namespace create3_coverage { 13 | 14 | class RotateBehavior : public Behavior 15 | { 16 | public: 17 | using TwistMsg = geometry_msgs::msg::Twist; 18 | 19 | struct Config 20 | { 21 | double target_rotation {0.785398}; 22 | double angular_vel {0.6}; 23 | bool robot_has_reflexes {false}; 24 | size_t max_hazard_retries {10}; 25 | rclcpp::Duration clear_hazard_time {rclcpp::Duration(std::chrono::seconds(2))}; 26 | }; 27 | 28 | RotateBehavior( 29 | Config config, 30 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 31 | rclcpp::Logger logger, 32 | rclcpp::Clock::SharedPtr clock); 33 | 34 | ~RotateBehavior() = default; 35 | 36 | State execute(const Data & data) override; 37 | 38 | int32_t get_id() const override { return create3_examples_msgs::action::Coverage::Feedback::ROTATE; } 39 | 40 | double get_rotation_amount() { return m_rotation_amount; } 41 | 42 | private: 43 | State handle_hazards(const Data & data); 44 | 45 | Config m_config; 46 | double m_rotation_amount; 47 | 48 | bool m_first_run; 49 | rclcpp::Time m_start_time; 50 | tf2::Quaternion m_initial_orientation; 51 | 52 | std::unique_ptr m_reflex_behavior; 53 | size_t m_hazards_count; 54 | 55 | rclcpp::Publisher::SharedPtr m_cmd_vel_publisher; 56 | rclcpp::Logger m_logger; 57 | rclcpp::Clock::SharedPtr m_clock; 58 | }; 59 | 60 | } // namespace create3_coverage 61 | -------------------------------------------------------------------------------- /create3_lidar_slam/launch/rviz_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2023 iRobot Corporation. All Rights Reserved. 3 | 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import DeclareLaunchArgument 7 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | 11 | def generate_launch_description(): 12 | # Gets the directory of the package and stores it as 'lidar_pkg' 13 | lidar_pkg = get_package_share_directory('create3_lidar_slam') 14 | 15 | # Generate the path to the rviz configuration file 16 | rviz2_config = PathJoinSubstitution( 17 | [lidar_pkg, 'rviz', 'create3_lidar_slam.rviz']) 18 | 19 | # Evaluate at launch the value of the launch configuration 'namespace' 20 | namespace = LaunchConfiguration('namespace') 21 | 22 | # Declares an action to allow users to pass the robot namespace from the 23 | # CLI into the launch description as an argument. 24 | namespace_argument = DeclareLaunchArgument( 25 | 'namespace', 26 | default_value='', 27 | description='Robot namespace') 28 | 29 | # Declares an action that will launch a node when executed by the launch description. 30 | # This node is responsible for configuring and running rviz2. 31 | rviz_node = Node( 32 | package='rviz2', 33 | executable='rviz2', 34 | arguments=['-d', rviz2_config], 35 | 36 | # Remaps topics used by the 'rviz2' package from absolute (with slash) to relative (no slash). 37 | # This is necessary to use namespaces with 'rviz2'. 38 | remappings=[ 39 | ('/tf_static', 'tf_static'), 40 | ('/tf', 'tf')], 41 | namespace=namespace, 42 | output='screen' 43 | ) 44 | 45 | 46 | ld = LaunchDescription() 47 | 48 | ld.add_action(namespace_argument) 49 | ld.add_action(rviz_node) 50 | 51 | # Launches all named actions 52 | return ld -------------------------------------------------------------------------------- /create3_republisher/bringup/create3_republisher_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2024 iRobot Corporation. All Rights Reserved. 3 | 4 | import os 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | from ament_index_python.packages import get_package_share_directory 11 | 12 | 13 | def generate_launch_description(): 14 | 15 | # Fast-DDS env variable check 16 | rmw_impl_env_var = "RMW_IMPLEMENTATION" 17 | fastdds_config_env_var = "FASTRTPS_DEFAULT_PROFILES_FILE" 18 | 19 | if rmw_impl_env_var in os.environ: 20 | print("RMW IMPLEMENTATION: ", os.environ[rmw_impl_env_var]) 21 | if fastdds_config_env_var in os.environ: 22 | print("FAST-DDS CONFIG FILE: ", os.environ[fastdds_config_env_var]) 23 | 24 | republisher_ns = LaunchConfiguration('republisher_ns') 25 | republisher_ns_argument = DeclareLaunchArgument( 26 | 'republisher_ns', 27 | default_value='/repub', 28 | description='Republisher namespace') 29 | 30 | robot_ns = LaunchConfiguration('robot_ns') 31 | robot_ns_argument = DeclareLaunchArgument( 32 | 'robot_ns', 33 | default_value='/', 34 | description='Create 3 robot namespace') 35 | 36 | params_yaml_file = get_package_share_directory("create3_republisher") + '/bringup/params.yaml' 37 | print("Using yaml file ", params_yaml_file) 38 | 39 | start_republisher_node = Node( 40 | parameters=[ 41 | params_yaml_file, 42 | {'robot_namespace': robot_ns} 43 | ], 44 | package='create3_republisher', 45 | executable='create3_republisher', 46 | name='create3_repub', 47 | output='screen', 48 | namespace=republisher_ns, 49 | ) 50 | 51 | ld = LaunchDescription() 52 | 53 | ld.add_action(republisher_ns_argument) 54 | ld.add_action(robot_ns_argument) 55 | ld.add_action(start_republisher_node) 56 | 57 | return ld 58 | -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/spiral-behavior.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include "create3_coverage/behaviors/spiral-behavior.hpp" 4 | #include "utils.hpp" 5 | 6 | namespace create3_coverage { 7 | 8 | SpiralBehavior::SpiralBehavior( 9 | Config config, 10 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 11 | rclcpp::Logger logger, 12 | rclcpp::Clock::SharedPtr clock) 13 | : m_cmd_vel_publisher(cmd_vel_publisher), m_logger(logger) 14 | { 15 | m_config = config; 16 | m_clock = clock; 17 | 18 | m_start_time = m_clock->now(); 19 | m_last_radius_update_time = m_start_time; 20 | m_radius = m_config.initial_radius; 21 | } 22 | 23 | State SpiralBehavior::execute(const Data & data) 24 | { 25 | auto now = m_clock->now(); 26 | rclcpp::Duration spiral_time = now - m_start_time; 27 | if (spiral_time > m_config.spiral_duration) { 28 | RCLCPP_INFO(m_logger, "Spiral completed!"); 29 | return State::SUCCESS; 30 | } 31 | 32 | bool driving_towards_dock = is_driving_towards_dock(data.opcodes); 33 | bool hazards_detected = is_front_hazard_active(data.hazards); 34 | // Pointing towards dock or found hazard 35 | if (driving_towards_dock || hazards_detected) { 36 | RCLCPP_INFO(m_logger, "Stop spiraling: time spent %f/%f hazards %ld dock %d", 37 | spiral_time.seconds(), m_config.spiral_duration.seconds(), 38 | data.hazards.detections.size(), driving_towards_dock); 39 | return State::FAILURE; 40 | } 41 | 42 | if (now - m_last_radius_update_time > m_config.radius_increment_interval) { 43 | m_radius += m_config.radius_increment; 44 | m_last_radius_update_time = now; 45 | } 46 | 47 | auto twist_msg = std::make_unique(); 48 | twist_msg->linear.x = m_config.linear_vel; 49 | twist_msg->angular.z = m_config.linear_vel / m_radius; 50 | 51 | RCLCPP_DEBUG(m_logger, "Spiral velocities: linear %f angular %f", 52 | twist_msg->linear.x, twist_msg->angular.z); 53 | 54 | m_cmd_vel_publisher->publish(std::move(twist_msg)); 55 | 56 | return State::RUNNING; 57 | } 58 | 59 | } // namespace create3_coverage 60 | -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/drive-straight-behavior.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include "create3_coverage/behaviors/drive-straight-behavior.hpp" 4 | #include "utils.hpp" 5 | 6 | namespace create3_coverage { 7 | 8 | DriveStraightBehavior::DriveStraightBehavior( 9 | Config config, 10 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 11 | rclcpp::Logger logger, 12 | rclcpp::Clock::SharedPtr clock) 13 | : m_cmd_vel_publisher(cmd_vel_publisher), m_logger(logger) 14 | { 15 | m_clock = clock; 16 | m_config = config; 17 | 18 | m_first_run = false; 19 | m_start_time = m_clock->now(); 20 | } 21 | 22 | State DriveStraightBehavior::execute(const Data & data) 23 | { 24 | if (!m_first_run) { 25 | m_first_run = true; 26 | m_initial_position = data.pose.position; 27 | } 28 | 29 | double traveled_distance = get_distance(m_initial_position, data.pose.position); 30 | // Handle maximum traveled distance 31 | if (traveled_distance >= m_config.max_distance) { 32 | RCLCPP_INFO(m_logger, "Reached drive straight max distance: %f", traveled_distance); 33 | return State::SUCCESS; 34 | } 35 | 36 | bool driving_towards_dock = is_driving_towards_dock(data.opcodes); 37 | bool hazards_detected = is_front_hazard_active(data.hazards); 38 | // Pointing towards dock or found hazard 39 | if (driving_towards_dock || hazards_detected) { 40 | RCLCPP_INFO(m_logger, "Stop driving straight: traveled %f/%f: hazards %ld dock %d", 41 | traveled_distance, m_config.max_distance, 42 | data.hazards.detections.size(), driving_towards_dock); 43 | 44 | // We are going to stop moving, we consider this a success or failure depending on how much we traveled 45 | if (traveled_distance >= m_config.min_distance) { 46 | return State::SUCCESS; 47 | } else { 48 | return State::FAILURE; 49 | } 50 | } 51 | 52 | auto twist_msg = std::make_unique(); 53 | twist_msg->linear.x = m_config.linear_vel; 54 | m_cmd_vel_publisher->publish(std::move(twist_msg)); 55 | 56 | return State::RUNNING; 57 | } 58 | 59 | } // namespace create3_coverage 60 | -------------------------------------------------------------------------------- /create3_lidar_slam/launch/sensors_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2022 iRobot Corporation. All Rights Reserved. 3 | 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import DeclareLaunchArgument, TimerAction 7 | from launch.substitutions import LaunchConfiguration 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | 11 | def generate_launch_description(): 12 | # Evaluate at launch the value of the launch configuration 'namespace' 13 | namespace = LaunchConfiguration('namespace') 14 | 15 | # Declares an action to allow users to pass the robot namespace from the 16 | # CLI into the launch description as an argument. 17 | namespace_argument = DeclareLaunchArgument( 18 | 'namespace', 19 | default_value='', 20 | description='Robot namespace') 21 | 22 | # Declares an action that will launch a node when executed by the launch description. 23 | # This node is responsible for providing a static transform from the robot's base_footprint 24 | # frame to a new laser_frame, which will be the coordinate frame for the lidar. 25 | static_transform_node = Node( 26 | package='tf2_ros', 27 | executable='static_transform_publisher', 28 | arguments=['-0.012', '0', '0.144', '0', '0', '0', 'base_footprint', 'laser_frame'], 29 | 30 | # Remaps topics used by the 'tf2_ros' package from absolute (with slash) to relative (no slash). 31 | # This is necessary to use namespaces with 'tf2_ros'. 32 | remappings=[ 33 | ('/tf_static', 'tf_static'), 34 | ('/tf', 'tf')], 35 | namespace=namespace 36 | ) 37 | 38 | # Declares an action that will launch a node when executed by the launch description. 39 | # This node is responsible for configuring the RPLidar sensor. 40 | rplidar_node = Node( 41 | package='rplidar_ros', 42 | executable='rplidar_composition', 43 | output='screen', 44 | parameters=[ 45 | get_package_share_directory("create3_lidar_slam") + '/config/rplidar_node.yaml' 46 | ], 47 | namespace=namespace 48 | ) 49 | 50 | # Launches all named actions 51 | return LaunchDescription([ 52 | namespace_argument, 53 | static_transform_node, 54 | TimerAction( 55 | period=2.0, 56 | actions=[rplidar_node] 57 | ) 58 | ]) -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/reflex-behavior.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include "create3_coverage/behaviors/reflex-behavior.hpp" 4 | #include "utils.hpp" 5 | 6 | namespace create3_coverage { 7 | 8 | ReflexBehavior::ReflexBehavior( 9 | Config config, 10 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 11 | rclcpp::Logger logger, 12 | rclcpp::Clock::SharedPtr clock) 13 | : m_cmd_vel_publisher(cmd_vel_publisher), m_logger(logger) 14 | { 15 | m_clock = clock; 16 | m_config = config; 17 | 18 | m_first_run = false; 19 | m_start_time = m_clock->now(); 20 | } 21 | 22 | State ReflexBehavior::execute(const Data & data) 23 | { 24 | if (!m_first_run) { 25 | m_first_run = true; 26 | m_initial_position = data.pose.position; 27 | 28 | if (!is_front_hazard_active(data.hazards)) { 29 | RCLCPP_INFO(m_logger, "No need to run reflex"); 30 | return State::SUCCESS; 31 | } 32 | } 33 | 34 | bool timeout = m_clock->now() - m_start_time > m_config.clear_hazard_time; 35 | bool moved_enough = get_distance(data.pose.position, m_initial_position) > m_config.backup_distance; 36 | bool limit_reached = backup_limit_reached(data.hazards); 37 | 38 | if (timeout || moved_enough || limit_reached) { 39 | if (is_front_hazard_active(data.hazards)) { 40 | RCLCPP_INFO(m_logger, "Reflex failed: was not able to clear hazard (timeout %d distance %d backup %d)", 41 | timeout, moved_enough, limit_reached); 42 | return State::FAILURE; 43 | } else { 44 | RCLCPP_INFO(m_logger, "Reflex successfully cleared hazard"); 45 | return State::SUCCESS; 46 | } 47 | } 48 | 49 | // Command a negative velocity to backup from hazard 50 | auto twist_msg = std::make_unique(); 51 | twist_msg->linear.x = - m_config.linear_vel; 52 | m_cmd_vel_publisher->publish(std::move(twist_msg)); 53 | 54 | return State::RUNNING; 55 | } 56 | 57 | bool ReflexBehavior::backup_limit_reached(const irobot_create_msgs::msg::HazardDetectionVector& hazards) 58 | { 59 | auto limit_hazard = std::find_if(hazards.detections.begin(), hazards.detections.end(), 60 | [](const irobot_create_msgs::msg::HazardDetection& detection){ 61 | return (detection.type == irobot_create_msgs::msg::HazardDetection::BACKUP_LIMIT); 62 | }); 63 | 64 | return limit_hazard != hazards.detections.end(); 65 | } 66 | 67 | } // namespace create3_coverage 68 | -------------------------------------------------------------------------------- /create3_lidar_slam/launch/slam_toolbox_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2022 iRobot Corporation. All Rights Reserved. 3 | 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.substitutions import LaunchConfiguration 7 | from launch_ros.actions import Node 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | 11 | def generate_launch_description(): 12 | # Evaluate at launch the value of the launch configuration 'use_sim_time' 13 | use_sim_time = LaunchConfiguration('use_sim_time') 14 | 15 | # Declares an action to allow users to pass the sim time from the 16 | # CLI into the launch description as an argument. 17 | declare_use_sim_time_argument = DeclareLaunchArgument( 18 | 'use_sim_time', 19 | default_value='false', 20 | description='Use simulation/Gazebo clock') 21 | 22 | # Evaluate at launch the value of the launch configuration 'namespace' 23 | namespace = LaunchConfiguration('namespace') 24 | 25 | # Declares an action to allow users to pass the robot namespace from the 26 | # CLI into the launch description as an argument. 27 | namespace_argument = DeclareLaunchArgument( 28 | 'namespace', 29 | default_value='', 30 | description='Robot namespace') 31 | 32 | # Declares an action that will launch a node when executed by the launch description. 33 | # This node is responsible for configuring and running slam toolbox. 34 | start_async_slam_toolbox_node = Node( 35 | parameters=[ 36 | get_package_share_directory("create3_lidar_slam") + '/config/mapper_params_online_async.yaml', 37 | {'use_sim_time': use_sim_time} 38 | ], 39 | package='slam_toolbox', 40 | executable='async_slam_toolbox_node', 41 | name='slam_toolbox', 42 | output='screen', 43 | namespace=namespace, 44 | # Remaps topics used by the 'slam_toolbox' package from absolute (with slash) to relative (no slash). 45 | # This is necessary to use namespaces with 'slam_toolbox'. 46 | remappings = [ 47 | ('/tf', 'tf'), 48 | ('/tf_static', 'tf_static'), 49 | ('/scan', 'scan'), 50 | ('/map', 'map'), 51 | ('/map_metadata', 'map_metadata') 52 | ]) 53 | 54 | ld = LaunchDescription() 55 | 56 | ld.add_action(declare_use_sim_time_argument) 57 | ld.add_action(namespace_argument) 58 | ld.add_action(start_async_slam_toolbox_node) 59 | 60 | # Launches all named actions 61 | return ld 62 | -------------------------------------------------------------------------------- /create3_coverage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(create3_coverage) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(create3_examples_msgs REQUIRED) 21 | find_package(geometry_msgs REQUIRED) 22 | find_package(irobot_create_msgs REQUIRED) 23 | find_package(nav_msgs REQUIRED) 24 | find_package(rclcpp REQUIRED) 25 | find_package(rclcpp_action REQUIRED) 26 | find_package(tf2_geometry_msgs REQUIRED) 27 | 28 | set(executable_name create3_coverage) 29 | set(library_name ${executable_name}_core) 30 | 31 | set(dependencies 32 | create3_examples_msgs 33 | geometry_msgs 34 | irobot_create_msgs 35 | nav_msgs 36 | rclcpp 37 | rclcpp_action 38 | tf2_geometry_msgs 39 | ) 40 | 41 | add_library(${library_name} SHARED 42 | src/behaviors/dock-behavior.cpp 43 | src/behaviors/drive-straight-behavior.cpp 44 | src/behaviors/reflex-behavior.cpp 45 | src/behaviors/rotate-behavior.cpp 46 | src/behaviors/spiral-behavior.cpp 47 | src/behaviors/undock-behavior.cpp 48 | src/behaviors/utils.cpp 49 | src/coverage_state_machine.cpp 50 | src/create3_coverage_node.cpp 51 | ) 52 | target_include_directories(${library_name} PUBLIC include) 53 | ament_target_dependencies(${library_name} ${dependencies}) 54 | 55 | add_executable(${executable_name} src/main.cpp) 56 | ament_target_dependencies(${executable_name} ${dependencies}) 57 | target_link_libraries(${executable_name} ${library_name}) 58 | 59 | install(TARGETS ${library_name} 60 | ARCHIVE DESTINATION lib 61 | LIBRARY DESTINATION lib 62 | RUNTIME DESTINATION bin 63 | ) 64 | 65 | install(TARGETS ${executable_name} 66 | RUNTIME DESTINATION lib/${PROJECT_NAME} 67 | ) 68 | 69 | install(DIRECTORY include/ 70 | DESTINATION include/ 71 | ) 72 | 73 | if(BUILD_TESTING) 74 | find_package(ament_lint_auto REQUIRED) 75 | # the following line skips the linter which checks for copyrights 76 | set(ament_cmake_copyright_FOUND TRUE) 77 | set(ament_cmake_cpplint_FOUND TRUE) 78 | ament_lint_auto_find_test_dependencies() 79 | endif() 80 | 81 | ament_export_include_directories(include) 82 | ament_export_libraries(${library_name}) 83 | ament_export_dependencies(${dependencies}) 84 | 85 | ament_package() 86 | -------------------------------------------------------------------------------- /create3_coverage/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package create3_coverage 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2024-09-28) 6 | ------------------ 7 | 8 | 0.0.5 (2024-06-27) 9 | ------------------ 10 | * update create3_coverage package.xml 11 | * Contributors: Alberto Soragna 12 | 13 | 0.0.4 (2024-06-27) 14 | ------------------ 15 | 16 | 0.0.3 (2024-06-27) 17 | ------------------ 18 | 19 | 0.0.2 (2024-06-05) 20 | ------------------ 21 | * Merge pull request `#39 `_ from iRobotEducation/jkearns/topic-fix 22 | change dock topic to dock_status for humble 23 | * fix topic for humble 24 | * minor changes to support ROS 2 Humble 25 | * update READMEs with link to ROS galactic and create3_coverage troubleshooting 26 | * Merge pull request `#17 `_ from iRobotEducation/asoragna/rename-duration 27 | rename coverage action max_duration into max_runtime 28 | * rename coverage action max_duration into max_runtime 29 | * Merge pull request `#9 `_ from iRobotEducation/asoragna/ready-to-start 30 | check if all robot topics/servers are available before accepting a goal 31 | * check if all robot topics/servers are available before accepting a goal 32 | * Documentation updates 33 | Typos, trademark symbols, and grammar fixes. 34 | * update license to BSD-3 35 | * Merge pull request `#5 `_ from iRobotEducation/asoragna/improvements 36 | improve dock detection, add backup limit hazard and minor improvements 37 | * reduce opcodes buffer time 38 | * improve dock detection, add backup limit hazard and minor improvements 39 | * Merge pull request `#4 `_ from iRobotEducation/asoragna/license 40 | change LICENSE to apache 2.0 41 | * change LICENSE to apache 2.0 42 | * Merge pull request `#2 `_ from iRobotEducation/asoragna/create3-coverage 43 | Add create3 coverage example 44 | * mention reflexes in readme 45 | * allow user to disable reflexes on the robot and run custom reflex implementation 46 | * keep list of past orientations when trying to evade from obstacles 47 | * add explore_time argument and cleanup of logs 48 | * go to drive_straight if spiral succeeds 49 | * small fixes handling cancel and abort 50 | * rework drive_straight behavior to support max distance 51 | * fix typo in README.md 52 | * enable kidnap detection 53 | * Add create3 coverage example 54 | * Contributors: Alberto Soragna, Justin Kearns, Steven Shamlian 55 | -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/dock-behavior.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include "create3_coverage/behaviors/dock-behavior.hpp" 4 | 5 | namespace create3_coverage { 6 | 7 | DockBehavior::DockBehavior( 8 | rclcpp_action::Client::SharedPtr dock_action_client, 9 | rclcpp::Logger logger) 10 | : m_dock_action_client(dock_action_client), m_logger(logger) 11 | { 12 | 13 | } 14 | 15 | State DockBehavior::execute(const Data & data) 16 | { 17 | // Make sure we are not docked 18 | if (!m_dock_action_sent && data.dock.is_docked) { 19 | RCLCPP_ERROR(m_logger, "Robot is already docked!"); 20 | return State::FAILURE; 21 | } 22 | 23 | // We can't dock until we discover the docking action server 24 | if (!m_dock_action_client->action_server_is_ready()) { 25 | RCLCPP_DEBUG(m_logger, "Waiting for dock action server"); 26 | return State::RUNNING; 27 | } 28 | 29 | // Send dock command if not already sent and if we are not waiting for result 30 | if (!m_dock_action_sent) { 31 | RCLCPP_INFO(m_logger, "Sending docking goal!"); 32 | auto goal_msg = DockAction::Goal(); 33 | 34 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 35 | send_goal_options.goal_response_callback = [this](const GoalHandleDock::SharedPtr & goal_handle){ 36 | m_dock_goal_handle_ready = true; 37 | m_dock_goal_handle = goal_handle; 38 | }; 39 | send_goal_options.result_callback = [this](const GoalHandleDock::WrappedResult & result){ 40 | m_dock_result_ready = true; 41 | m_dock_result = result; 42 | }; 43 | 44 | m_dock_action_client->async_send_goal(goal_msg, send_goal_options); 45 | m_dock_action_sent = true; 46 | 47 | return State::RUNNING; 48 | } 49 | 50 | if (m_dock_goal_handle_ready && !m_dock_goal_handle) { 51 | RCLCPP_ERROR(m_logger, "Docking goal was rejected by server"); 52 | return State::FAILURE; 53 | } 54 | 55 | if (m_dock_result_ready) { 56 | if (m_dock_result.code == rclcpp_action::ResultCode::SUCCEEDED) { 57 | RCLCPP_INFO(m_logger, "Docking succeeded!"); 58 | return State::SUCCESS; 59 | } else { 60 | RCLCPP_ERROR(m_logger, "Docking failed!"); 61 | return State::FAILURE; 62 | } 63 | } 64 | 65 | return State::RUNNING; 66 | } 67 | 68 | void DockBehavior::cleanup() 69 | { 70 | // This behavior is being cancelled, so send a cancel request to dock action server if it's running 71 | if (!m_dock_result_ready && m_dock_goal_handle_ready && m_dock_goal_handle) { 72 | m_dock_action_client->async_cancel_goal(m_dock_goal_handle); 73 | } 74 | } 75 | 76 | } // namespace create3_coverage 77 | -------------------------------------------------------------------------------- /create3_republisher/dds-config/fastdds-robot-passive-unicast.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | udp_transport 8 | UDPv4 9 | 10 | 4096 11 | 4096 12 | 13 | 4096 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | udp_transport 23 | 24 | 25 | false 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 |
127.0.0.1
34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 0 47 | 0 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 0 58 | 0 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/undock-behavior.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include "create3_coverage/behaviors/undock-behavior.hpp" 4 | 5 | namespace create3_coverage { 6 | 7 | UndockBehavior::UndockBehavior( 8 | rclcpp_action::Client::SharedPtr undock_action_client, 9 | rclcpp::Logger logger) 10 | : m_undock_action_client(undock_action_client), m_logger(logger) 11 | { 12 | 13 | } 14 | 15 | State UndockBehavior::execute(const Data & data) 16 | { 17 | // Make sure we are not docked 18 | if (!m_undock_action_sent && !data.dock.is_docked) { 19 | RCLCPP_ERROR(m_logger, "Robot is already undocked!"); 20 | return State::FAILURE; 21 | } 22 | 23 | // We can't undock until we discover the undocking action server 24 | if (!m_undock_action_client->action_server_is_ready()) { 25 | RCLCPP_DEBUG(m_logger, "Waiting for undock action server"); 26 | return State::RUNNING; 27 | } 28 | 29 | // Send undock command if not already sent and if we are not waiting for result 30 | if (!m_undock_action_sent) { 31 | RCLCPP_INFO(m_logger, "Sending undocking goal!"); 32 | auto goal_msg = UndockAction::Goal(); 33 | 34 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 35 | send_goal_options.goal_response_callback = [this](const GoalHandleUndock::SharedPtr & goal_handle){ 36 | m_undock_goal_handle_ready = true; 37 | m_undock_goal_handle = goal_handle; 38 | }; 39 | send_goal_options.result_callback = [this](const GoalHandleUndock::WrappedResult & result){ 40 | m_undock_result_ready = true; 41 | m_undock_result = result; 42 | }; 43 | 44 | m_undock_action_client->async_send_goal(goal_msg, send_goal_options); 45 | m_undock_action_sent = true; 46 | 47 | return State::RUNNING; 48 | } 49 | 50 | if (m_undock_goal_handle_ready && !m_undock_goal_handle) { 51 | RCLCPP_ERROR(m_logger, "Undock goal was rejected by server"); 52 | return State::FAILURE; 53 | } 54 | 55 | if (m_undock_result_ready) { 56 | if (m_undock_result.code == rclcpp_action::ResultCode::SUCCEEDED) { 57 | RCLCPP_INFO(m_logger, "Undocking succeeded!"); 58 | return State::SUCCESS; 59 | } else { 60 | RCLCPP_ERROR(m_logger, "Undocking failed!"); 61 | return State::FAILURE; 62 | } 63 | } 64 | 65 | return State::RUNNING; 66 | } 67 | 68 | void UndockBehavior::cleanup() 69 | { 70 | // This behavior is being cancelled, so send a cancel request to dock action server if it's running 71 | if (!m_undock_result_ready && m_undock_goal_handle_ready && m_undock_goal_handle) { 72 | m_undock_action_client->async_cancel_goal(m_undock_goal_handle); 73 | } 74 | } 75 | 76 | } // namespace create3_coverage 77 | -------------------------------------------------------------------------------- /create3_republisher/bringup/params.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | create3_repub: 3 | ros__parameters: 4 | # These lists define which ROS 2 entities should be republished from/to the Create 3 robot. 5 | # Tip: rather than deleting the ones that you are not interested in, try commenting them. 6 | # This will make it easier to re-enable them if they become needed. 7 | # Note: if you want to fully disable a category (e.g. the services), you'll need to comment out 8 | # all the associated lines in this file, and not only all the elements in the list 9 | # NOTE: for best performance, uncomment only the entities that you need to interact with. 10 | robot_publishers: [ 11 | "battery_state", "sensor_msgs/msg/BatteryState", 12 | # "cliff_intensity", "irobot_create_msgs/msg/IrIntensityVector", 13 | "dock_status", "irobot_create_msgs/msg/DockStatus", 14 | # "hazard_detection", "irobot_create_msgs/msg/HazardDetectionVector", 15 | "imu", "sensor_msgs/msg/Imu", 16 | "interface_buttons", "irobot_create_msgs/msg/InterfaceButtons", 17 | # "ir_intensity", "irobot_create_msgs/msg/IrIntensityVector", 18 | # "ir_opcode", "irobot_create_msgs/msg/IrOpcode", 19 | # "kidnap_status", "irobot_create_msgs/msg/KidnapStatus", 20 | # "mouse", "irobot_create_msgs/msg/Mouse", 21 | "odom", "nav_msgs/msg/Odometry", 22 | # "slip_status", "irobot_create_msgs/msg/SlipStatus", 23 | # "stop_status", "irobot_create_msgs/msg/StopStatus", 24 | "tf", "tf2_msgs/msg/TFMessage", 25 | "tf_static", "tf2_msgs/msg/TFMessage", 26 | "wheel_status", "irobot_create_msgs/msg/WheelStatus", 27 | # "wheel_ticks", "irobot_create_msgs/msg/WheelTicks", 28 | # "wheel_vels", "irobot_create_msgs/msg/WheelVels", 29 | ] 30 | robot_subscriptions: [ 31 | "cmd_audio", "irobot_create_msgs/msg/AudioNoteVector", 32 | "cmd_lightring", "irobot_create_msgs/msg/LightringLeds", 33 | "cmd_vel", "geometry_msgs/msg/Twist", 34 | "cmd_vel_stamped", "geometry_msgs/msg/TwistStamped", 35 | ] 36 | robot_services: [ 37 | "e_stop", "irobot_create_msgs/srv/EStop", 38 | "reset_pose", "irobot_create_msgs/srv/ResetPose", 39 | "robot_power", "irobot_create_msgs/srv/RobotPower", 40 | ] 41 | robot_actions: [ 42 | "audio_note_sequence", "irobot_create_msgs/action/AudioNoteSequence", 43 | "dock", "irobot_create_msgs/action/Dock", 44 | "drive_arc", "irobot_create_msgs/action/DriveArc", 45 | "drive_distance", "irobot_create_msgs/action/DriveDistance", 46 | "led_animation", "irobot_create_msgs/action/LedAnimation", 47 | "navigate_to_position", "irobot_create_msgs/action/NavigateToPosition", 48 | "rotate_angle", "irobot_create_msgs/action/RotateAngle", 49 | "undock", "irobot_create_msgs/action/Undock", 50 | "wall_follow", "irobot_create_msgs/action/WallFollow", 51 | ] 52 | -------------------------------------------------------------------------------- /create3_lidar_slam/config/mapper_params_online_async.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | slam_toolbox: 3 | ros__parameters: 4 | 5 | # Plugin params 6 | solver_plugin: solver_plugins::CeresSolver 7 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 8 | ceres_preconditioner: SCHUR_JACOBI 9 | ceres_trust_strategy: LEVENBERG_MARQUARDT 10 | ceres_dogleg_type: TRADITIONAL_DOGLEG 11 | ceres_loss_function: None 12 | 13 | # ROS Parameters 14 | odom_frame: odom 15 | map_frame: map 16 | base_frame: base_footprint 17 | scan_topic: scan 18 | mode: mapping #localization 19 | 20 | # if you'd like to immediately start continuing a map at a given pose 21 | # or at the dock, but they are mutually exclusive, if pose is given 22 | # will use pose 23 | #map_file_name: test_steve 24 | # map_start_pose: [0.0, 0.0, 0.0] 25 | #map_start_at_dock: true 26 | 27 | debug_logging: false 28 | throttle_scans: 1 29 | transform_publish_period: 0.02 #if 0 never publishes odometry 30 | map_update_interval: 1.0 31 | resolution: 0.05 32 | max_laser_range: 4.0 #for rastering images 33 | minimum_time_interval: 0.5 34 | transform_timeout: 0.2 35 | tf_buffer_duration: 30. 36 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 37 | enable_interactive_mode: true 38 | 39 | # General Parameters 40 | use_scan_matching: true 41 | use_scan_barycenter: true 42 | minimum_travel_distance: 0.25 43 | minimum_travel_heading: 0.5 44 | scan_buffer_size: 10 45 | scan_buffer_maximum_scan_distance: 10.0 46 | link_match_minimum_response_fine: 0.1 47 | link_scan_maximum_distance: 1.5 48 | loop_search_maximum_distance: 3.0 49 | do_loop_closing: true 50 | loop_match_minimum_chain_size: 10 51 | loop_match_maximum_variance_coarse: 3.0 52 | loop_match_minimum_response_coarse: 0.35 53 | loop_match_minimum_response_fine: 0.45 54 | 55 | # Correlation Parameters - Correlation Parameters 56 | correlation_search_space_dimension: 0.5 57 | correlation_search_space_resolution: 0.01 58 | correlation_search_space_smear_deviation: 0.1 59 | 60 | # Correlation Parameters - Loop Closure Parameters 61 | loop_search_space_dimension: 8.0 62 | loop_search_space_resolution: 0.05 63 | loop_search_space_smear_deviation: 0.03 64 | 65 | # Scan Matcher Parameters 66 | distance_variance_penalty: 0.5 67 | angle_variance_penalty: 1.0 68 | 69 | fine_search_angle_offset: 0.00349 70 | coarse_search_angle_offset: 0.349 71 | coarse_angle_resolution: 0.0349 72 | minimum_angle_penalty: 0.9 73 | minimum_distance_penalty: 0.5 74 | use_response_expansion: true 75 | 76 | -------------------------------------------------------------------------------- /create3_teleop/launch/teleop_joystick_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2021 iRobot Corporation. All Rights Reserved. 3 | # @author Alexis Pojomovsky (apojomovsky@irobot.com) 4 | # 5 | # Launches a joystick teleop node. 6 | 7 | import os 8 | from typing import Text 9 | 10 | from ament_index_python.packages import get_package_share_directory, PackageNotFoundError 11 | 12 | from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution 13 | from launch.actions import DeclareLaunchArgument 14 | from launch.substitutions import LaunchConfiguration 15 | from launch_ros.actions import Node 16 | 17 | 18 | class JoystickConfigParser(Substitution): 19 | def __init__( 20 | self, 21 | package_name: Text, 22 | device_type: SomeSubstitutionsType 23 | ) -> None: 24 | self.__package_name = package_name 25 | self.__device_type = device_type 26 | 27 | def perform( 28 | self, 29 | context: LaunchContext = None, 30 | ) -> Text: 31 | device_type_str = self.__device_type.perform(context) 32 | package_str = self.__package_name 33 | try: 34 | package_share_dir = get_package_share_directory( 35 | package_str) 36 | config_filepath = [package_share_dir, 'config', 37 | f'{device_type_str}.config.yaml'] 38 | return os.path.join(*config_filepath) 39 | except PackageNotFoundError: 40 | raise PackageNotFoundError(package_str) 41 | 42 | 43 | def generate_launch_description(): 44 | joy_config = LaunchConfiguration('joy_config') 45 | joy_dev = LaunchConfiguration('joy_dev') 46 | 47 | # Invokes a node that interfaces a generic joystick to ROS 2. 48 | joy_node = Node(package='joy', executable='joy_node', name='joy_node', 49 | parameters=[{ 50 | 'dev': joy_dev, 51 | 'deadzone': 0.3, 52 | 'autorepeat_rate': 20.0, 53 | }]) 54 | 55 | # Retrieve the path to the correct configuration .yaml depending on 56 | # the joy_config argument 57 | config_filepath = JoystickConfigParser('teleop_twist_joy', joy_config) 58 | 59 | # Publish unstamped Twist message from an attached USB Joystick. 60 | teleop_node = Node(package='teleop_twist_joy', executable='teleop_node', 61 | name='teleop_twist_joy_node', parameters=[config_filepath]) 62 | 63 | # Declare launchfile arguments 64 | ld_args = [] 65 | ld_args.append(DeclareLaunchArgument('joy_config', 66 | default_value='xbox', 67 | choices=['xbox', 'ps3', 'ps3-holonomic', 'atk3', 'xd3'])) 68 | ld_args.append(DeclareLaunchArgument('joy_dev', 69 | default_value='/dev/input/js0')) 70 | 71 | # Define LaunchDescription variable 72 | ld = LaunchDescription(ld_args) 73 | 74 | # Add nodes to LaunchDescription 75 | ld.add_action(joy_node) 76 | ld.add_action(teleop_node) 77 | 78 | return ld 79 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/coverage_state_machine.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include "create3_coverage/behaviors/behavior.hpp" 6 | #include "create3_coverage/behaviors/dock-behavior.hpp" 7 | #include "create3_coverage/behaviors/drive-straight-behavior.hpp" 8 | #include "create3_coverage/behaviors/rotate-behavior.hpp" 9 | #include "create3_coverage/behaviors/spiral-behavior.hpp" 10 | #include "create3_coverage/behaviors/undock-behavior.hpp" 11 | #include "create3_coverage/state.hpp" 12 | #include "create3_examples_msgs/action/coverage.hpp" 13 | #include "geometry_msgs/msg/twist.hpp" 14 | #include "irobot_create_msgs/action/dock.hpp" 15 | #include "irobot_create_msgs/action/undock.hpp" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "rclcpp_action/rclcpp_action.hpp" 18 | 19 | namespace create3_coverage { 20 | 21 | class CoverageStateMachine 22 | { 23 | public: 24 | using DockAction = irobot_create_msgs::action::Dock; 25 | using UndockAction = irobot_create_msgs::action::Undock; 26 | using TwistMsg = geometry_msgs::msg::Twist; 27 | 28 | struct CoverageOutput 29 | { 30 | int32_t current_behavior; 31 | State state; 32 | }; 33 | 34 | CoverageStateMachine( 35 | create3_examples_msgs::action::Coverage::Goal goal, 36 | rclcpp::Clock::SharedPtr clock, 37 | rclcpp::Logger logger, 38 | rclcpp_action::Client::SharedPtr dock_action_client, 39 | rclcpp_action::Client::SharedPtr undock_action_client, 40 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 41 | bool has_reflexes); 42 | 43 | ~CoverageStateMachine(); 44 | 45 | CoverageOutput execute(const Behavior::Data& data); 46 | 47 | void cancel(); 48 | 49 | private: 50 | using FeedbackMsg = create3_examples_msgs::action::Coverage::Feedback; 51 | 52 | void select_start_behavior(const Behavior::Data& data); 53 | 54 | void select_next_behavior(const Behavior::Data& data); 55 | 56 | void goto_dock(); 57 | 58 | void goto_drive_straight(const DriveStraightBehavior::Config& config = DriveStraightBehavior::Config()); 59 | 60 | void goto_rotate(const RotateBehavior::Config& config = RotateBehavior::Config()); 61 | 62 | void goto_spiral(const SpiralBehavior::Config& config = SpiralBehavior::Config()); 63 | 64 | void goto_undock(); 65 | 66 | double compute_evade_rotation(const geometry_msgs::msg::Pose& pose, double resolution); 67 | 68 | std::shared_ptr m_current_behavior; 69 | State m_behavior_state; 70 | 71 | bool m_undocking; 72 | rclcpp::Time m_last_spiral_time; 73 | bool m_preparing_spiral; 74 | std::vector m_evade_attempts; 75 | 76 | CoverageOutput m_coverage_output; 77 | create3_examples_msgs::action::Coverage::Goal m_goal; 78 | rclcpp::Time m_start_time; 79 | bool m_has_reflexes; 80 | 81 | rclcpp_action::Client::SharedPtr m_dock_action_client; 82 | rclcpp_action::Client::SharedPtr m_undock_action_client; 83 | rclcpp::Publisher::SharedPtr m_cmd_vel_publisher; 84 | rclcpp::Logger m_logger; 85 | rclcpp::Clock::SharedPtr m_clock; 86 | }; 87 | 88 | } // namespace create3_coverage 89 | -------------------------------------------------------------------------------- /create3_coverage/include/create3_coverage/create3_coverage_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "create3_coverage/behaviors/behavior.hpp" 10 | #include "create3_examples_msgs/action/coverage.hpp" 11 | #include "geometry_msgs/msg/twist.hpp" 12 | #include "irobot_create_msgs/action/dock.hpp" 13 | #include "irobot_create_msgs/action/undock.hpp" 14 | #include "irobot_create_msgs/msg/dock_status.hpp" 15 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 16 | #include "irobot_create_msgs/msg/ir_opcode.hpp" 17 | #include "irobot_create_msgs/msg/kidnap_status.hpp" 18 | #include "nav_msgs/msg/odometry.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "rclcpp_action/rclcpp_action.hpp" 21 | 22 | namespace create3_coverage { 23 | 24 | class Create3CoverageNode : public rclcpp::Node 25 | { 26 | public: 27 | /** @brief Node constructor, initializes ROS 2 entities */ 28 | Create3CoverageNode(); 29 | 30 | private: 31 | using CoverageAction = create3_examples_msgs::action::Coverage; 32 | using GoalHandleCoverage = rclcpp_action::ServerGoalHandle; 33 | 34 | using DockAction = irobot_create_msgs::action::Dock; 35 | using UndockAction = irobot_create_msgs::action::Undock; 36 | using DockMsg = irobot_create_msgs::msg::DockStatus; 37 | using HazardMsg = irobot_create_msgs::msg::HazardDetectionVector; 38 | using KidnapMsg = irobot_create_msgs::msg::KidnapStatus; 39 | using OdometryMsg = nav_msgs::msg::Odometry; 40 | using OpCodeMsg = irobot_create_msgs::msg::IrOpcode; 41 | using TwistMsg = geometry_msgs::msg::Twist; 42 | 43 | bool reflexes_setup(); 44 | 45 | bool ready_to_start(); 46 | 47 | rclcpp_action::GoalResponse 48 | handle_goal( 49 | const rclcpp_action::GoalUUID& uuid, 50 | std::shared_ptr goal); 51 | 52 | rclcpp_action::CancelResponse 53 | handle_cancel( 54 | const std::shared_ptr goal_handle); 55 | 56 | void handle_accepted(const std::shared_ptr goal_handle); 57 | 58 | void execute(const std::shared_ptr goal_handle); 59 | 60 | void dock_callback(DockMsg::ConstSharedPtr msg); 61 | 62 | void hazards_callback(HazardMsg::ConstSharedPtr msg); 63 | 64 | void ir_opcode_callback(OpCodeMsg::ConstSharedPtr msg); 65 | 66 | void kidnap_callback(KidnapMsg::ConstSharedPtr msg); 67 | 68 | void odom_callback(OdometryMsg::ConstSharedPtr msg); 69 | 70 | int32_t m_last_behavior; 71 | 72 | double m_rate_hz; 73 | int m_opcodes_buffer_ms; 74 | 75 | std::atomic m_is_running; 76 | std::mutex m_mutex; 77 | 78 | std::atomic m_dock_msgs_received; 79 | DockMsg m_last_dock; 80 | HazardMsg m_last_hazards; 81 | KidnapMsg m_last_kidnap; 82 | OdometryMsg m_last_odom; 83 | std::vector m_last_opcodes; 84 | rclcpp::Time m_last_opcodes_cleared_time; 85 | 86 | rclcpp_action::Server::SharedPtr m_coverage_action_server; 87 | 88 | rclcpp_action::Client::SharedPtr m_dock_action_client; 89 | rclcpp_action::Client::SharedPtr m_undock_action_client; 90 | 91 | rclcpp::Publisher::SharedPtr m_cmd_vel_publisher; 92 | 93 | rclcpp::AsyncParametersClient::SharedPtr m_reflexes_param_client; 94 | 95 | rclcpp::Subscription::SharedPtr m_dock_subscription; 96 | rclcpp::Subscription::SharedPtr m_hazards_subscription; 97 | rclcpp::Subscription::SharedPtr m_kidnap_subscription; 98 | rclcpp::Subscription::SharedPtr m_odom_subscription; 99 | rclcpp::Subscription::SharedPtr m_ir_opcode_subscription; 100 | }; 101 | 102 | } // namespace create3_coverage 103 | -------------------------------------------------------------------------------- /create3_coverage/src/behaviors/rotate-behavior.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include 4 | 5 | #include "create3_coverage/behaviors/rotate-behavior.hpp" 6 | #include "tf2/utils.h" 7 | 8 | #include "utils.hpp" 9 | 10 | namespace create3_coverage { 11 | 12 | RotateBehavior::RotateBehavior( 13 | Config config, 14 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 15 | rclcpp::Logger logger, 16 | rclcpp::Clock::SharedPtr clock) 17 | : m_cmd_vel_publisher(cmd_vel_publisher), m_logger(logger) 18 | { 19 | m_clock = clock; 20 | m_config = config; 21 | 22 | m_first_run = false; 23 | m_start_time = m_clock->now(); 24 | m_hazards_count = 0; 25 | } 26 | 27 | State RotateBehavior::execute(const Data & data) 28 | { 29 | if (m_hazards_count > m_config.max_hazard_retries) { 30 | RCLCPP_INFO(m_logger, "Failed to clear hazard! Too many trials"); 31 | return State::FAILURE; 32 | } 33 | 34 | // Use reflexes to handle hazards if we have hazard detections 35 | State reflex_state = handle_hazards(data); 36 | // Handle RUNNING and FAILURE reflex states. 37 | if (reflex_state != State::SUCCESS) { 38 | return reflex_state; 39 | } 40 | m_reflex_behavior.reset(); 41 | 42 | if (!m_first_run) { 43 | // After reflex returns SUCCESS, we are ready to start the behavior 44 | m_first_run = true; 45 | tf2::convert(data.pose.orientation, m_initial_orientation); 46 | 47 | RCLCPP_DEBUG(m_logger, "Rotation initial yaw: %f", tf2::getYaw(m_initial_orientation)); 48 | } 49 | 50 | tf2::Quaternion current_orientation; 51 | tf2::convert(data.pose.orientation, current_orientation); 52 | tf2::Quaternion relative_orientation = current_orientation * m_initial_orientation.inverse(); 53 | 54 | double relative_yaw = tf2::getYaw(relative_orientation); 55 | if (std::abs(relative_yaw) >= std::abs(m_config.target_rotation)) { 56 | RCLCPP_INFO(m_logger, "Rotation completed: from %f to %f", 57 | tf2::getYaw(m_initial_orientation), tf2::getYaw(current_orientation)); 58 | return State::SUCCESS; 59 | } 60 | 61 | 62 | RCLCPP_DEBUG(m_logger, "Rotating: current orientation %f progress %f/%f", 63 | tf2::getYaw(current_orientation), relative_yaw, m_config.target_rotation); 64 | 65 | auto twist_msg = std::make_unique(); 66 | twist_msg->angular.z = std::copysign(m_config.angular_vel, m_config.target_rotation); 67 | m_cmd_vel_publisher->publish(std::move(twist_msg)); 68 | 69 | return State::RUNNING; 70 | } 71 | 72 | State RotateBehavior::handle_hazards(const Data & data) 73 | { 74 | if (!is_front_hazard_active(data.hazards) && !m_reflex_behavior) { 75 | return State::SUCCESS; 76 | } 77 | 78 | if (m_config.robot_has_reflexes) { 79 | // Wait for reflexes to clear the hazards. 80 | // Making sure that they do not take too much time. 81 | if (m_clock->now() - m_start_time > m_config.clear_hazard_time) { 82 | RCLCPP_INFO(m_logger, "Aborting ROTATE because initial hazard is not getting cleared"); 83 | return State::FAILURE; 84 | } 85 | 86 | // Return SUCCESS or RUNNING depending on whether the hazards have been cleared 87 | if (!is_front_hazard_active(data.hazards)) { 88 | return State::SUCCESS; 89 | } else { 90 | return State::RUNNING; 91 | } 92 | } else { 93 | // Run a reflex behavior to clear the hazard because the robot does not have 94 | // active reflexes. 95 | 96 | // Initialize the reflex behavior if necessary 97 | if (!m_reflex_behavior) { 98 | m_hazards_count++; 99 | RCLCPP_INFO(m_logger, "Starting reflex behavior to clear hazard"); 100 | 101 | auto config = ReflexBehavior::Config(); 102 | config.clear_hazard_time = m_config.clear_hazard_time; 103 | config.backup_distance = 0.05; 104 | config.linear_vel = 0.1; 105 | m_reflex_behavior = std::make_unique(config, m_cmd_vel_publisher, m_logger, m_clock); 106 | } 107 | 108 | // Run the reflex behavior 109 | auto reflex_state = m_reflex_behavior->execute(data); 110 | return reflex_state; 111 | } 112 | } 113 | 114 | } // namespace create3_coverage 115 | -------------------------------------------------------------------------------- /create3_examples_py/create3_examples_py/dance/create3_dance.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | import rclpy 4 | import sys 5 | from create3_examples_py.dance import dance_choreograph as dc 6 | 7 | def main(args=None): 8 | rclpy.init(args=args) 9 | cp = dc.ColorPalette() 10 | ''' 11 | DANCE SEQUENCE is defined as a list of pairs with 12 | (time to start action in seconds, action to take) 13 | ''' 14 | DANCE_SEQUENCE = [ 15 | (0.0, dc.Lights([cp.green, cp.green, cp.green, cp.green, cp.green, cp.green])), 16 | (0.0, dc.Move(0.0, 0.0)), 17 | (0.5, dc.Lights([cp.white, cp.grey, cp.white, cp.grey, cp.white, cp.grey])), 18 | (0.5, dc.Move(0.15, 70)), 19 | (1.0, dc.Lights([cp.grey, cp.white, cp.grey, cp.white, cp.grey, cp.white])), 20 | (1.0, dc.Move(0.15, -70)), 21 | (1.5, dc.Lights([cp.white, cp.grey, cp.white, cp.grey, cp.white, cp.grey])), 22 | (1.5, dc.Move(0.15, 70)), 23 | (2.0, dc.Lights([cp.grey, cp.white, cp.grey, cp.white, cp.grey, cp.white])), 24 | (2.0, dc.Move(0.15, -70)), 25 | (2.5, dc.Lights([cp.white, cp.grey, cp.white, cp.grey, cp.white, cp.grey])), 26 | (2.5, dc.Move(-0.15, 70)), 27 | (3.0, dc.Lights([cp.grey, cp.white, cp.grey, cp.white, cp.grey, cp.white])), 28 | (3.0, dc.Move(-0.15, -70)), 29 | (3.5, dc.Lights([cp.white, cp.grey, cp.white, cp.grey, cp.white, cp.grey])), 30 | (3.5, dc.Move(-0.15, 70)), 31 | (4.0, dc.Lights([cp.grey, cp.white, cp.grey, cp.white, cp.grey, cp.white])), 32 | (4.0, dc.Move(-0.15, -70)), 33 | (4.5, dc.Lights([cp.red, cp.green, cp.blue, cp.yellow, cp.pink, cp.cyan])), 34 | (4.5, dc.Move(0.0, 60)), 35 | (5.0, dc.Lights([cp.cyan, cp.red, cp.green, cp.blue, cp.yellow, cp.pink])), 36 | (5.5, dc.Lights([cp.pink, cp.cyan, cp.red, cp.green, cp.blue, cp.yellow])), 37 | (6.0, dc.Lights([cp.yellow, cp.pink, cp.cyan, cp.red, cp.green, cp.blue])), 38 | (6.5, dc.Lights([cp.blue, cp.yellow, cp.pink, cp.cyan, cp.red, cp.green])), 39 | (6.5, dc.Move(0.0, -75)), 40 | (7.0, dc.Lights([cp.cyan, cp.red, cp.green, cp.blue, cp.yellow, cp.pink])), 41 | (7.5, dc.Lights([cp.pink, cp.cyan, cp.red, cp.green, cp.blue, cp.yellow])), 42 | (8.0, dc.Lights([cp.yellow, cp.pink, cp.cyan, cp.red, cp.green, cp.blue])), 43 | (8.5, dc.Lights([cp.blue, cp.yellow, cp.pink, cp.cyan, cp.red, cp.green])), 44 | (9.0, dc.Lights([cp.green, cp.blue, cp.yellow, cp.pink, cp.cyan, cp.red])), 45 | (9.5, dc.Lights([cp.red, cp.green, cp.blue, cp.yellow, cp.pink, cp.cyan])), 46 | (9.5, dc.Move(-0.15, 50)), 47 | (10.0, dc.Lights([cp.cyan, cp.red, cp.green, cp.blue, cp.yellow, cp.pink])), 48 | (10.0, dc.Move(-0.15, -50)), 49 | (10.5, dc.Lights([cp.pink, cp.cyan, cp.red, cp.green, cp.blue, cp.yellow])), 50 | (10.5, dc.Move(-0.15, 50)), 51 | (11.0, dc.Lights([cp.yellow, cp.pink, cp.cyan, cp.red, cp.green, cp.blue])), 52 | (11.0, dc.Move(-0.15, -50)), 53 | (11.5, dc.Lights([cp.blue, cp.yellow, cp.pink, cp.cyan, cp.red, cp.green])), 54 | (11.5, dc.Move(0.0, 75)), 55 | (13.0, dc.Lights([cp.green, cp.blue, cp.yellow, cp.pink, cp.cyan, cp.red])), 56 | (13.0, dc.Move(0.0, 75)), 57 | (14.0, dc.Lights([cp.red, cp.green, cp.blue, cp.yellow, cp.pink, cp.cyan])), 58 | (14.0, dc.Move(0.0, -100)), 59 | (15.5, dc.Lights([cp.cyan, cp.red, cp.green, cp.blue, cp.yellow, cp.pink])), 60 | (15.5, dc.Move(0.0, 100)), 61 | (20.5, dc.Lights([cp.red, cp.yellow, cp.red, cp.yellow, cp.red, cp.yellow])), 62 | (20.5, dc.Move(-0.15, -70)), 63 | (21.0, dc.Lights([cp.yellow, cp.red, cp.yellow, cp.red, cp.yellow, cp.red])), 64 | (21.0, dc.Move(-0.15, 70)), 65 | (21.5, dc.Lights([cp.red, cp.yellow, cp.red, cp.yellow, cp.red, cp.yellow])), 66 | (21.5, dc.Move(0.15, -70)), 67 | (22.0, dc.Lights([cp.yellow, cp.red, cp.yellow, cp.red, cp.yellow, cp.red])), 68 | (22.0, dc.Move(0.15, 70)), 69 | (22.5, dc.FinishedDance()) 70 | ] 71 | 72 | dance_publisher = None 73 | try: 74 | dance_choreographer = dc.DanceChoreographer(DANCE_SEQUENCE) 75 | dance_publisher = dc.DanceCommandPublisher(dance_choreographer) 76 | rclpy.spin(dance_publisher) 77 | except dc.FinishedDance: 78 | print('Finished Dance') 79 | except KeyboardInterrupt: 80 | print('Caught keyboard interrupt') 81 | except BaseException: 82 | print('Exception in dance:', file=sys.stderr) 83 | finally: 84 | # Destroy the node explicitly 85 | # (optional - otherwise it will be done automatically 86 | # when the garbage collector destroys the node object) 87 | if dance_publisher: 88 | dance_publisher.destroy_node() 89 | rclpy.shutdown() 90 | 91 | 92 | if __name__ == '__main__': 93 | main() 94 | -------------------------------------------------------------------------------- /create3_lidar_slam/README.md: -------------------------------------------------------------------------------- 1 | # iRobot® Create® 3 LIDAR SLAM demo 2 | 3 | This example sets up LIDAR SLAM with a Create® 3 robot and Slamtec RPLIDAR spinning laser rangefinder. 4 | 5 | ## Parts List 6 | 7 | * Raspberry Pi® 4[^1] 8 | * USB-C® to USB-C® cable[^2] 9 | * Slamtec RPLidar A1M8 10 | * USB Micro B to USB A cable 11 | * 4x M2.5 x 8 flat head machine screw 12 | * 4x M2.5 x 6 self-tapping screw 13 | * 10x M3 x 10 self-tapping screw 14 | 15 | ## Setup 16 | 17 | ### Hardware Setup 18 | 19 | The files in this example assume the RPLIDAR is mounted 12 mm behind the center of rotation on the top of the Create 3 robot, in the arrangement shown below. 20 | The SLAM solver relies on a proper `tf` tree; if you wish to mount the sensor in another location, you will need to modify the parameters in the static transform publisher launched from `launch/sensors_launch.py`. 21 | 22 | Note that all STLs of brackets referenced in this example are found in our [create3_docs](https://github.com/iRobotEducation/create3_docs/tree/main/docs/hw/data/brackets) repository. 23 | 24 | ![Image of Create 3 showing setup and placement of sensors](https://iroboteducation.github.io/create3_docs/examples/data/create3_lidar_top.jpg) 25 | 26 | 1. Affix a LIDAR to your robot. 27 | STLs are available to mount an RPLidar A1M8 as well as its USB adapter. 28 | The LIDAR can be attached to the mounting plate using qty. 4 of M2.5 x 8 screws. 29 | The USB adapter can be attached to its mounting plate by heat staking (we used a soldering iron with an old tip in a well-ventilated space). 30 | Both mounting plates can be attached to the Create® 3 faceplate with M3 x 10 self-tapping screws. 31 | 32 | 1. Affix a single board computer (abbreviated as SBC for this guide) to your robot. 33 | In this example, we use a Raspberry Pi® 4, but other devices should work. 34 | STLs are available to mount a Raspberry Pi® to the Create® 3 robot. 35 | The Raspberry Pi® can be attached to the mounting plate using qty. 4 of M2.5 x 6 self-tapping screws. 36 | The mounting plate can be attached to the Create® 3 cargo bay with M3 x 10 self-tapping screws. 37 | 38 | 1. Connect the LIDAR to the SBC with a USB Micro B to USB A cable. 39 | Thread the cable through the passthrough in the top of the robot. 40 | 41 | 1. Connect the SBC to the Create® 3 adapter board with a USB-C® to USB-C® cable. 42 | 43 | ### SBC Setup 44 | 45 | On the SBC, clone and build the [create3_examples repository](https://github.com/iRobotEducation/create3_examples). 46 | 47 | Source the setup shell scripts in **EVERY** terminal you open: 48 | 49 | ```bash 50 | source ~/create3_examples_ws/install/local_setup.sh 51 | ``` 52 | 53 | Run the sensors launch script, which includes the LIDAR driver and transform from the laser scan to the robot: 54 | 55 | ```bash 56 | ros2 launch create3_lidar_slam sensors_launch.py 57 | ``` 58 | If your robot is using a namespace, you should add `namespace:='ROBOTNAMESPACE'` to the previous command, where `ROBOTNAMESPACE` is the namespace of your robot 59 | 60 | In a separate terminal run slam toolbox: 61 | 62 | ```bash 63 | ros2 launch create3_lidar_slam slam_toolbox_launch.py 64 | ``` 65 | If your robot is using a namespace, you should add `namespace:='ROBOTNAMESPACE'` to the previous command, where `ROBOTNAMESPACE` is the namespace of your robot. 66 | 67 | There may be some warnings and errors on startup, but the following message will be printed once everything is ready: 68 | 69 | ```bash 70 | [async_slam_toolbox_node-1] Registering sensor: [Custom Described Lidar] 71 | ``` 72 | 73 | In a third terminal, drive the robot around: 74 | 75 | ```bash 76 | ros2 run teleop_twist_keyboard teleop_twist_keyboard 77 | ``` 78 | If your robot is using a namespace, you should add `--ros-args -r __ns:=/ROBOTNAMESPACE` to the previous command, where `ROBOTNAMESPACE` is the namespace of your robot. 79 | 80 | ### Computer Setup 81 | 82 | Clone and build the [create3_examples repository](https://github.com/iRobotEducation/create3_examples). 83 | Then source the setup shell scripts. 84 | 85 | ``` 86 | source ~/create3_examples_ws/install/local_setup.sh 87 | ``` 88 | 89 | 90 | Run the rviz launch script, which launches rviz2 with the appropriate configuration: 91 | 92 | ```bash 93 | ros2 launch create3_lidar_slam rviz_launch.py 94 | ``` 95 | If your robot is using a namespace, you should add `namespace:='ROBOTNAMESPACE'` to the previous command, where `ROBOTNAMESPACE` is the namespace of your robot. 96 | 97 | The rviz2 configuration this command imports will configure rviz2 to subscribe to the laser, the occupancy map, and display the `base_footprint` tf frame that the laser is building off of from the map frame. 98 | 99 | ![Image of rviz with custom configuration](https://iroboteducation.github.io/create3_docs/examples/data/create3_lidar_rviz.png) 100 | 101 | 102 | 103 | 104 | ## Tips and Tricks 105 | 106 | * Limit rotation speed for best results. 107 | 108 | 109 | ### Troubleshooting 110 | 111 | * Ensure the robot, SBC, and computer are all on the same network, using the same middleware. 112 | * If using CycloneDDS, and you are using multiple network interfaces on either the SBC or the computer, be sure to set up your [XML profile(s)](https://iroboteducation.github.io/create3_docs/setup/xml-config/) properly. 113 | * Check your tf tree and make sure it has all the same transforms as [this sample tree](TF_Tree.pdf). 114 | 115 | [^1]: Raspberry Pi® is a trademark of Raspberry Pi Trading. 116 | [^2]: USB-C® is a trademark of USB Implementers Forum. 117 | [^3]: All other trademarks mentioned are the property of their respective owners. 118 | -------------------------------------------------------------------------------- /create3_examples_py/create3_examples_py/dance/dance_choreograph.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | import math 4 | 5 | import rclpy 6 | from rclpy.node import Node 7 | 8 | from rcl_interfaces.msg import Parameter 9 | from rcl_interfaces.msg import ParameterType 10 | from rcl_interfaces.msg import ParameterValue 11 | from rcl_interfaces.srv import SetParameters 12 | 13 | from geometry_msgs.msg import Twist 14 | from irobot_create_msgs.msg import LedColor 15 | from irobot_create_msgs.msg import LightringLeds 16 | 17 | class ColorPalette(): 18 | """ Helper Class to define frequently used colors""" 19 | def __init__(self): 20 | self.red = LedColor(red=255,green=0,blue=0) 21 | self.green = LedColor(red=0,green=255,blue=0) 22 | self.blue = LedColor(red=0,green=0,blue=255) 23 | self.yellow = LedColor(red=255,green=255,blue=0) 24 | self.pink = LedColor(red=255,green=0,blue=255) 25 | self.cyan = LedColor(red=0,green=255,blue=255) 26 | self.purple = LedColor(red=127,green=0,blue=255) 27 | self.white = LedColor(red=255,green=255,blue=255) 28 | self.grey = LedColor(red=189,green=189,blue=189) 29 | 30 | class Move(): 31 | """ Class to tell the robot to move as part of dance sequence""" 32 | def __init__(self, x_m_s, theta_degrees_second): 33 | """ 34 | Parameters 35 | ---------- 36 | x_m_s : float 37 | The speed to drive the robot forward (positive) /backwards (negative) in m/s 38 | theta_degrees_second : float 39 | The speed to rotate the robot counter clockwise (positive) / clockwise (negative) in deg/s 40 | """ 41 | self.x = x_m_s 42 | self.theta = math.radians(theta_degrees_second) 43 | 44 | class Lights(): 45 | """ Class to tell the robot to set lightring lights as part of dance sequence""" 46 | def __init__(self, led_colors): 47 | """ 48 | Parameters 49 | ---------- 50 | led_colors : list of LedColor 51 | The list of 6 LedColors corresponding to the 6 LED lights on the lightring 52 | """ 53 | self.led_colors = led_colors 54 | 55 | class FinishedDance(Exception): 56 | """ Class to tell the robot dance sequence has finished""" 57 | pass 58 | 59 | class DanceChoreographer(): 60 | """ Class to manage a dance sequence, returning current actions to perform""" 61 | def __init__(self, dance_sequence): 62 | ''' 63 | Parameters 64 | ---------- 65 | dance_sequence : list of (time, action) pairs 66 | The time is time since start_dance was called to initiate action, 67 | the action is one of the classes above [Move,Lights,FinishedDance] 68 | ''' 69 | self.dance_sequence = dance_sequence 70 | self.action_index = 0 71 | 72 | def start_dance(self, time): 73 | ''' 74 | Parameters 75 | ---------- 76 | time : rclpy::Time 77 | The ROS 2 time to mark the start of the sequence 78 | ''' 79 | self.start_time = time 80 | self.action_index = 0 81 | 82 | def get_next_actions(self, time): 83 | ''' 84 | Parameters 85 | ---------- 86 | time : rclpy::Time 87 | The ROS 2 time to compare against start time to give actions that should be applied given how much time sequence has been running for 88 | ''' 89 | time_into_dance = time - self.start_time 90 | time_into_dance_seconds = time_into_dance.nanoseconds / float(1e9) 91 | actions = [] 92 | while self.action_index < len(self.dance_sequence) and time_into_dance_seconds >= self.dance_sequence[self.action_index][0]: 93 | actions.append(self.dance_sequence[self.action_index][1]) 94 | self.action_index += 1 95 | return actions 96 | 97 | class DanceCommandPublisher(Node): 98 | """ Class to publish actions produced by the DanceChoreographer""" 99 | def __init__(self, dance_choreographer): 100 | ''' 101 | Parameters 102 | ---------- 103 | dance_choreographer : DanceChoreographer 104 | The configured DanceChoreographer to give time and query for actions to publish 105 | ''' 106 | super().__init__('dance_command_publisher') 107 | self.dance_choreographer = dance_choreographer 108 | self.lights_publisher = self.create_publisher(LightringLeds, 'cmd_lightring', 10) 109 | self.vel_publisher = self.create_publisher(Twist, 'cmd_vel', 10) 110 | timer_period = 0.05 # seconds 111 | self.timer = self.create_timer(timer_period, self.timer_callback) 112 | self.last_twist = Twist() 113 | self.last_lightring = LightringLeds() 114 | self.last_lightring.override_system = False 115 | self.ready = False 116 | self.wait_on_params = False 117 | self.last_wait_subscriber_printout = None 118 | self.finished = False 119 | self.params_cli = self.create_client(SetParameters, '/motion_control/set_parameters') 120 | while not self.params_cli.wait_for_service(timeout_sec=1.0): 121 | self.get_logger().info('service not available, waiting again...') 122 | 123 | def timer_callback(self): 124 | if self.finished: 125 | return 126 | current_time = self.get_clock().now() 127 | # Wait for subscribers and parameter set before starting dance 128 | if not self.ready: 129 | # Check if waiting on parameter set to return 130 | if self.wait_on_params: 131 | if self.params_future.done(): 132 | try: 133 | response = self.params_future.result() 134 | self.get_logger().info('Set Params Service response %r' % (response,)) 135 | except Exception as e: 136 | self.get_logger().info('Set Params Service call failed %r' % (e,)) 137 | else: 138 | self.get_logger().info('Wait on future') 139 | return 140 | # Finished trying to set parameters, start dance sequence 141 | self.ready = True 142 | self.get_logger().info('Finished params set, start dance at time %f' % (current_time.nanoseconds / float(1e9))) 143 | self.dance_choreographer.start_dance(current_time) 144 | # Check is subscribers are ready 145 | elif self.vel_publisher.get_subscription_count() > 0 and self.lights_publisher.get_subscription_count() > 0: 146 | self.get_logger().info('Subscribers connected, send safety_override param at time %f' % (current_time.nanoseconds / float(1e9))) 147 | self.send_params_request() 148 | self.wait_on_params = True 149 | return 150 | elif not self.last_wait_subscriber_printout or ((current_time - self.last_wait_subscriber_printout).nanoseconds / float(1e9)) > 5.0: 151 | # Only print once every 5 seconds 152 | self.last_wait_subscriber_printout = current_time 153 | self.get_logger().info('Waiting for publishers to connect to subscribers') 154 | return 155 | else: 156 | return 157 | # get actions from dance_choreographer given time 158 | next_actions = self.dance_choreographer.get_next_actions(current_time) 159 | twist = self.last_twist 160 | lightring = self.last_lightring 161 | for next_action in next_actions: 162 | if isinstance(next_action, Move): 163 | twist = Twist() 164 | twist.linear.x = next_action.x 165 | twist.angular.z = next_action.theta 166 | self.last_twist = twist 167 | self.get_logger().info('Time %f New move action: %f, %f' % (current_time.nanoseconds / float(1e9), twist.linear.x, twist.angular.z)) 168 | elif isinstance(next_action, Lights): 169 | lightring = LightringLeds() 170 | lightring.override_system = True 171 | lightring.leds = next_action.led_colors 172 | self.last_lightring = lightring 173 | self.get_logger().info('Time %f New lights action, first led (%d,%d,%d)' % (current_time.nanoseconds / float(1e9), lightring.leds[0].red, lightring.leds[0].green, lightring.leds[0].blue)) 174 | else: 175 | twist = Twist() 176 | twist.linear.x = 0.0 177 | twist.angular.z = 0.0 178 | self.last_twist = twist 179 | lightring = LightringLeds() 180 | lightring.override_system = False 181 | self.last_lightring = lightring 182 | self.finished = True 183 | self.get_logger().info('Time %f Finished Dance Sequence' % (current_time.nanoseconds / float(1e9))) 184 | raise FinishedDance 185 | 186 | lightring.header.stamp = current_time.to_msg() 187 | self.vel_publisher.publish(twist) 188 | self.lights_publisher.publish(lightring) 189 | 190 | # Set safety_override to backup_only so robot can backup during dance sequence 191 | def send_params_request(self): 192 | safety_override = ParameterValue(type=ParameterType.PARAMETER_STRING, string_value="backup_only") 193 | req = SetParameters.Request() 194 | req.parameters = [Parameter(name='safety_override', value=safety_override)] 195 | self.params_future = self.params_cli.call_async(req) 196 | -------------------------------------------------------------------------------- /create3_lidar_slam/rviz/create3_lidar_slam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | Splitter Ratio: 0.5 10 | Tree Height: 639 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | Visualization Manager: 25 | Class: "" 26 | Displays: 27 | - Alpha: 0.5 28 | Cell Size: 0.019999999552965164 29 | Class: rviz_default_plugins/Grid 30 | Color: 160; 160; 164 31 | Enabled: true 32 | Line Style: 33 | Line Width: 0.029999999329447746 34 | Value: Lines 35 | Name: Grid 36 | Normal Cell Count: 0 37 | Offset: 38 | X: 0 39 | Y: 0 40 | Z: 0 41 | Plane: XY 42 | Plane Cell Count: 100 43 | Reference Frame: 44 | Value: true 45 | - Class: rviz_default_plugins/TF 46 | Enabled: true 47 | Frame Timeout: 15 48 | Frames: 49 | All Enabled: false 50 | base_footprint: 51 | Value: true 52 | base_link: 53 | Value: false 54 | bump_front_center: 55 | Value: false 56 | bump_front_left: 57 | Value: false 58 | bump_front_right: 59 | Value: false 60 | bump_left: 61 | Value: false 62 | bump_right: 63 | Value: false 64 | button_1: 65 | Value: false 66 | button_2: 67 | Value: false 68 | button_power: 69 | Value: false 70 | cliff_front_left: 71 | Value: false 72 | cliff_front_right: 73 | Value: false 74 | cliff_side_left: 75 | Value: false 76 | cliff_side_right: 77 | Value: false 78 | ir_intensity_front_center_left: 79 | Value: false 80 | ir_intensity_front_center_right: 81 | Value: false 82 | ir_intensity_front_left: 83 | Value: false 84 | ir_intensity_front_right: 85 | Value: false 86 | ir_intensity_left: 87 | Value: false 88 | ir_intensity_right: 89 | Value: false 90 | ir_intensity_side_left: 91 | Value: false 92 | ir_omni: 93 | Value: false 94 | laser_frame: 95 | Value: false 96 | left_wheel: 97 | Value: false 98 | map: 99 | Value: false 100 | mouse: 101 | Value: false 102 | odom: 103 | Value: false 104 | right_wheel: 105 | Value: false 106 | Marker Scale: 1 107 | Name: TF 108 | Show Arrows: true 109 | Show Axes: true 110 | Show Names: false 111 | Tree: 112 | map: 113 | {} 114 | odom: 115 | base_footprint: 116 | laser_frame: 117 | {} 118 | base_link: 119 | bump_front_center: 120 | {} 121 | bump_front_left: 122 | {} 123 | bump_front_right: 124 | {} 125 | bump_left: 126 | {} 127 | bump_right: 128 | {} 129 | button_1: 130 | {} 131 | button_2: 132 | {} 133 | button_power: 134 | {} 135 | cliff_front_left: 136 | {} 137 | cliff_front_right: 138 | {} 139 | cliff_side_left: 140 | {} 141 | cliff_side_right: 142 | {} 143 | ir_intensity_front_center_left: 144 | {} 145 | ir_intensity_front_center_right: 146 | {} 147 | ir_intensity_front_left: 148 | {} 149 | ir_intensity_front_right: 150 | {} 151 | ir_intensity_left: 152 | {} 153 | ir_intensity_right: 154 | {} 155 | ir_intensity_side_left: 156 | {} 157 | ir_omni: 158 | {} 159 | left_wheel: 160 | {} 161 | mouse: 162 | {} 163 | right_wheel: 164 | {} 165 | Update Interval: 0 166 | Value: true 167 | - Alpha: 0.699999988079071 168 | Class: rviz_default_plugins/Map 169 | Color Scheme: map 170 | Draw Behind: false 171 | Enabled: true 172 | Name: Map 173 | Topic: 174 | Depth: 5 175 | Durability Policy: Volatile 176 | Filter size: 10 177 | History Policy: Keep Last 178 | Reliability Policy: Reliable 179 | Value: map 180 | Update Topic: 181 | Depth: 5 182 | Durability Policy: Volatile 183 | History Policy: Keep Last 184 | Reliability Policy: Reliable 185 | Value: map_updates 186 | Use Timestamp: false 187 | Value: true 188 | - Alpha: 1 189 | Autocompute Intensity Bounds: true 190 | Autocompute Value Bounds: 191 | Max Value: 10 192 | Min Value: -10 193 | Value: true 194 | Axis: Z 195 | Channel Name: intensity 196 | Class: rviz_default_plugins/LaserScan 197 | Color: 255; 255; 255 198 | Color Transformer: Intensity 199 | Decay Time: 0 200 | Enabled: true 201 | Invert Rainbow: false 202 | Max Color: 255; 255; 255 203 | Max Intensity: 47 204 | Min Color: 0; 0; 0 205 | Min Intensity: 47 206 | Name: LaserScan 207 | Position Transformer: XYZ 208 | Selectable: true 209 | Size (Pixels): 3 210 | Size (m): 0.009999999776482582 211 | Style: Flat Squares 212 | Topic: 213 | Depth: 5 214 | Durability Policy: Volatile 215 | Filter size: 10 216 | History Policy: Keep Last 217 | Reliability Policy: Reliable 218 | Value: scan 219 | Use Fixed Frame: true 220 | Use rainbow: true 221 | Value: true 222 | Enabled: true 223 | Global Options: 224 | Background Color: 48; 48; 48 225 | Fixed Frame: map 226 | Frame Rate: 5 227 | Name: root 228 | Tools: 229 | - Class: rviz_default_plugins/Interact 230 | Hide Inactive Objects: true 231 | - Class: rviz_default_plugins/MoveCamera 232 | - Class: rviz_default_plugins/Select 233 | - Class: rviz_default_plugins/FocusCamera 234 | - Class: rviz_default_plugins/Measure 235 | Line color: 128; 128; 0 236 | - Class: rviz_default_plugins/SetInitialPose 237 | Covariance x: 0.25 238 | Covariance y: 0.25 239 | Covariance yaw: 0.06853891909122467 240 | Topic: 241 | Depth: 5 242 | Durability Policy: Volatile 243 | History Policy: Keep Last 244 | Reliability Policy: Reliable 245 | Value: initialpose 246 | - Class: rviz_default_plugins/SetGoal 247 | Topic: 248 | Depth: 5 249 | Durability Policy: Volatile 250 | History Policy: Keep Last 251 | Reliability Policy: Reliable 252 | Value: goal_pose 253 | - Class: rviz_default_plugins/PublishPoint 254 | Single click: true 255 | Topic: 256 | Depth: 5 257 | Durability Policy: Volatile 258 | History Policy: Keep Last 259 | Reliability Policy: Reliable 260 | Value: clicked_point 261 | Transformation: 262 | Current: 263 | Class: rviz_default_plugins/TF 264 | Value: true 265 | Views: 266 | Current: 267 | Class: rviz_default_plugins/Orbit 268 | Distance: 7.917714595794678 269 | Enable Stereo Rendering: 270 | Stereo Eye Separation: 0.05999999865889549 271 | Stereo Focal Distance: 1 272 | Swap Stereo Eyes: false 273 | Value: false 274 | Focal Point: 275 | X: 0.17446282505989075 276 | Y: 0.11711662262678146 277 | Z: 0.35509517788887024 278 | Focal Shape Fixed Size: true 279 | Focal Shape Size: 0.05000000074505806 280 | Invert Z Axis: false 281 | Name: Current View 282 | Near Clip Distance: 0.009999999776482582 283 | Pitch: 0.514796793460846 284 | Target Frame: 285 | Value: Orbit (rviz) 286 | Yaw: 3.0386605262756348 287 | Saved: ~ 288 | Window Geometry: 289 | Displays: 290 | collapsed: false 291 | Height: 846 292 | Hide Left Dock: false 293 | Hide Right Dock: true 294 | QMainWindow State: 000000ff00000000fd0000000400000000000001e00000030afc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000030a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004d006f006e006f00380000000298000000990000000000000000fb0000000c004d006f006e006f0031003602000001bb00000167000002b600000242fb0000000a00440065007000740068020000009700000393000003c2000002e7fb0000000a004e006f00690073006502000001b60000018c000002d7000001c2fb0000000a0049006d0061006700650000000270000000c10000000000000000fb0000000a0049006d0061006700650000000270000000c10000000000000000000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002890000030a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 295 | Selection: 296 | collapsed: false 297 | Tool Properties: 298 | collapsed: false 299 | Views: 300 | collapsed: true 301 | Width: 1135 302 | X: 65 303 | Y: 79 -------------------------------------------------------------------------------- /create3_coverage/src/coverage_state_machine.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include 4 | #include 5 | 6 | #include "create3_coverage/coverage_state_machine.hpp" 7 | #include "tf2/utils.h" 8 | 9 | namespace create3_coverage { 10 | 11 | CoverageStateMachine::CoverageStateMachine( 12 | create3_examples_msgs::action::Coverage::Goal goal, 13 | rclcpp::Clock::SharedPtr clock, 14 | rclcpp::Logger logger, 15 | rclcpp_action::Client::SharedPtr dock_action_client, 16 | rclcpp_action::Client::SharedPtr undock_action_client, 17 | rclcpp::Publisher::SharedPtr cmd_vel_publisher, 18 | bool has_reflexes) 19 | : m_logger(logger) 20 | { 21 | m_goal = goal; 22 | 23 | m_clock = clock; 24 | m_start_time = m_clock->now(); 25 | m_has_reflexes = has_reflexes; 26 | 27 | m_dock_action_client = dock_action_client; 28 | m_undock_action_client = undock_action_client; 29 | m_cmd_vel_publisher = cmd_vel_publisher; 30 | 31 | m_undocking = false; 32 | m_preparing_spiral = false; 33 | } 34 | 35 | CoverageStateMachine::~CoverageStateMachine() 36 | { 37 | this->cancel(); 38 | } 39 | 40 | CoverageStateMachine::CoverageOutput CoverageStateMachine::execute(const Behavior::Data& data) 41 | { 42 | if (!m_current_behavior) { 43 | this->select_start_behavior(data); 44 | } else { 45 | this->select_next_behavior(data); 46 | } 47 | 48 | // Handle failure and success 49 | if (m_coverage_output.state != State::RUNNING) { 50 | return m_coverage_output; 51 | } 52 | 53 | m_behavior_state = m_current_behavior->execute(data); 54 | m_coverage_output.current_behavior = m_current_behavior->get_id(); 55 | 56 | return m_coverage_output; 57 | } 58 | 59 | void CoverageStateMachine::cancel() 60 | { 61 | if (m_current_behavior) { 62 | m_current_behavior->cleanup(); 63 | m_current_behavior.reset(); 64 | } 65 | } 66 | 67 | void CoverageStateMachine::select_start_behavior(const Behavior::Data& data) 68 | { 69 | if (data.dock.is_docked) { 70 | this->goto_undock(); 71 | } else { 72 | this->goto_spiral(SpiralBehavior::Config()); 73 | } 74 | } 75 | 76 | void CoverageStateMachine::select_next_behavior(const Behavior::Data& data) 77 | { 78 | // Keep going with the current behavior if it's still running 79 | if (m_behavior_state == State::RUNNING) { 80 | m_coverage_output.state = State::RUNNING; 81 | return; 82 | } 83 | 84 | // Check if it's time to wrap up the behavior 85 | bool explore_duration_elapsed = m_clock->now() - m_start_time >= m_goal.explore_duration; 86 | bool max_runtime_elapsed = m_clock->now() - m_start_time >= m_goal.max_runtime; 87 | if (max_runtime_elapsed) { 88 | m_coverage_output.state = State::SUCCESS; 89 | return; 90 | } 91 | if (m_current_behavior->get_id() != FeedbackMsg::DOCK && 92 | explore_duration_elapsed && data.dock.dock_visible) 93 | { 94 | this->goto_dock(); 95 | return; 96 | } 97 | 98 | switch (m_current_behavior->get_id()) 99 | { 100 | case FeedbackMsg::DOCK: 101 | { 102 | // A dock action should indicate the termination of the behavior. 103 | // Do not set a new behavior, either return SUCCESS or FAILURE. 104 | if (m_behavior_state == State::FAILURE || !data.dock.is_docked) { 105 | m_coverage_output.state = State::FAILURE; 106 | break; 107 | } 108 | m_coverage_output.state = State::SUCCESS; 109 | break; 110 | } 111 | case FeedbackMsg::DRIVE_STRAIGHT: 112 | { 113 | // If we just undocked, then we want to start with a spiral motion 114 | if (m_undocking) { 115 | m_undocking = false; 116 | this->goto_spiral(SpiralBehavior::Config()); 117 | break; 118 | } 119 | 120 | // If we were trying to get to spiral motion, do it only if drive straight succeeded (i.e. we moved enough from obstacle) 121 | if (m_preparing_spiral && m_behavior_state == State::SUCCESS) { 122 | m_preparing_spiral = false; 123 | this->goto_spiral(SpiralBehavior::Config()); 124 | break; 125 | } 126 | 127 | // Usually after a DRIVE_STRAIGHT go to ROTATE 128 | // If we failed previous drive straight, let's use a random angle 129 | auto rotate_config = RotateBehavior::Config(); 130 | if (m_behavior_state == State::FAILURE) { 131 | 132 | // Check if we failed too many times consecutively 133 | if (m_evade_attempts.size() > 20) { 134 | m_coverage_output.state = State::FAILURE; 135 | break; 136 | } 137 | 138 | constexpr double evade_resolution = 0.175433; // 10 degrees 139 | rotate_config.target_rotation = compute_evade_rotation(data.pose, evade_resolution); 140 | } else { 141 | m_evade_attempts.clear(); 142 | } 143 | rotate_config.robot_has_reflexes = m_has_reflexes; 144 | this->goto_rotate(rotate_config); 145 | break; 146 | } 147 | case FeedbackMsg::ROTATE: 148 | { 149 | // A rotate failure indicates that we haven't been able to clear hazards 150 | if (m_behavior_state == State::FAILURE) { 151 | m_coverage_output.state = State::FAILURE; 152 | break; 153 | } 154 | 155 | auto drive_config = DriveStraightBehavior::Config(); 156 | // Check if it's time to go back spiraling, if it's the case we will do only a short DRIVE_STRAIGHT to 157 | // move away from current obstacle (rather than driving forever until a new obstacle is hit). 158 | // Alternatively we will go into DRIVE_STRAIGHT forever. 159 | if (m_clock->now() - m_last_spiral_time >= rclcpp::Duration(std::chrono::seconds(60))) { 160 | drive_config.max_distance = 0.25; 161 | drive_config.min_distance = 0.25; 162 | m_preparing_spiral = true; 163 | } 164 | 165 | this->goto_drive_straight(drive_config); 166 | break; 167 | } 168 | case FeedbackMsg::SPIRAL: 169 | { 170 | if (m_behavior_state == State::SUCCESS) { 171 | this->goto_drive_straight(DriveStraightBehavior::Config()); 172 | } else { 173 | auto rotate_config = RotateBehavior::Config(); 174 | rotate_config.robot_has_reflexes = m_has_reflexes; 175 | this->goto_rotate(rotate_config); 176 | } 177 | break; 178 | } 179 | case FeedbackMsg::UNDOCK: 180 | { 181 | if (m_behavior_state == State::FAILURE || data.dock.is_docked) { 182 | m_coverage_output.state = State::FAILURE; 183 | break; 184 | } 185 | 186 | // After undocking, try to move away from the dock a little bit 187 | auto drive_config = DriveStraightBehavior::Config(); 188 | drive_config.max_distance = 0.25; 189 | drive_config.min_distance = 0.25; 190 | this->goto_drive_straight(drive_config); 191 | break; 192 | } 193 | } 194 | } 195 | 196 | double CoverageStateMachine::compute_evade_rotation(const geometry_msgs::msg::Pose& pose, double resolution) 197 | { 198 | tf2::Quaternion current_orientation; 199 | tf2::convert(pose.orientation, current_orientation); 200 | 201 | // Add current orientation to the list of failed attempts 202 | double current_yaw = tf2::getYaw(current_orientation); 203 | m_evade_attempts.push_back(current_yaw); 204 | 205 | tf2::Quaternion target_orientation; 206 | size_t i = 0; 207 | // We don't want this loop to search forever. 208 | // Eventually, if we failed too many times, return an orientation regardless of how different it is 209 | // from previous attempts. 210 | while (i < 100) { 211 | // Generate a new, random, target orientation 212 | double random_num = static_cast(rand()) / static_cast(RAND_MAX); 213 | double random_angle = random_num * 2 * M_PI - M_PI; 214 | target_orientation.setRPY(0.0, 0.0, random_angle); 215 | 216 | // Check if the random orientation is different enough from past evade attempts 217 | bool valid_target = true; 218 | for (double angle : m_evade_attempts) { 219 | tf2::Quaternion attempt_orientation; 220 | attempt_orientation.setRPY(0.0, 0.0, angle); 221 | 222 | tf2::Quaternion relative_orientation = target_orientation * attempt_orientation.inverse(); 223 | double relative_yaw = tf2::getYaw(relative_orientation); 224 | if (std::abs(relative_yaw) < std::abs(resolution)) { 225 | valid_target = false; 226 | break; 227 | } 228 | } 229 | 230 | // Exit as soon as we find a valid target orientation 231 | if (valid_target) { 232 | break; 233 | } 234 | i++; 235 | } 236 | 237 | tf2::Quaternion relative_orientation = target_orientation * current_orientation.inverse(); 238 | double relative_yaw_rotation = tf2::getYaw(relative_orientation); 239 | return relative_yaw_rotation; 240 | } 241 | 242 | void CoverageStateMachine::goto_dock() 243 | { 244 | m_current_behavior = std::make_unique(m_dock_action_client, m_logger); 245 | m_coverage_output.state = State::RUNNING; 246 | } 247 | 248 | void CoverageStateMachine::goto_drive_straight(const DriveStraightBehavior::Config& config) 249 | { 250 | m_current_behavior = std::make_shared(config, m_cmd_vel_publisher, m_logger, m_clock); 251 | m_coverage_output.state = State::RUNNING; 252 | } 253 | 254 | void CoverageStateMachine::goto_rotate(const RotateBehavior::Config& config) 255 | { 256 | m_current_behavior = std::make_shared(config, m_cmd_vel_publisher, m_logger, m_clock); 257 | m_coverage_output.state = State::RUNNING; 258 | } 259 | 260 | void CoverageStateMachine::goto_spiral(const SpiralBehavior::Config& config) 261 | { 262 | m_last_spiral_time = m_clock->now(); 263 | m_current_behavior = std::make_shared(config, m_cmd_vel_publisher, m_logger, m_clock); 264 | m_coverage_output.state = State::RUNNING; 265 | } 266 | 267 | void CoverageStateMachine::goto_undock() 268 | { 269 | m_undocking = true; 270 | m_current_behavior = std::make_unique(m_undock_action_client, m_logger); 271 | m_coverage_output.state = State::RUNNING; 272 | } 273 | 274 | } // namespace create3_coverage 275 | -------------------------------------------------------------------------------- /create3_republisher/README.md: -------------------------------------------------------------------------------- 1 | # create3_republisher 2 | 3 | ## What is this? 4 | 5 | This package contains a ROS 2 applications capable of republishing ROS 2 entities. 6 | It's designed to work with the iRobot Create 3 robot. 7 | 8 | This means that if the robot is publishing on topic `/my_robot/odom` this application can subscribe to this topic and republish its messages into a new namespace, e.g. `/my_republisher/odom`. 9 | This works also with publishers, services and actions. 10 | 11 | For example, with `/my_robot/cmd_vel` remapped to `/my_republisher/cmd_vel`, users can then send motion commands to `/my_republisher/cmd_vel` to move the robot around, or send a ROS 2 action goal to `/my_republisher/drive_distance`, etc.. 12 | 13 | ## Why using this? 14 | 15 | ROS 2 middlewares have a tendency to discover all the ROS 2 entities in the network and allocate resources for all of them, even if they don't need to communicate. 16 | This can cause performance issues due to excessive memory allocations, discovery traffic and double delivery of messages. 17 | This republisher can be used to get around this problem. 18 | 19 | For example, this occurs when having your Create 3 robot connected to a RaspberryPi (or other SBC), which runs applications made of a high number of ROS 2 entities or that deal with very big messages (e.g. the navigation stack, or rtabmap, etc). 20 | 21 | **IMPORTANT:** use this tool only if you experience problems in your setup. 22 | 23 | **NOTE:** Using this tool requires at least some understanding of how to configure ROS 2 application via DDS XML configuration files. 24 | 25 | ## Prerequisites 26 | 27 | - Your robot and other ROS 2 applications should be on ROS 2 Humble. 28 | 29 | - You should use Fast-DDS as your RMW on the robot and throughout all your applications. 30 | This approach is not RMW-specific, but the instructions and the provided DDS config file are. 31 | Use the webserver to check or modify the Create 3 robot RMW (see [webserver docs](https://iroboteducation.github.io/create3_docs/webserver/application/#application-configuration) for details) 32 | 33 | - Ensure that you can discover and communicate via ROS 2 between all your devices before starting. 34 | For example: 35 | - If using a RaspberryPi (or other SBC) try pinging the Create 3 robot via `usb0` interface with `ping 192.168.186.2` 36 | - Verify that `ros2 topic list` shows the Create 3 robot topics 37 | 38 | This procedure will use advanced communication configuration, and if stuff wasn't working before, it will be hard to debug it later. 39 | These instructions assume that you didn't need any custom DDS configuration to get communication working. 40 | If that's not the case, your custom configuration will likely need to be integrated with the provided DDS configuration files, which is not covered by this tutorial. 41 | 42 | ## Connecting the Create 3 Robot and an SBC via the republisher 43 | 44 | The following instructions will show how to isolate the robot to discover and communicate only with an entity, the republisher, and ignore all other processes running on your RaspberryPi (or other SBC). 45 | Other entities, e.g. your navigation application can then interact with the republisher (i.e. they can subscribe to Create 3 topics, and send requests) without "being discovered by it". 46 | This is the "default setup" for this republisher. 47 | The next sections will show examples on how to customize it for other common scenarios. 48 | 49 | 1. Build this repository on the RaspberryPi (or other SBC). 50 | For example: 51 | 52 | ```bash 53 | mkdir -p ~/ws/src 54 | cd ~/ws/src 55 | git clone https://github.com/iRobotEducation/create3_examples.git 56 | cd ~/ws 57 | colcon build --symlink-install 58 | ``` 59 | 60 | 1. Modify the Fast-DDS XML profile on the Create 3, through the webserver, to exactly match the [`fastdds-robot-passive-unicast.xml` profile](dds-config/fastdds-robot-passive-unicast.xml). 61 | See [webserver docs](https://iroboteducation.github.io/create3_docs/webserver/rmw-profile-override/) for details. 62 | 63 | 1. Launch the republisher app in a terminal on your RaspberryPi (or other SBC) using the provided [`fastdds-active-unicast.xml` profile](dds-config/fastdds-active-unicast.xml). 64 | For example: 65 | 66 | ```bash 67 | source ~/ws/install/setup.sh 68 | export RMW_IMPLEMENTATION=rmw_fastrtps_cpp 69 | export FASTRTPS_DEFAULT_PROFILES_FILE=~/ws/src/create3_examples/create3_republisher/dds-config/fastdds-active-unicast.xml 70 | ros2 daemon stop 71 | ros2 launch create3_republisher create3_republisher_launch.py 72 | ``` 73 | 74 | This command assumes that the Create 3 robot is using the default namespace and it will republish all Create 3 robot names into the `/repub` namespace. 75 | 76 | If your Create 3 robot is running with a namespace, for example `/my_robot`, and/or you want to change the republication namespace, for example `/my_repub`, you can do it via command line arguments. 77 | 78 | ```bash 79 | ros2 launch create3_republisher create3_republisher_launch.py republisher_ns:=/my_repub robot_ns:=/my_robot 80 | ``` 81 | 82 | **IMPORTANT: THE TWO NAMESPACES MUST BE DIFFERENT!!** 83 | 84 | **NOTE:** the list of entities that's republished is defined by the `params.yaml` file which you'll find at `~/ws/install/create3_republisher/share/create3_republisher/bringup/params.yaml`. 85 | Comment or uncomment entries in this file to customize it to your needs. 86 | 87 | 1. Run your other applicatios that want to communicate with Create 3 in another terminal of the RaspberryPi (or other SBC) using a passive unicast DDS configuration profile. 88 | For example: 89 | 90 | ```bash 91 | export RMW_IMPLEMENTATION=rmw_fastrtps_cpp 92 | export FASTRTPS_DEFAULT_PROFILES_FILE=~/ws/src/create3_examples/create3_republisher/dds-config/fastdds-passive-unicast.xml 93 | ros2 daemon stop 94 | ros2 topic echo /repub/tf 95 | ros2 action send_goal /repub/drive_distance irobot_create_msgs/action/DriveDistance "{distance: 0.5,max_translation_speed: 0.15}" 96 | ``` 97 | 98 | 1. **OPTIONAL: Connect your laptop and your RaspberryPi (or other SBC)** 99 | With the setup described so far, you should have your RaspberryPi and your Create 3 robot able to communicate via ROS 2. 100 | However, your laptop (or other devices in the network) won't be able to discover neither the robot nor the SBC. 101 | To enable this communication, which again will occurr through the republisher, you will need a DDS configuration file also on your laptop (or other device). 102 | - Get the Wi-Fi IP address of the RaspberryPi (that's the one you use to SSH into it). 103 | - Create a copy of the [`fastdds-active-unicast.xml` profile](dds-config/fastdds-active-unicast.xml), which will be referenced as `fastdds-laptop-unicast.xml` in the following steps. 104 | - Modify the new `fastdds-laptop-unicast.xml` file, replacing the IP address in the line `
192.168.186.2
` with the RaspberryPi IP address mentioned before. 105 | For example `
192.168.1.212
`. 106 | 107 | ## Connecting the Create 3 Robot and your laptop via the republisher 108 | 109 | If you don't have a RaspberryPi or other SBC, you can still use the republisher application on your laptop. 110 | Most of the instructions presented above remain unchaged, but you will not be able to use the provided `fastdds-active-unicast.xml` profile for the republisher process. 111 | This DDS profile contains hardcoded the `usb0` IP address of the Create 3 robot. 112 | You will need to modify it to use the Wi-Fi IP address of the Create 3 instead. 113 | - Get the Wi-Fi IP address of the Create 3 robot (this is the IP address you use to access the Create 3 webserver). 114 | - Create a copy of the [`fastdds-active-unicast.xml` profile](dds-config/fastdds-active-unicast.xml), which will be referenced as `fastdds-laptop-unicast.xml` in the following steps. 115 | - Modify the new `fastdds-laptop-unicast.xml` file, replacing the IP address in the line `
192.168.186.2
` with the Create 3 IP address mentioned before. 116 | For example `
192.168.1.103
`. 117 | 118 | Note: these changes to the `fastdds-active-unicast.xml` profile are very similar to what done in the previous instructions to allow the laptop to communicate with the RaspberryPi. 119 | The only, but fundamental, difference is that here we are using the Create 3 IP address, rather than the RaspberryPi IP address. 120 | 121 | ### Notes and Tips 122 | 123 | #### Only the republisher should connect with the robot 124 | 125 | **IMPORTANT: this republisher is powered by the concept of "unicast discovery" protocols. There must be only 1 DDS profile in your setup that references the Create 3 IP address and it must be used only by the republisher process. 126 | Keep this in mind if you need to do further modifications to the configs, besides what described in this page.** 127 | 128 | #### Ensure that your discovery info is up-to-date 129 | 130 | Run `ros2 daemon stop` whenever you modify DDS configuration file and/or environment variables. 131 | 132 | #### Why do I see the Create 3 names, but I can't communicate with them 133 | 134 | **NOTE:** having different applications run with different DDS configuration profiles (as it's done with the republisher) could mess up the ROS 2 daemon. 135 | Don't blindly trust the output of `ros2 topic list` and similar commands. 136 | Run with `--no-daemon` when possible and call `ros2 daemon stop` in every new terminal after setting the DDS configuration profiles. 137 | 138 | The "old" Create 3 entity names will not be usable anymore in this configuration. 139 | After running those steps, for example, you won't be able to subscribe to the robot tf topic via `ros2 topic echo /tf` but only via `ros2 topic echo /repub/tf`. 140 | 141 | Inspecting the ROS graph (e.g. via `ros2 topic list`) may seem to still show the robot's topics (e.g. you may see `/tf` being listed there), but you can notice that subscribing to those topics won't produce any data. 142 | Indeed, querying more information about the topics will show you that what you are seeing is a "subscriber" (that lives on the republisher) and not the robot's publisher. 143 | 144 | ``` 145 | pi@raspberrypi:~$ ros2 topic info -v /tf 146 | Type: tf2_msgs/msg/TFMessage 147 | 148 | Publisher count: 0 149 | 150 | Subscription count: 1 151 | 152 | Node name: create3_repub 153 | Node namespace: /repub 154 | Topic type: tf2_msgs/msg/TFMessage 155 | Endpoint type: SUBSCRIPTION 156 | GID: 01.0f.e8.52.41.09.d1.75.01.00.00.00.00.00.19.04.00.00.00.00.00.00.00.00 157 | QoS profile: 158 | Reliability: BEST_EFFORT 159 | History (Depth): UNKNOWN 160 | Durability: VOLATILE 161 | Lifespan: Infinite 162 | Deadline: Infinite 163 | Liveliness: AUTOMATIC 164 | Liveliness lease duration: Infinite 165 | ``` 166 | 167 | #### I have a navigation application with hardcoded Create 3 topic names and I don't want to change them 168 | 169 | If you have a ROS 2 application that has hardcoded Create 3 topics and action names (e.g. `/cmd_vel` or `/my_create3/dock`), you don't necessarily need to modify it to use the republisher. 170 | Just change the Create 3 namespace to "something else" and set the republisher namespace as the old robot's namespace. 171 | For example, if you robot has a namespace `/my_create3` you can: first of all change its namespace to `/my_robot` and then run the republisher as: 172 | 173 | ```bash 174 | ros2 launch create3_republisher create3_republisher_launch.py republisher_ns:=/my_create3 robot_ns:=/my_robot 175 | ``` 176 | 177 | This approach can be used also if the robot didn't have a namespace to start with! 178 | Assign it to a new namespace and then use `/` as `republisher_ns`. 179 | -------------------------------------------------------------------------------- /create3_coverage/src/create3_coverage_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | 3 | #include "create3_coverage/coverage_state_machine.hpp" 4 | #include "create3_coverage/create3_coverage_node.hpp" 5 | 6 | #include "behaviors/utils.hpp" 7 | 8 | using std::placeholders::_1; 9 | using std::placeholders::_2; 10 | 11 | namespace create3_coverage { 12 | 13 | Create3CoverageNode::Create3CoverageNode() 14 | : rclcpp::Node("create3_coverage") 15 | { 16 | m_coverage_action_server = rclcpp_action::create_server( 17 | this->get_node_base_interface(), 18 | this->get_node_clock_interface(), 19 | this->get_node_logging_interface(), 20 | this->get_node_waitables_interface(), 21 | "coverage", 22 | std::bind(&Create3CoverageNode::handle_goal, this, _1, _2), 23 | std::bind(&Create3CoverageNode::handle_cancel, this, _1), 24 | std::bind(&Create3CoverageNode::handle_accepted, this, _1)); 25 | 26 | m_dock_action_client = rclcpp_action::create_client( 27 | this->get_node_base_interface(), 28 | this->get_node_graph_interface(), 29 | this->get_node_logging_interface(), 30 | this->get_node_waitables_interface(), 31 | "dock"); 32 | 33 | m_undock_action_client = rclcpp_action::create_client( 34 | this->get_node_base_interface(), 35 | this->get_node_graph_interface(), 36 | this->get_node_logging_interface(), 37 | this->get_node_waitables_interface(), 38 | "undock"); 39 | 40 | m_cmd_vel_publisher = this->create_publisher("cmd_vel", 10); 41 | 42 | m_reflexes_param_client = std::make_shared( 43 | this->get_node_base_interface(), 44 | this->get_node_topics_interface(), 45 | this->get_node_graph_interface(), 46 | this->get_node_services_interface(), 47 | "motion_control", 48 | rmw_qos_profile_parameters); 49 | 50 | m_dock_subscription = this->create_subscription( 51 | "dock_status", 52 | rclcpp::SensorDataQoS(), 53 | std::bind(&Create3CoverageNode::dock_callback, this, _1)); 54 | 55 | m_hazards_subscription = this->create_subscription( 56 | "hazard_detection", 57 | rclcpp::SensorDataQoS(), 58 | std::bind(&Create3CoverageNode::hazards_callback, this, _1)); 59 | 60 | m_ir_opcode_subscription = this->create_subscription( 61 | "ir_opcode", 62 | rclcpp::SensorDataQoS(), 63 | std::bind(&Create3CoverageNode::ir_opcode_callback, this, _1)); 64 | 65 | m_odom_subscription = this->create_subscription( 66 | "odom", 67 | rclcpp::SensorDataQoS(), 68 | std::bind(&Create3CoverageNode::odom_callback, this, _1)); 69 | 70 | m_kidnap_subscription = this->create_subscription( 71 | "kidnap_status", 72 | rclcpp::SensorDataQoS(), 73 | std::bind(&Create3CoverageNode::kidnap_callback, this, _1)); 74 | 75 | m_rate_hz = this->declare_parameter("rate_hz", 30); 76 | m_opcodes_buffer_ms = this->declare_parameter("opcodes_buffer_ms", 200); 77 | 78 | m_dock_msgs_received = false; 79 | m_is_running = false; 80 | m_last_behavior = -1; 81 | m_last_opcodes_cleared_time = this->now(); 82 | RCLCPP_INFO(this->get_logger(), "Node created!"); 83 | } 84 | 85 | rclcpp_action::GoalResponse 86 | Create3CoverageNode::handle_goal( 87 | const rclcpp_action::GoalUUID& uuid, 88 | std::shared_ptr goal) 89 | { 90 | (void)uuid; 91 | (void)goal; 92 | 93 | if (!this->ready_to_start()) { 94 | RCLCPP_WARN(this->get_logger(), "Rejecting goal request: robot nodes have not been discovered yet"); 95 | return rclcpp_action::GoalResponse::REJECT; 96 | } 97 | 98 | bool is_kidnapped = false; 99 | { 100 | std::lock_guard guard(m_mutex); 101 | is_kidnapped = m_last_kidnap.is_kidnapped; 102 | } 103 | if (is_kidnapped) { 104 | RCLCPP_WARN(this->get_logger(), "Rejecting goal request: robot is currently kidnapped"); 105 | return rclcpp_action::GoalResponse::REJECT; 106 | } 107 | 108 | if (m_is_running.exchange(true)) { 109 | RCLCPP_WARN(this->get_logger(), "Rejecting goal request: can only handle 1 goal at the time"); 110 | return rclcpp_action::GoalResponse::REJECT; 111 | } 112 | 113 | RCLCPP_INFO(this->get_logger(), "Accepting goal request"); 114 | return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 115 | } 116 | 117 | rclcpp_action::CancelResponse 118 | Create3CoverageNode::handle_cancel( 119 | const std::shared_ptr goal_handle) 120 | { 121 | (void)goal_handle; 122 | 123 | RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); 124 | return rclcpp_action::CancelResponse::ACCEPT; 125 | } 126 | 127 | void Create3CoverageNode::handle_accepted(const std::shared_ptr goal_handle) 128 | { 129 | // this needs to return quickly to avoid blocking the executor, so spin up a new thread 130 | std::thread{std::bind(&Create3CoverageNode::execute, this, _1), goal_handle}.detach(); 131 | } 132 | 133 | void Create3CoverageNode::execute(const std::shared_ptr goal_handle) 134 | { 135 | RCLCPP_INFO(this->get_logger(), "Executing goal"); 136 | 137 | rclcpp::Rate loop_rate(m_rate_hz); 138 | const auto goal = goal_handle->get_goal(); 139 | auto start_time = this->now(); 140 | 141 | // Check if the robot has reflexes enabled or if we need to manually handle hazards 142 | bool robot_has_reflexes = this->reflexes_setup(); 143 | 144 | auto state_machine = std::make_unique( 145 | *goal, 146 | this->get_clock(), 147 | this->get_logger(), 148 | m_dock_action_client, 149 | m_undock_action_client, 150 | m_cmd_vel_publisher, 151 | robot_has_reflexes); 152 | 153 | CoverageStateMachine::CoverageOutput output; 154 | output.state = State::RUNNING; 155 | bool is_docked = false; 156 | bool is_kidnapped = false; 157 | do { 158 | 159 | Behavior::Data data; 160 | { 161 | std::lock_guard guard(m_mutex); 162 | 163 | data.hazards = m_last_hazards; 164 | data.dock = m_last_dock; 165 | data.pose = m_last_odom.pose.pose; 166 | data.opcodes = m_last_opcodes; 167 | 168 | if (this->now() - m_last_opcodes_cleared_time >= rclcpp::Duration(std::chrono::milliseconds(m_opcodes_buffer_ms))) { 169 | m_last_opcodes_cleared_time = this->now(); 170 | m_last_opcodes.clear(); 171 | } 172 | 173 | is_kidnapped = m_last_kidnap.is_kidnapped; 174 | is_docked = m_last_dock.is_docked; 175 | } 176 | 177 | // Check if there is a cancel request 178 | if (goal_handle->is_canceling()) { 179 | m_is_running = false; 180 | state_machine->cancel(); 181 | auto result = std::make_shared(); 182 | result->success = false; 183 | result->is_docked = is_docked; 184 | result->duration = this->now() - start_time; 185 | goal_handle->canceled(result); 186 | RCLCPP_INFO(this->get_logger(), "Goal canceled!"); 187 | return; 188 | } 189 | 190 | // Check if the robot is kidnapped 191 | if (is_kidnapped) { 192 | m_is_running = false; 193 | state_machine->cancel(); 194 | auto result = std::make_shared(); 195 | result->success = false; 196 | result->is_docked = is_docked; 197 | result->duration = this->now() - start_time; 198 | goal_handle->abort(result); 199 | RCLCPP_ERROR(this->get_logger(), "Aborting goal! Robot has been kidnapped!"); 200 | return; 201 | } 202 | 203 | // Run the state machine! 204 | output = state_machine->execute(data); 205 | if (m_last_behavior != output.current_behavior) { 206 | auto feedback = std::make_shared(); 207 | feedback->current_behavior = output.current_behavior; 208 | goal_handle->publish_feedback(feedback); 209 | m_last_behavior = output.current_behavior; 210 | } 211 | 212 | loop_rate.sleep(); 213 | } while (output.state == State::RUNNING && rclcpp::ok()); 214 | 215 | RCLCPP_INFO(this->get_logger(), "Coverage action terminated"); 216 | 217 | if (rclcpp::ok()) { 218 | m_is_running = false; 219 | auto result = std::make_shared(); 220 | result->success = (output.state == State::SUCCESS); 221 | result->is_docked = is_docked; 222 | result->duration = this->now() - start_time; 223 | if (result->success) { 224 | goal_handle->succeed(result); 225 | } else { 226 | goal_handle->abort(result); 227 | } 228 | } 229 | } 230 | 231 | bool Create3CoverageNode::reflexes_setup() 232 | { 233 | bool robot_has_reflexes = true; 234 | 235 | const std::vector param_names = { 236 | "reflexes.REFLEX_BUMP", 237 | "reflexes.REFLEX_CLIFF", 238 | "reflexes.REFLEX_WHEEL_DROP", 239 | "reflexes_enabled" 240 | }; 241 | 242 | // Check if reflexes are active 243 | auto get_params_future = m_reflexes_param_client->get_parameters(param_names); 244 | auto parameters = get_params_future.get(); 245 | bool all_enabled = true; 246 | bool all_disabled = true; 247 | for (const rclcpp::Parameter& param : parameters) { 248 | all_enabled = all_enabled && param.as_bool(); 249 | all_disabled = all_disabled && !param.as_bool(); 250 | } 251 | 252 | if (all_enabled) { 253 | robot_has_reflexes = true; 254 | RCLCPP_INFO(this->get_logger(), "Reflexes are enabled on the robot!"); 255 | } else if (all_disabled) { 256 | robot_has_reflexes = false; 257 | RCLCPP_INFO(this->get_logger(), "Reflexes are disabled on the robot!"); 258 | } else { 259 | // If some reflexes are enabled and some are disabled, activate them all. 260 | RCLCPP_WARN(this->get_logger(), "Some reflexes were disabled, activating all of them"); 261 | std::vector parameters; 262 | for (const std::string & name : param_names) { 263 | parameters.push_back(rclcpp::Parameter(name, true)); 264 | } 265 | auto set_params_future = m_reflexes_param_client->set_parameters(parameters); 266 | auto results = set_params_future.get(); 267 | bool success = true; 268 | for (const rcl_interfaces::msg::SetParametersResult& res : results) { 269 | success = success && res.successful; 270 | } 271 | 272 | if (!success) { 273 | throw std::runtime_error{"Unable to activate required parameters"}; 274 | } 275 | robot_has_reflexes = true; 276 | } 277 | 278 | return robot_has_reflexes; 279 | } 280 | 281 | bool Create3CoverageNode::ready_to_start() 282 | { 283 | 284 | if (m_dock_subscription->get_publisher_count() == 0 || 285 | m_hazards_subscription->get_publisher_count() == 0 || 286 | m_ir_opcode_subscription->get_publisher_count() == 0 || 287 | m_odom_subscription->get_publisher_count() == 0 || 288 | m_kidnap_subscription->get_publisher_count() == 0) 289 | { 290 | RCLCPP_WARN(this->get_logger(), "Some subscriptions haven't discovered their publishers yet"); 291 | return false; 292 | } 293 | 294 | if (m_cmd_vel_publisher->get_subscription_count() == 0) { 295 | RCLCPP_WARN(this->get_logger(), "Some publishers haven't discovered their subscriptions yet"); 296 | return false; 297 | } 298 | 299 | if (!m_reflexes_param_client->service_is_ready()) { 300 | RCLCPP_WARN(this->get_logger(), "Some parameters servers are not ready yet"); 301 | return false; 302 | } 303 | 304 | if (!m_dock_action_client->action_server_is_ready() || 305 | !m_undock_action_client->action_server_is_ready()) 306 | { 307 | RCLCPP_WARN(this->get_logger(), "Some actions servers are not ready yet"); 308 | return false; 309 | } 310 | 311 | // We must know if the robot is docked or not before starting the behavior 312 | if (!m_dock_msgs_received) { 313 | RCLCPP_WARN(this->get_logger(), "Didn't receive a dock message yet"); 314 | return false; 315 | } 316 | 317 | return true; 318 | } 319 | 320 | void Create3CoverageNode::dock_callback(DockMsg::ConstSharedPtr msg) 321 | { 322 | std::lock_guard guard(m_mutex); 323 | m_dock_msgs_received = true; 324 | m_last_dock = *msg; 325 | } 326 | 327 | void Create3CoverageNode::hazards_callback(HazardMsg::ConstSharedPtr msg) 328 | { 329 | std::lock_guard guard(m_mutex); 330 | m_last_hazards = *msg; 331 | } 332 | 333 | void Create3CoverageNode::ir_opcode_callback(OpCodeMsg::ConstSharedPtr msg) 334 | { 335 | std::lock_guard guard(m_mutex); 336 | m_last_opcodes.push_back(*msg); 337 | } 338 | 339 | void Create3CoverageNode::kidnap_callback(KidnapMsg::ConstSharedPtr msg) 340 | { 341 | std::lock_guard guard(m_mutex); 342 | m_last_kidnap = *msg; 343 | } 344 | 345 | void Create3CoverageNode::odom_callback(OdometryMsg::ConstSharedPtr msg) 346 | { 347 | std::lock_guard guard(m_mutex); 348 | m_last_odom = *msg; 349 | } 350 | 351 | } // namespace create3_coverage 352 | -------------------------------------------------------------------------------- /create3_republisher/republisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 iRobot Corporation. All Rights Reserved. 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "irobot_create_msgs/srv/e_stop.hpp" 10 | #include "irobot_create_msgs/srv/reset_pose.hpp" 11 | #include "irobot_create_msgs/srv/robot_power.hpp" 12 | 13 | #include "irobot_create_msgs/action/audio_note_sequence.hpp" 14 | #include "irobot_create_msgs/action/dock.hpp" 15 | #include "irobot_create_msgs/action/drive_arc.hpp" 16 | #include "irobot_create_msgs/action/drive_distance.hpp" 17 | #include "irobot_create_msgs/action/led_animation.hpp" 18 | #include "irobot_create_msgs/action/navigate_to_position.hpp" 19 | #include "irobot_create_msgs/action/rotate_angle.hpp" 20 | #include "irobot_create_msgs/action/undock.hpp" 21 | #include "irobot_create_msgs/action/wall_follow.hpp" 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "rclcpp_action/rclcpp_action.hpp" 25 | #include "rclcpp_action/create_client.hpp" 26 | #include "rclcpp_action/create_server.hpp" 27 | 28 | // Macros to reduce boilerplate 29 | #define TRY_SETUP_SERVICE(type_arg) setup_service_if_matches(#type_arg, client_topic, server_topic, type) 30 | #define TRY_SETUP_ACTION(type_arg) setup_action_if_matches(#type_arg, client_topic, server_topic, type) 31 | 32 | class RepublisherNode : public rclcpp::Node 33 | { 34 | public: 35 | RepublisherNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 36 | : rclcpp::Node("create3_repub", options) 37 | { 38 | auto verbose_log_period_sec = 39 | this->declare_parameter("verbose_log_period_sec", rclcpp::ParameterValue(-1)).get(); 40 | m_services_timeout_sec = 41 | this->declare_parameter("services_timeout_sec", rclcpp::ParameterValue(60)).get(); 42 | m_actions_timeout_sec = 43 | this->declare_parameter("actions_timeout_sec", rclcpp::ParameterValue(600)).get(); 44 | m_actions_period_ms = 45 | this->declare_parameter("actions_period_ms", rclcpp::ParameterValue(50)).get(); 46 | 47 | if (verbose_log_period_sec > 0) { 48 | m_log_timer = this->create_wall_timer(std::chrono::seconds(verbose_log_period_sec), [this]() { this->debug_log(); }); 49 | } 50 | 51 | const auto robot_namespace = this->get_robot_namespace(); 52 | RCLCPP_INFO( 53 | this->get_logger(), 54 | "Creating republisher node with namespace '%s' to interact with robot '%s'", 55 | this->get_namespace(), 56 | robot_namespace.c_str()); 57 | 58 | bool success = setup_republishers(robot_namespace); 59 | assert(success); 60 | 61 | RCLCPP_INFO(this->get_logger(), "Ready to go!"); 62 | } 63 | 64 | private: 65 | struct remapped_names_t 66 | { 67 | std::string robot_entity {}; 68 | std::string this_entity {}; 69 | }; 70 | 71 | remapped_names_t get_remapped_names(std::string robot_namespace, const std::string & relative_name) 72 | { 73 | if (relative_name.empty() || relative_name[0] == '/') { 74 | throw std::runtime_error("Invalid relative_name name: " + relative_name); 75 | } 76 | 77 | if (robot_namespace.back() != '/') { 78 | robot_namespace += '/'; 79 | } 80 | std::string this_namespace = this->get_namespace(); 81 | if (this_namespace.back() != '/') { 82 | this_namespace += '/'; 83 | } 84 | 85 | remapped_names_t remapped_names; 86 | remapped_names.robot_entity = robot_namespace + relative_name; 87 | remapped_names.this_entity = this_namespace + relative_name; 88 | return remapped_names; 89 | } 90 | 91 | bool setup_republishers(const std::string & robot_namespace) 92 | { 93 | auto robot_publications = get_entity_pairs("robot_publishers"); 94 | for (const auto & [topic, type] : robot_publications) { 95 | auto names = get_remapped_names(robot_namespace, topic); 96 | setup_topic_republisher(names.robot_entity, names.this_entity, type); 97 | } 98 | auto robot_subscriptions = get_entity_pairs("robot_subscriptions"); 99 | for (const auto & [topic, type] : robot_subscriptions) { 100 | auto names = get_remapped_names(robot_namespace, topic); 101 | setup_topic_republisher(names.this_entity, names.robot_entity, type); 102 | } 103 | auto robot_services = get_entity_pairs("robot_services"); 104 | for (const auto & [service, type] : robot_services) { 105 | auto names = get_remapped_names(robot_namespace, service); 106 | setup_service_republisher(names.robot_entity, names.this_entity, type); 107 | } 108 | auto robot_actions = get_entity_pairs("robot_actions"); 109 | for (const auto & [action, type] : robot_actions) { 110 | auto names = get_remapped_names(robot_namespace, action); 111 | setup_action_republisher(names.robot_entity, names.this_entity, type); 112 | } 113 | return true; 114 | } 115 | 116 | std::vector> get_entity_pairs(const std::string & param_name) 117 | { 118 | auto entity_list = 119 | this->declare_parameter( 120 | param_name, 121 | rclcpp::ParameterValue(std::vector())) 122 | .get>(); 123 | if (entity_list.size() % 2 != 0) { 124 | throw std::runtime_error( 125 | "Parameter must have an even number of elements: " + param_name + " found " + std::to_string(entity_list.size())); 126 | } 127 | std::vector> entity_pairs; 128 | // Use int iteration index to avoid wrap-around errors 129 | const int end_index = entity_list.size() - 1; 130 | for (int i = 0; i < end_index; i += 2) { 131 | entity_pairs.push_back(std::make_pair(entity_list[i], entity_list[i + 1])); 132 | } 133 | RCLCPP_INFO(this->get_logger(), "Found %ld entities for %s", entity_pairs.size(), param_name.c_str()); 134 | return entity_pairs; 135 | } 136 | 137 | void setup_topic_republisher( 138 | const std::string & subscribed_topic, 139 | const std::string & published_topic, 140 | const std::string & topic_type) 141 | { 142 | RCLCPP_INFO(this->get_logger(), "Subscribing to topic '%s' and republishing it as '%s' with type '%s'", 143 | subscribed_topic.c_str(), 144 | published_topic.c_str(), 145 | topic_type.c_str()); 146 | 147 | auto publisher = this->create_generic_publisher( 148 | published_topic, 149 | topic_type, 150 | rclcpp::QoS(1).durability(rclcpp::DurabilityPolicy::TransientLocal)); 151 | 152 | auto subscriber = this->create_generic_subscription( 153 | subscribed_topic, 154 | topic_type, 155 | rclcpp::QoS(1).durability(rclcpp::DurabilityPolicy::Volatile).reliability(rclcpp::ReliabilityPolicy::BestEffort), 156 | [this, publisher=publisher](std::shared_ptr message) { 157 | m_msg_counter++; 158 | ++m_topics_map[publisher->get_topic_name()]; 159 | publisher->publish(*message); 160 | } 161 | ); 162 | 163 | m_publishers.push_back(publisher); 164 | m_subscriptions.push_back(subscriber); 165 | } 166 | 167 | std::string cpp_type_to_name(const std::string & cpp_type) 168 | { 169 | // Converts "irobot_create_msgs::action::AudioNoteSequence" into "irobot_create_msgs/action/AudioNoteSequence" 170 | std::string type_name = cpp_type; 171 | const std::string to_replace = "::"; 172 | const std::string replacement = "/"; 173 | size_t pos = 0; 174 | while ((pos = type_name.find(to_replace, pos)) != std::string::npos) { 175 | type_name.replace(pos, to_replace.length(), replacement); 176 | pos += replacement.length(); 177 | } 178 | return type_name; 179 | } 180 | 181 | template 182 | bool setup_service_if_matches(const std::string & expected_type, const std::string & client_topic, const std::string & server_topic, const std::string & type) 183 | { 184 | if (cpp_type_to_name(expected_type) == type) { 185 | make_service_pair(client_topic, server_topic); 186 | return true; 187 | } 188 | return false; 189 | } 190 | 191 | void setup_service_republisher( 192 | const std::string & client_topic, 193 | const std::string & server_topic, 194 | const std::string & type) 195 | { 196 | RCLCPP_INFO(this->get_logger(), "Remapping robot service '%s' as '%s' with type '%s'", 197 | client_topic.c_str(), 198 | server_topic.c_str(), 199 | type.c_str()); 200 | 201 | bool setup_success = 202 | TRY_SETUP_SERVICE(irobot_create_msgs::srv::EStop) || 203 | TRY_SETUP_SERVICE(irobot_create_msgs::srv::ResetPose) || 204 | TRY_SETUP_SERVICE(irobot_create_msgs::srv::RobotPower); 205 | 206 | if (!setup_success) { 207 | throw std::runtime_error("Unrecognized service client " + client_topic); 208 | } 209 | } 210 | 211 | template 212 | bool setup_action_if_matches(const std::string & expected_type, const std::string & client_topic, const std::string & server_topic, const std::string & type) 213 | { 214 | if (cpp_type_to_name(expected_type) == type) { 215 | make_action_pair(client_topic, server_topic); 216 | return true; 217 | } 218 | return false; 219 | } 220 | 221 | void setup_action_republisher( 222 | const std::string & client_topic, 223 | const std::string & server_topic, 224 | const std::string & type) 225 | { 226 | RCLCPP_INFO(this->get_logger(), "Remapping robot action '%s' as '%s' with type '%s'", 227 | client_topic.c_str(), 228 | server_topic.c_str(), 229 | type.c_str()); 230 | 231 | bool setup_success = 232 | TRY_SETUP_ACTION(irobot_create_msgs::action::AudioNoteSequence) || 233 | TRY_SETUP_ACTION(irobot_create_msgs::action::Dock) || 234 | TRY_SETUP_ACTION(irobot_create_msgs::action::DriveArc) || 235 | TRY_SETUP_ACTION(irobot_create_msgs::action::DriveDistance) || 236 | TRY_SETUP_ACTION(irobot_create_msgs::action::LedAnimation) || 237 | TRY_SETUP_ACTION(irobot_create_msgs::action::NavigateToPosition) || 238 | TRY_SETUP_ACTION(irobot_create_msgs::action::RotateAngle) || 239 | TRY_SETUP_ACTION(irobot_create_msgs::action::Undock) || 240 | TRY_SETUP_ACTION(irobot_create_msgs::action::WallFollow); 241 | 242 | if (!setup_success) { 243 | throw std::runtime_error("Unrecognized action client " + client_topic); 244 | } 245 | } 246 | 247 | template 248 | void make_service_pair(const std::string & client_name, const std::string & server_name) 249 | { 250 | auto cb_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); 251 | auto executor = std::make_shared(); 252 | executor->add_callback_group(cb_group, this->get_node_base_interface()); 253 | 254 | // We should do some thread management here but it's not a big deal. 255 | // The app will not shutdown nicely. 256 | auto executor_thread = std::thread([executor=executor](){ 257 | executor->spin(); 258 | }); 259 | executor_thread.detach(); 260 | 261 | // IMPORTANT: the client stays in the default callback group; the new one is only for the server 262 | auto client = this->create_client(client_name, rmw_qos_profile_services_default, nullptr); 263 | auto server = this->create_service( 264 | server_name, 265 | [this, client=client](typename ServiceT::Request::SharedPtr req, typename ServiceT::Response::SharedPtr res) 266 | { 267 | // Forward the request through our client and block for a response 268 | RCLCPP_INFO(this->get_logger(), "Forwarding service request to %s", client->get_service_name()); 269 | auto future = client->async_send_request(req); 270 | if (future.wait_for(std::chrono::seconds(m_services_timeout_sec)) != std::future_status::ready) { 271 | RCLCPP_ERROR(this->get_logger(), "Timed out service %s", client->get_service_name()); 272 | return; 273 | } 274 | res = future.get(); 275 | RCLCPP_INFO(this->get_logger(), "Forwarding service response from %s", client->get_service_name()); 276 | }, 277 | rmw_qos_profile_services_default, 278 | cb_group); 279 | 280 | m_callback_groups.push_back(cb_group); 281 | m_clients.push_back(client); 282 | m_services.push_back(server); 283 | } 284 | 285 | template 286 | void make_action_pair(const std::string & client_name, const std::string & server_name) 287 | { 288 | auto action_timeout = std::chrono::seconds(m_actions_timeout_sec); 289 | auto action_period = std::chrono::milliseconds(m_actions_period_ms); 290 | auto client = rclcpp_action::create_client(this, client_name, nullptr); 291 | auto server = rclcpp_action::create_server( 292 | this, server_name, 293 | [client_name](const rclcpp_action::GoalUUID &, std::shared_ptr) 294 | { 295 | std::cerr<<"Received action request for " << client_name << std::endl; 296 | return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 297 | }, 298 | [client_name](std::shared_ptr>) 299 | { 300 | std::cerr<<"Received action cancel request for " << client_name << std::endl; 301 | return rclcpp_action::CancelResponse::ACCEPT; 302 | }, 303 | [action_timeout, action_period, client=client, client_name](std::shared_ptr> handle) 304 | { 305 | std::cerr<<"Forwarding action request to " << client_name << std::endl; 306 | auto action_thread = std::thread([action_timeout, action_period, client=client, user_goal_handle=handle, client_name]() 307 | { 308 | const auto user_goal = user_goal_handle->get_goal(); 309 | auto start_time = std::chrono::high_resolution_clock::now(); 310 | auto robot_goal_handle_fut = client->async_send_goal(*user_goal); 311 | while(true) { 312 | if (std::chrono::high_resolution_clock::now() - start_time >= action_timeout) { 313 | std::cerr << "WARNING: ROS 2 action " << client_name << " timed-out while it's still waiting for a goal handle from the robot" << std::endl; 314 | user_goal_handle->abort(std::make_shared()); 315 | return; 316 | } 317 | if (user_goal_handle->is_canceling()) { 318 | std::cerr << "ERROR: Cancelling ROS 2 action " << client_name << " while it's still waiting for a goal handle from the robot" << std::endl; 319 | user_goal_handle->canceled(std::make_shared()); 320 | return; 321 | } 322 | if (robot_goal_handle_fut.wait_for(std::chrono::seconds(0)) == std::future_status::ready) { 323 | break; 324 | } 325 | std::this_thread::sleep_for(action_period); 326 | } 327 | 328 | auto robot_goal_handle = robot_goal_handle_fut.get(); 329 | std::cerr << "Action request " << client_name << " received goal handle from the robot" << std::endl; 330 | auto result_fut = client->async_get_result(robot_goal_handle); 331 | while(true) { 332 | if (std::chrono::high_resolution_clock::now() - start_time >= action_timeout) { 333 | std::cerr << "WARNING: ROS 2 action " << client_name << " timed-out while running" << std::endl; 334 | client->async_cancel_goal(robot_goal_handle); 335 | user_goal_handle->abort(std::make_shared()); 336 | return; 337 | } 338 | if (user_goal_handle->is_canceling()) { 339 | std::cerr << "Cancelling ROS 2 action " << client_name << std::endl; 340 | client->async_cancel_goal(robot_goal_handle); 341 | user_goal_handle->canceled(std::make_shared()); 342 | return; 343 | } 344 | if (result_fut.wait_for(std::chrono::seconds(0)) == std::future_status::ready) { 345 | break; 346 | } 347 | std::this_thread::sleep_for(action_period); 348 | } 349 | auto wrapped_result = result_fut.get(); 350 | if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { 351 | std::cerr << "Action request " << client_name << " SUCCESS received from the robot" << std::endl; 352 | user_goal_handle->succeed(wrapped_result.result); 353 | } else { 354 | std::cerr << "Action request " << client_name << " ERROR " << static_cast(wrapped_result.code) << " received from the robot" << std::endl; 355 | user_goal_handle->abort(wrapped_result.result); 356 | } 357 | }); 358 | action_thread.detach(); 359 | } 360 | ); 361 | 362 | m_action_clients.push_back(client); 363 | m_action_servers.push_back(server); 364 | } 365 | 366 | std::string get_robot_namespace() 367 | { 368 | auto robot_namespace = 369 | this->declare_parameter("robot_namespace", rclcpp::ParameterValue("/")).get(); 370 | if (robot_namespace.empty()) { 371 | throw std::runtime_error("The 'robot_namespace' parameter can't be an empty string"); 372 | } 373 | if (robot_namespace == this->get_namespace()) { 374 | throw std::runtime_error("The republisher node must have a different namespace from the robot!"); 375 | } 376 | 377 | return robot_namespace; 378 | } 379 | 380 | void debug_log() 381 | { 382 | RCLCPP_INFO(this->get_logger(), "Total republished messages: %zu", m_msg_counter.load()); 383 | for (const auto & [key, value] : m_topics_map) { 384 | RCLCPP_INFO(this->get_logger(), " - Topic '%s' count: %zu", key.c_str(), value); 385 | } 386 | } 387 | 388 | std::vector> m_callback_groups; 389 | std::vector> m_subscriptions; 390 | std::vector> m_publishers; 391 | std::vector> m_clients; 392 | std::vector> m_services; 393 | std::vector> m_action_clients; 394 | std::vector> m_action_servers; 395 | 396 | int m_services_timeout_sec {60}; 397 | int m_actions_timeout_sec {600}; 398 | int m_actions_period_ms {50}; 399 | std::atomic m_msg_counter {0}; 400 | std::map m_topics_map; 401 | 402 | rclcpp::TimerBase::SharedPtr m_log_timer; 403 | }; 404 | 405 | int main(int argc, char ** argv) 406 | { 407 | rclcpp::init(argc, argv); 408 | 409 | rclcpp::NodeOptions options; 410 | auto node = std::make_shared(options); 411 | auto executor = std::make_shared(); 412 | executor->add_node(node); 413 | executor->spin(); 414 | 415 | rclcpp::shutdown(); 416 | 417 | return 0; 418 | } 419 | --------------------------------------------------------------------------------