├── .gitignore ├── .github ├── CODEOWNERS ├── ISSUE_TEMPLATE │ ├── BUG_REPORT.md │ └── FEATURE_REQUEST.md ├── workflows │ └── ci.yml └── PULL_REQUEST_TEMPLATE.md ├── irobot_create_gz ├── irobot_create_gz_plugins │ ├── README.md │ ├── Create3Hmi │ │ ├── Create3Hmi.config │ │ ├── images │ │ │ ├── Power.png │ │ │ ├── One Dot.png │ │ │ └── Two Dots.png │ │ ├── Create3Hmi.qrc │ │ ├── CMakeLists.txt │ │ ├── Create3Hmi.hh │ │ ├── Create3Hmi.cc │ │ └── Create3Hmi.qml │ ├── CMakeLists.txt │ ├── package.xml │ └── CHANGELOG.rst ├── irobot_create_gz_sim │ ├── CMakeLists.txt │ ├── package.xml │ └── CHANGELOG.rst ├── irobot_create_gz_toolbox │ ├── src │ │ ├── sensors │ │ │ ├── sensors_main.cpp │ │ │ ├── sensors_node.cpp │ │ │ ├── mouse.cpp │ │ │ ├── cliff.cpp │ │ │ ├── wheel_drop.cpp │ │ │ ├── ir_intensity.cpp │ │ │ └── bumper.cpp │ │ ├── pose_republisher │ │ │ └── pose_republisher_main.cpp │ │ └── interface_buttons │ │ │ ├── interface_buttons_main.cpp │ │ │ └── interface_buttons_node.cpp │ ├── include │ │ └── irobot_create_gz_toolbox │ │ │ ├── sensors │ │ │ ├── ir_intensity.hpp │ │ │ ├── cliff.hpp │ │ │ ├── mouse.hpp │ │ │ ├── wheel_drop.hpp │ │ │ ├── sensors_node.hpp │ │ │ ├── bumper.hpp │ │ │ └── ir_opcode.hpp │ │ │ ├── interface_buttons │ │ │ └── interface_buttons_node.hpp │ │ │ ├── pose_republisher │ │ │ └── pose_republisher.hpp │ │ │ └── utils.hpp │ ├── package.xml │ ├── CMakeLists.txt │ └── CHANGELOG.rst └── irobot_create_gz_bringup │ ├── config │ ├── pose_republisher_params.yaml │ └── sensors_params.yaml │ ├── CMakeLists.txt │ ├── README.md │ ├── worlds │ └── depot.sdf │ ├── package.xml │ ├── launch │ ├── create3_gz_nodes.launch.py │ ├── create3_gz.launch.py │ └── sim.launch.py │ └── gui │ └── create3 │ └── gui.config ├── irobot_create_common ├── irobot_create_common_bringup │ ├── irobot_create_common_bringup │ │ ├── __init__.py │ │ ├── namespace.py │ │ └── offset.py │ ├── rviz │ │ └── irobot_logo.jpg │ ├── config │ │ ├── mock_params.yaml │ │ ├── kidnap_estimator_params.yaml │ │ ├── ui_mgr_params.yaml │ │ ├── wheel_status_params.yaml │ │ ├── ir_intensity_vector_params.yaml │ │ ├── hazard_vector_params.yaml │ │ └── robot_state_params.yaml │ ├── CMakeLists.txt │ ├── launch │ │ ├── rviz2.launch.py │ │ ├── robot_description.launch.py │ │ └── dock_description.launch.py │ └── package.xml ├── irobot_create_toolbox │ ├── README.md │ ├── src │ │ └── polar_coordinates.cpp │ ├── include │ │ └── irobot_create_toolbox │ │ │ ├── polar_coordinates.hpp │ │ │ ├── sensors │ │ │ └── bumpers.hpp │ │ │ └── math.hpp │ ├── package.xml │ ├── CMakeLists.txt │ └── CHANGELOG.rst ├── irobot_create_nodes │ ├── README.md │ ├── include │ │ └── irobot_create_nodes │ │ │ ├── mock_publisher.hpp │ │ │ ├── hazards_vector_publisher.hpp │ │ │ ├── kidnap_estimator_publisher.hpp │ │ │ ├── ir_intensity_vector_publisher.hpp │ │ │ ├── wheels_publisher.hpp │ │ │ ├── motion_control │ │ │ ├── behaviors_scheduler.hpp │ │ │ └── wall_follow_behavior.hpp │ │ │ ├── robot_state.hpp │ │ │ └── ui_mgr.hpp │ ├── package.xml │ ├── src │ │ ├── mock_publisher.cpp │ │ ├── hazards_vector_publisher.cpp │ │ ├── ir_intensity_vector_publisher.cpp │ │ └── kidnap_estimator_publisher.cpp │ └── CHANGELOG.rst ├── irobot_create_control │ ├── CMakeLists.txt │ ├── package.xml │ ├── config │ │ └── control.yaml │ └── launch │ │ └── include │ │ └── control.py └── irobot_create_description │ ├── CMakeLists.txt │ ├── urdf │ ├── button.urdf.xacro │ ├── wheel_with_wheeldrop.urdf.xacro │ ├── sensors │ │ ├── optical_mouse.urdf.xacro │ │ ├── ir_intensity.urdf.xacro │ │ ├── cliff_sensor.urdf.xacro │ │ ├── imu.urdf.xacro │ │ └── ir_opcode_receivers.urdf.xacro │ ├── caster.urdf.xacro │ ├── dock │ │ ├── ir_emitter.urdf.xacro │ │ └── standard_dock.urdf.xacro │ ├── wheel.urdf.xacro │ ├── wheel_drop.urdf.xacro │ └── bumper.urdf.xacro │ ├── package.xml │ └── CHANGELOG.rst ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | __pycache__ 3 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @alsora @justinIRBT 2 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/README.md: -------------------------------------------------------------------------------- 1 | # turtlebot4_gz_gui_plugins 2 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.config: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/README.md: -------------------------------------------------------------------------------- 1 | # iRobot Create3 Toolbox 2 | 3 | Utilities and tools for building Create 3 nodes. 4 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(irobot_create_gz_sim) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() 5 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/rviz/irobot_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iRobotEducation/create3_sim/HEAD/irobot_create_common/irobot_create_common_bringup/rviz/irobot_logo.jpg -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Power.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iRobotEducation/create3_sim/HEAD/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Power.png -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/One Dot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iRobotEducation/create3_sim/HEAD/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/One Dot.png -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Two Dots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iRobotEducation/create3_sim/HEAD/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Two Dots.png -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | mock_publisher: 4 | ros__parameters: 5 | # Mock slip status publisher topic 6 | slip_status_topic: slip_status 7 | # Publishers rate 8 | slip_status_publish_rate: 62.0 9 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/README.md: -------------------------------------------------------------------------------- 1 | # iRobot® Create® 3 Nodes 2 | 3 | This package contains a set of nodes used for simulating a iRobot® Create® 3 robot. 4 | Each node is defined in its own shared library and registered using `rclcpp_components` to allow run-time composition in applications. 5 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | kidnap_estimator_publisher: 4 | ros__parameters: 5 | # Kidnap status publisher topic 6 | kidnap_status_topic: kidnap_status 7 | # Subscription topics 8 | hazard_topic: hazard_detection 9 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | Create3Hmi.qml 4 | images/One Dot.png 5 | images/Power.png 6 | images/Two Dots.png 7 | 8 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | ui_mgr: 4 | ros__parameters: 5 | # Buttons publisher topic 6 | button_topic: interface_buttons 7 | # Publishers rate 8 | buttons_publish_rate: 1.0 9 | # Subscription topics 10 | lightring_topic: cmd_lightring 11 | audio_topic: cmd_audio 12 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(irobot_create_control) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | install( 8 | DIRECTORY 9 | launch 10 | config 11 | DESTINATION 12 | share/${PROJECT_NAME}/ 13 | ) 14 | 15 | if(BUILD_TESTING) 16 | find_package(ament_lint_auto REQUIRED) 17 | ament_lint_auto_find_test_dependencies() 18 | endif() 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(irobot_create_description) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | install( 8 | DIRECTORY 9 | meshes 10 | urdf 11 | DESTINATION 12 | share/${PROJECT_NAME}/ 13 | ) 14 | 15 | if(BUILD_TESTING) 16 | find_package(ament_lint_auto REQUIRED) 17 | ament_lint_auto_find_test_dependencies() 18 | endif() 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/button.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | wheel_status_publisher: 4 | ros__parameters: 5 | # Publish rate 6 | publish_rate: 62.0 7 | # Encoder resolution 8 | encoder_resolution: 508.8 9 | # Wheel radius 10 | wheel_radius: 0.03575 11 | # Wheels angular velocity topic 12 | velocity_topic: wheel_vels 13 | # Wheels' net encoder ticks topic 14 | ticks_topic: wheel_ticks 15 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include 7 | 8 | #include "irobot_create_gz_toolbox/sensors/sensors_node.hpp" 9 | 10 | int main(int argc, char * argv[]) 11 | { 12 | rclcpp::init(argc, argv); 13 | rclcpp::spin(std::make_shared()); 14 | rclcpp::shutdown(); 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/pose_republisher/pose_republisher_main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include "irobot_create_gz_toolbox/pose_republisher/pose_republisher.hpp" 7 | #include 8 | 9 | int main(int argc, char * argv[]) 10 | { 11 | rclcpp::init(argc, argv); 12 | rclcpp::spin(std::make_shared()); 13 | rclcpp::shutdown(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include "irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp" 7 | #include 8 | 9 | int main(int argc, char * argv[]) 10 | { 11 | rclcpp::init(argc, argv); 12 | rclcpp::spin(std::make_shared()); 13 | rclcpp::shutdown(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/config/pose_republisher_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | pose_republisher_node: 4 | ros__parameters: 5 | robot_publisher_topic: sim_ground_truth_pose 6 | robot_subscriber_topic: _internal/sim_ground_truth_pose 7 | mouse_publisher_topic: _internal/sim_ground_truth_mouse_pose 8 | dock_subscriber_topic: _internal/sim_ground_truth_dock_pose 9 | dock_publisher_topic: sim_ground_truth_dock_pose 10 | ir_emitter_publisher_topic: _internal/sim_ground_truth_ir_emitter_pose 11 | ir_receiver_publisher_topic: _internal/sim_ground_truth_ir_receiver_pose -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(irobot_create_gz_plugins) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | 11 | set(OpenGL_GL_PREFERENCE LEGACY) 12 | 13 | # Find the Ignition gui library 14 | find_package(gz_gui_vendor REQUIRED) 15 | add_subdirectory(Create3Hmi) 16 | 17 | if(BUILD_TESTING) 18 | find_package(ament_lint_auto REQUIRED) 19 | ament_lint_auto_find_test_dependencies() 20 | endif() 21 | 22 | ament_package() 23 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | ir_intensity_vector_publisher: 4 | ros__parameters: 5 | # IR intensity publisher topic 6 | publisher_topic: ir_intensity 7 | publish_rate: 62.0 8 | subscription_topics: 9 | # IR intensity topics 10 | - _internal/ir_intensity_front_center_left 11 | - _internal/ir_intensity_front_center_right 12 | - _internal/ir_intensity_front_left 13 | - _internal/ir_intensity_front_right 14 | - _internal/ir_intensity_left 15 | - _internal/ir_intensity_right 16 | - _internal/ir_intensity_side_left 17 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/BUG_REPORT.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Additional context** 27 | Add any other context about the problem here. 28 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/FEATURE_REQUEST.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: enhancement 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: Testing 2 | 3 | on: 4 | pull_request: 5 | push: 6 | branches: 7 | - main 8 | workflow_dispatch: 9 | 10 | jobs: 11 | build_and_test: 12 | name: build_and_test 13 | runs-on: ubuntu-24.04 14 | steps: 15 | - name: Set Ignition Version 16 | run: | 17 | echo "GZ_VERSION=harmonic" >> $GITHUB_ENV 18 | - uses: actions/checkout@v2.3.4 19 | - uses: ros-tooling/setup-ros@0.7.15 20 | with: 21 | required-ros-distributions: jazzy 22 | - uses: ros-tooling/action-ros-ci@0.4.4 23 | id: action_ros_ci_step 24 | with: 25 | target-ros2-distro: jazzy 26 | import-token: ${{ secrets.REPO_TOKEN }} 27 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | hazards_vector_publisher: 4 | ros__parameters: 5 | # Hazard detection publisher topic 6 | publisher_topic: hazard_detection 7 | publish_rate: 62.0 8 | subscription_topics: 9 | # Bumper topic 10 | - _internal/bumper/event 11 | # Cliff topics 12 | - _internal/cliff_front_left/event 13 | - _internal/cliff_front_right/event 14 | - _internal/cliff_side_left/event 15 | - _internal/cliff_side_right/event 16 | # Wheel drop topics 17 | - _internal/wheel_drop/left_wheel/event 18 | - _internal/wheel_drop/right_wheel/event 19 | # Backup limit topic 20 | - _internal/backup_limit 21 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/src/polar_coordinates.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 iRobot Corporation. All Rights Reserved. 2 | // @author Alberto Soragna (asoragna@irobot.com) 3 | 4 | #include 5 | 6 | #include "gz/math/Vector2.hh" 7 | #include "irobot_create_toolbox/polar_coordinates.hpp" 8 | 9 | namespace irobot_create_toolbox 10 | { 11 | 12 | PolarCoordinate toPolar(const gz::math::Vector2d & cartesian) 13 | { 14 | return PolarCoordinate{cartesian.Length(), atan2(cartesian.Y(), cartesian.X())}; 15 | } 16 | 17 | gz::math::Vector2d fromPolar(const PolarCoordinate & polar) 18 | { 19 | gz::math::Vector2d cartesian{ 20 | polar.radius * cos(polar.azimuth), polar.radius * sin(polar.azimuth)}; 21 | return cartesian; 22 | } 23 | 24 | } // namespace irobot_create_toolbox 25 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(irobot_create_gz_bringup) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ros_gz_interfaces QUIET) 11 | 12 | if(NOT ros_gz_interfaces_FOUND) 13 | message(WARNING "ros_gz_interfaces not found. Skipping ${PROJECT_NAME} package.") 14 | ament_package() 15 | return() 16 | endif() 17 | 18 | install( 19 | DIRECTORY gui launch worlds config 20 | DESTINATION share/${PROJECT_NAME} 21 | ) 22 | 23 | if(BUILD_TESTING) 24 | find_package(ament_lint_auto REQUIRED) 25 | ament_lint_auto_find_test_dependencies() 26 | endif() 27 | 28 | ament_package() 29 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/include/irobot_create_toolbox/polar_coordinates.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 iRobot Corporation. All Rights Reserved. 2 | // @author Alberto Soragna (asoragna@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_TOOLBOX__POLAR_COORDINATES_HPP_ 5 | #define IROBOT_CREATE_TOOLBOX__POLAR_COORDINATES_HPP_ 6 | 7 | #include "gz/math/Vector2.hh" 8 | 9 | namespace irobot_create_toolbox 10 | { 11 | 12 | // Very useful Polar Coordinates tools when working with Fields of views and distances. 13 | struct PolarCoordinate 14 | { 15 | double radius; 16 | double azimuth; 17 | }; 18 | 19 | PolarCoordinate toPolar(const gz::math::Vector2d & cartesian); 20 | 21 | gz::math::Vector2d fromPolar(const PolarCoordinate & polar); 22 | 23 | } // namespace irobot_create_toolbox 24 | 25 | #endif // IROBOT_CREATE_TOOLBOX__POLAR_COORDINATES_HPP_ 26 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include 7 | 8 | #include "irobot_create_gz_toolbox/sensors/sensors_node.hpp" 9 | 10 | using irobot_create_gz_toolbox::SensorsNode; 11 | 12 | SensorsNode::SensorsNode() 13 | : rclcpp::Node("sensors_node") 14 | { 15 | // Create node handle 16 | nh_ = std::shared_ptr(this, [](rclcpp::Node *) {}); 17 | 18 | // Add sensors 19 | bumper_ = std::make_unique(nh_); 20 | cliff_ = std::make_unique(nh_); 21 | ir_intensity_ = std::make_unique(nh_); 22 | mouse_ = std::make_unique(nh_); 23 | wheel_drop_ = std::make_unique(nh_); 24 | ir_opcode_ = std::make_unique(nh_); 25 | } 26 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(irobot_create_common_bringup) 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(ament_cmake_python REQUIRED) 15 | 16 | install( 17 | DIRECTORY 18 | config 19 | launch 20 | rviz 21 | DESTINATION 22 | share/${PROJECT_NAME} 23 | ) 24 | 25 | # Install Python modules 26 | ament_python_install_package(${PROJECT_NAME}) 27 | 28 | if(BUILD_TESTING) 29 | find_package(ament_lint_auto REQUIRED) 30 | ament_lint_auto_find_test_dependencies() 31 | endif() 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/README.md: -------------------------------------------------------------------------------- 1 | # turtlebot4_gz 2 | 3 | To launch the simulation, run 4 | ```bash 5 | ros2 launch irobot_create_gz_bringup create3_gz.launch.py 6 | ``` 7 | 8 | Wait for the simulation environment to load completely, then press the orange `play` button in the 9 | lower-left corner to start the simulation. The robot starts docked on its charger. 10 | 11 | ## Worlds 12 | 13 | The default simulation world is the `depot` environment. To chage worlds, use the `world` 14 | argument. Supported worlds are: 15 | - `depot` (default) 16 | - `maze` 17 | 18 | ## Robot spawn location 19 | 20 | By default the robot spawns at the origin. To change the robot's spawn location, use the 21 | `x`, `y`, `z` and `yaw` arguments. Note that changing `z` may result in the robot spawning 22 | in the air or below the ground plane, depending in the value used. -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | robot_state: 4 | ros__parameters: 5 | # Stop status publisher topic 6 | stop_status_topic: stop_status 7 | # Mock battery state publisher topic 8 | battery_state_topic: battery_state 9 | # Publishers rate 10 | kidnap_status_publish_rate: 1.0 11 | stop_status_publish_rate: 62.0 12 | battery_state_publish_rate: 0.1 13 | # Subscription topics 14 | dock_topic: dock_status 15 | wheel_vels_topic: odom 16 | # Stop status position difference tolerance 17 | linear_velocity_tolerance: 0.01 18 | angular_velocity_tolerance: 0.1 19 | # Battery parameters 20 | full_charge_percentage: 1.0 21 | battery_high_percentage: 0.9 22 | # Dock Parameters 23 | undocked_charge_limit: 0.03 24 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_gz_sim 5 | 3.0.4 6 | Metapackage for the iRobot(R) Create(R) 3 robot Ignition simulator 7 | 8 | rkreinin 9 | 10 | BSD 11 | 12 | iRobot 13 | 14 | ament_cmake 15 | 16 | irobot_create_gz_bringup 17 | irobot_create_gz_plugins 18 | irobot_create_gz_toolbox 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | ## Description 2 | 3 | Please include a summary of the change and which issue is fixed. Also include relevant motivation and context. 4 | 5 | Fixes # (issue). 6 | 7 | ## Type of change 8 | 9 | - [ ] Bug fix (non-breaking change which fixes an issue) 10 | - [ ] New feature (non-breaking change which adds functionality) 11 | 12 | ## How Has This Been Tested? 13 | 14 | Please describe the tests that you ran to verify your changes. 15 | Provide instructions so we can reproduce. Also list any relevant details for your test configuration. 16 | 17 | ```bash 18 | # Run this command 19 | ros2 launch package launch.py 20 | ``` 21 | 22 | ## Checklist 23 | 24 | - [ ] I have performed a self-review of my own code 25 | - [ ] I have commented my code, particularly in hard-to-understand areas 26 | - [ ] I have made corresponding changes to the documentation 27 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/wheel_with_wheeldrop.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/worlds/depot.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | false 6 | 7 | 8 | 0.003 9 | 1.0 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Depot 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_description 5 | 3.0.4 6 | Provides the model description for the iRobot(R) Create(R) 3 Educational Robot. 7 | Ekumen 8 | BSD 9 | 10 | iRobot 11 | 12 | ament_cmake 13 | 14 | irobot_create_control 15 | urdf 16 | xacro 17 | 18 | ament_lint_auto 19 | ament_cmake_lint_cmake 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_gz_plugins 5 | 3.0.4 6 | Ignition plugins for simulated iRobot(R) Create(R) 3 Educational Robot. 7 | rkreinin 8 | BSD 9 | 10 | ament_cmake 11 | 12 | gz_gui_vendor 13 | ros_gz 14 | 15 | ament_lint_auto 16 | ament_cmake_cppcheck 17 | ament_cmake_cpplint 18 | ament_cmake_flake8 19 | ament_cmake_lint_cmake 20 | ament_cmake_pep257 21 | ament_cmake_xmllint 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_toolbox 5 | 3.0.4 6 | Components and helpers for the iRobot(R) Create(R) 3 Educational Robot. 7 | Ekumen 8 | BSD 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | gz_math_vendor 14 | 15 | ament_lint_auto 16 | ament_cmake_cppcheck 17 | ament_cmake_cpplint 18 | ament_cmake_flake8 19 | ament_cmake_lint_cmake 20 | ament_cmake_pep257 21 | ament_cmake_uncrustify 22 | ament_cmake_xmllint 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) 2 | 3 | if(POLICY CMP0100) 4 | cmake_policy(SET CMP0100 NEW) 5 | endif() 6 | 7 | set(CMAKE_AUTOMOC ON) 8 | 9 | find_package(gz_gui_vendor REQUIRED) 10 | find_package(gz-gui REQUIRED) 11 | find_package(Qt5 12 | COMPONENTS 13 | Core 14 | Quick 15 | QuickControls2 16 | REQUIRED 17 | ) 18 | 19 | qt5_add_resources(resources_rcc Create3Hmi.qrc) 20 | 21 | add_library( 22 | Create3Hmi 23 | SHARED 24 | ${headers_MOC} 25 | Create3Hmi.cc 26 | ${resources_rcc} 27 | ) 28 | include_directories( 29 | ${Qt5Core_INCLUDE_DIRS} 30 | ${Qt5Qml_INCLUDE_DIRS} 31 | ${Qt5Quick_INCLUDE_DIRS} 32 | ${Qt5QuickControls2_INCLUDE_DIRS} 33 | ) 34 | target_link_libraries(Create3Hmi 35 | PUBLIC 36 | gz-gui::gz-gui 37 | ${Qt5Core_LIBRARIES} 38 | ${Qt5Qml_LIBRARIES} 39 | ${Qt5Quick_LIBRARIES} 40 | ${Qt5QuickControls2_LIBRARIES} 41 | ) 42 | ament_target_dependencies(Create3Hmi 43 | gz_gui_vendor 44 | gz-gui 45 | ) 46 | 47 | install( 48 | TARGETS Create3Hmi 49 | DESTINATION share/${PROJECT_NAME}/lib 50 | ) 51 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/namespace.py: -------------------------------------------------------------------------------- 1 | from typing import Union 2 | 3 | from launch import LaunchContext, SomeSubstitutionsType, Substitution 4 | 5 | 6 | class GetNamespacedName(Substitution): 7 | def __init__( 8 | self, 9 | namespace: Union[SomeSubstitutionsType, str], 10 | name: Union[SomeSubstitutionsType, str] 11 | ) -> None: 12 | self.__namespace = namespace 13 | self.__name = name 14 | 15 | def perform( 16 | self, 17 | context: LaunchContext = None, 18 | ) -> str: 19 | if isinstance(self.__namespace, Substitution): 20 | namespace = str(self.__namespace.perform(context)) 21 | else: 22 | namespace = str(self.__namespace) 23 | 24 | if isinstance(self.__name, Substitution): 25 | name = str(self.__name.perform(context)) 26 | else: 27 | name = str(self.__name) 28 | 29 | if namespace == '': 30 | namespaced_name = name 31 | else: 32 | namespaced_name = namespace + '/' + name 33 | return namespaced_name 34 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_control 5 | 3.0.4 6 | Provides the diff-drive controller for the iRobot(R) Create(R) 3 Educational Robot. 7 | Ekumen 8 | BSD 9 | 10 | iRobot 11 | 12 | ament_cmake 13 | 14 | joint_state_broadcaster 15 | ros2launch 16 | ros2_controllers 17 | rsl 18 | 19 | ament_lint_auto 20 | ament_cmake_flake8 21 | ament_cmake_lint_cmake 22 | ament_cmake_pep257 23 | ament_cmake_xmllint 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/sensors/optical_mouse.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | ${namespace} 21 | ~/out:=${name} 22 | 23 | ${update_rate} 24 | ${name} 25 | 26 | 27 | 28 | 29 | 30 | true 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/launch/rviz2.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | # @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | # 4 | # Launch RViz. 5 | 6 | from ament_index_python.packages import get_package_share_directory 7 | from launch import LaunchDescription 8 | from launch.substitutions import PathJoinSubstitution 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | create_bringup = get_package_share_directory('irobot_create_common_bringup') 14 | 15 | # Rviz 16 | rviz_config = PathJoinSubstitution([create_bringup, 'rviz', 'irobot_create_view.rviz']) 17 | rviz_logo = PathJoinSubstitution([create_bringup, 'rviz', 'irobot_logo.jpg']) 18 | 19 | rviz = Node( 20 | package='rviz2', 21 | executable='rviz2', 22 | name='rviz2', 23 | arguments=[ 24 | '--display-config', rviz_config, 25 | '--splash-screen', rviz_logo, 26 | ], 27 | remappings=[ 28 | ('/tf', 'tf'), 29 | ('/tf_static', 'tf_static'), 30 | ] 31 | ) 32 | 33 | # Define LaunchDescription variable 34 | ld = LaunchDescription() 35 | # Add nodes to LaunchDescription 36 | ld.add_action(rviz) 37 | 38 | return ld 39 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_intensity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "irobot_create_msgs/msg/ir_intensity.hpp" 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "sensor_msgs/msg/laser_scan.hpp" 17 | 18 | namespace irobot_create_gz_toolbox 19 | { 20 | 21 | class IrIntensity 22 | { 23 | public: 24 | explicit IrIntensity(std::shared_ptr & nh); 25 | virtual ~IrIntensity() {} 26 | 27 | private: 28 | void ir_scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr ir_msg); 29 | 30 | std::shared_ptr nh_; 31 | 32 | std::vector::SharedPtr> ir_scan_sub_; 33 | std::map::SharedPtr> ir_intensity_pub_; 35 | 36 | std::string ir_intensity_sensors_[7]; 37 | }; 38 | 39 | } // namespace irobot_create_gz_toolbox 40 | 41 | #endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ 42 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/cliff.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__CLIFF_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__CLIFF_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "irobot_create_msgs/msg/hazard_detection.hpp" 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "sensor_msgs/msg/laser_scan.hpp" 17 | 18 | namespace irobot_create_gz_toolbox 19 | { 20 | 21 | class Cliff 22 | { 23 | public: 24 | explicit Cliff(std::shared_ptr & nh); 25 | virtual ~Cliff() {} 26 | 27 | enum CliffSensors 28 | { 29 | FRONT_LEFT, 30 | FRONT_RIGHT, 31 | SIDE_LEFT, 32 | SIDE_RIGHT 33 | }; 34 | 35 | private: 36 | void cliff_callback(const sensor_msgs::msg::LaserScan::SharedPtr cliff_msg); 37 | 38 | std::shared_ptr nh_; 39 | 40 | std::vector::SharedPtr> cliff_sub_; 41 | std::map::SharedPtr> hazard_pub_; 43 | 44 | std::string cliff_sensors_[4]; 45 | }; 46 | 47 | } // namespace irobot_create_gz_toolbox 48 | 49 | #endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__CLIFF_HPP_ 50 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/mock_publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Lola Segura (lsegura@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__MOCK_PUBLISHER_HPP_ 5 | #define IROBOT_CREATE_NODES__MOCK_PUBLISHER_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "irobot_create_msgs/msg/slip_status.hpp" 13 | #include "rclcpp/rclcpp.hpp" 14 | #include "rclcpp_action/rclcpp_action.hpp" 15 | 16 | namespace irobot_create_nodes 17 | { 18 | 19 | class MockPublisher : public rclcpp::Node 20 | { 21 | public: 22 | /// \brief Constructor 23 | explicit MockPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 24 | 25 | private: 26 | // Publish aggregated detections on timer_'s frequency 27 | rclcpp::TimerBase::SharedPtr slip_status_timer_; 28 | 29 | // Publishers 30 | rclcpp::Publisher::SharedPtr slip_status_publisher_{nullptr}; 31 | 32 | // Topic to publish slip status to 33 | std::string slip_status_publisher_topic_; 34 | 35 | // Message to store the slip status 36 | irobot_create_msgs::msg::SlipStatus slip_status_msg_; 37 | 38 | const std::string base_frame_ {"base_link"}; 39 | }; 40 | 41 | } // namespace irobot_create_nodes 42 | 43 | #endif // IROBOT_CREATE_NODES__MOCK_PUBLISHER_HPP_ 44 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/mouse.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__MOUSE_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__MOUSE_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "irobot_create_msgs/msg/mouse.hpp" 14 | #include "nav_msgs/msg/odometry.hpp" 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 17 | 18 | namespace irobot_create_gz_toolbox 19 | { 20 | 21 | class Mouse 22 | { 23 | public: 24 | explicit Mouse(std::shared_ptr & nh); 25 | virtual ~Mouse() {} 26 | 27 | private: 28 | void robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); 29 | void mouse_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); 30 | 31 | std::shared_ptr nh_; 32 | 33 | // Timers 34 | rclcpp::TimerBase::SharedPtr timer_; 35 | 36 | rclcpp::Subscription::SharedPtr mouse_pose_sub_; 37 | rclcpp::Publisher::SharedPtr mouse_pub_; 38 | 39 | tf2::Vector3 integrated_position_; 40 | tf2::Vector3 last_mouse_position_; 41 | }; 42 | 43 | } // namespace irobot_create_gz_toolbox 44 | 45 | #endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__MOUSE_HPP_ 46 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/caster.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | true 33 | 34 | 35 | 36 | Gazebo/DarkGrey 37 | 0.1 38 | 0.1 39 | 1000000.0 40 | 1.0 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/wheel_drop.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "irobot_create_msgs/msg/hazard_detection.hpp" 14 | #include "rclcpp/rclcpp.hpp" 15 | #include "sensor_msgs/msg/joint_state.hpp" 16 | 17 | namespace irobot_create_gz_toolbox 18 | { 19 | 20 | class WheelDrop 21 | { 22 | public: 23 | explicit WheelDrop(std::shared_ptr & nh); 24 | virtual ~WheelDrop() {} 25 | 26 | private: 27 | void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state_msg); 28 | 29 | std::shared_ptr nh_; 30 | 31 | rclcpp::Subscription::SharedPtr joint_state_sub_; 32 | 33 | std::map::SharedPtr> hazard_pub_; 35 | 36 | double detection_threshold_; 37 | double lower_limit_, upper_limit_; 38 | std::string joints_[2]; 39 | std::map wheeldrop_detected_; 40 | std::map displacement_; 41 | }; 42 | 43 | } // namespace irobot_create_gz_toolbox 44 | 45 | #endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ 46 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/include/irobot_create_toolbox/sensors/bumpers.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 iRobot Corporation. All Rights Reserved. 2 | // @author Alberto Soragna (asoragna@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_TOOLBOX__SENSORS__BUMPERS_HPP_ 5 | #define IROBOT_CREATE_TOOLBOX__SENSORS__BUMPERS_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace irobot_create_toolbox 12 | { 13 | namespace sensors 14 | { 15 | 16 | // Bumper zones types 17 | enum class BumperZoneType 18 | { 19 | RIGHT, 20 | CENTER_RIGHT, 21 | CENTER, 22 | CENTER_LEFT, 23 | LEFT 24 | }; 25 | 26 | // Auxiliary data structure to hold bumper zone details 27 | struct BumperZone 28 | { 29 | double left_limit; 30 | double right_limit; 31 | std::string name; 32 | }; 33 | 34 | // Data structure to hold the definitions related to bumper zones 35 | const std::map BUMPER_ZONES_MAP = { 36 | {BumperZoneType::RIGHT, {-M_PI / 2, -3 * M_PI / 10, "bump_right"}}, 37 | {BumperZoneType::CENTER_RIGHT, {-3 * M_PI / 10, -M_PI / 10, "bump_front_right"}}, 38 | {BumperZoneType::CENTER, {-M_PI / 10, M_PI / 10, "bump_front_center"}}, 39 | {BumperZoneType::CENTER_LEFT, {M_PI / 10., 3 * M_PI / 10, "bump_front_left"}}, 40 | {BumperZoneType::LEFT, {3 * M_PI / 10, M_PI / 2, "bump_left"}}}; 41 | 42 | } // namespace sensors 43 | } // namespace irobot_create_toolbox 44 | 45 | #endif // IROBOT_CREATE_TOOLBOX__SENSORS__BUMPERS_HPP_ 46 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "irobot_create_msgs/msg/interface_buttons.hpp" 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "std_msgs/msg/int32.hpp" 17 | 18 | namespace irobot_create_gz_toolbox 19 | { 20 | 21 | enum Create3Buttons 22 | { 23 | NONE, 24 | BUTTON_1, 25 | BUTTON_POWER, 26 | BUTTON_2 27 | }; 28 | 29 | class InterfaceButtons : public rclcpp::Node 30 | { 31 | public: 32 | // Constructor and Destructor 33 | InterfaceButtons(); 34 | 35 | private: 36 | void create3_buttons_callback(const std_msgs::msg::Int32::SharedPtr create3_buttons_msg); 37 | 38 | rclcpp::Subscription::SharedPtr interface_buttons_sub_; 39 | rclcpp::Publisher::SharedPtr interface_buttons_pub_; 40 | 41 | std::unique_ptr interface_buttons_; 42 | }; 43 | 44 | } // namespace irobot_create_gz_toolbox 45 | 46 | #endif // IROBOT_CREATE_GZ_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ 47 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/sensors_node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "irobot_create_gz_toolbox/sensors/bumper.hpp" 15 | #include "irobot_create_gz_toolbox/sensors/cliff.hpp" 16 | #include "irobot_create_gz_toolbox/sensors/ir_intensity.hpp" 17 | #include "irobot_create_gz_toolbox/sensors/mouse.hpp" 18 | #include "irobot_create_gz_toolbox/sensors/wheel_drop.hpp" 19 | #include "irobot_create_gz_toolbox/sensors/ir_opcode.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | namespace irobot_create_gz_toolbox 23 | { 24 | 25 | class SensorsNode : public rclcpp::Node 26 | { 27 | public: 28 | // Constructor and Destructor 29 | SensorsNode(); 30 | 31 | private: 32 | // Node 33 | rclcpp::Node::SharedPtr nh_; 34 | 35 | // Sensors 36 | std::unique_ptr bumper_; 37 | std::unique_ptr cliff_; 38 | std::unique_ptr ir_intensity_; 39 | std::unique_ptr mouse_; 40 | std::unique_ptr wheel_drop_; 41 | std::unique_ptr ir_opcode_; 42 | }; 43 | 44 | } // namespace irobot_create_gz_toolbox 45 | 46 | #endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ 47 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/config/sensors_params.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | /**: 3 | sensors_node: 4 | ros__parameters: 5 | cliff_subscription_topics: 6 | - _internal/cliff_front_left/scan 7 | - _internal/cliff_front_right/scan 8 | - _internal/cliff_side_left/scan 9 | - _internal/cliff_side_right/scan 10 | 11 | cliff_publish_topics: 12 | - _internal/cliff_front_left/event 13 | - _internal/cliff_front_right/event 14 | - _internal/cliff_side_left/event 15 | - _internal/cliff_side_right/event 16 | 17 | ir_scan_subscription_topics: 18 | - _internal/ir_intensity_front_center_left/scan 19 | - _internal/ir_intensity_front_center_right/scan 20 | - _internal/ir_intensity_front_left/scan 21 | - _internal/ir_intensity_front_right/scan 22 | - _internal/ir_intensity_left/scan 23 | - _internal/ir_intensity_right/scan 24 | - _internal/ir_intensity_side_left/scan 25 | 26 | ir_intensity_publish_topics: 27 | - _internal/ir_intensity_front_center_left 28 | - _internal/ir_intensity_front_center_right 29 | - _internal/ir_intensity_front_left 30 | - _internal/ir_intensity_front_right 31 | - _internal/ir_intensity_left 32 | - _internal/ir_intensity_right 33 | - _internal/ir_intensity_side_left 34 | 35 | ir_opcode_sensor_0_fov: 3.839724 36 | ir_opcode_sensor_0_range: 0.1 37 | ir_opcode_sensor_1_fov: 1.570796 38 | ir_opcode_sensor_1_range: 0.5 -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_gz_toolbox 5 | 3.0.4 6 | Nodes and tools for simulating in Ignition iRobot(R) Create(R) 3 Educational Robot. 7 | rkreinin 8 | BSD 9 | 10 | ament_cmake 11 | 12 | control_msgs 13 | irobot_create_msgs 14 | irobot_create_toolbox 15 | nav_msgs 16 | rclcpp 17 | rclcpp_action 18 | rcutils 19 | ros_gz_interfaces 20 | std_msgs 21 | sensor_msgs 22 | tf2 23 | tf2_geometry_msgs 24 | 25 | ament_lint_auto 26 | ament_cmake_cppcheck 27 | ament_cmake_cpplint 28 | ament_cmake_flake8 29 | ament_cmake_lint_cmake 30 | ament_cmake_pep257 31 | ament_cmake_uncrustify 32 | ament_cmake_xmllint 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/bumper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__BUMPER_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__BUMPER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "irobot_create_msgs/msg/hazard_detection.hpp" 15 | #include "nav_msgs/msg/odometry.hpp" 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "ros_gz_interfaces/msg/contacts.hpp" 18 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 19 | 20 | namespace irobot_create_gz_toolbox 21 | { 22 | 23 | class Bumper 24 | { 25 | public: 26 | explicit Bumper(std::shared_ptr & nh); 27 | virtual ~Bumper() {} 28 | 29 | private: 30 | void bumper_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr bumper_contact_msg); 31 | void robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); 32 | 33 | std::shared_ptr nh_; 34 | 35 | rclcpp::Subscription::SharedPtr bumper_sub_; 36 | rclcpp::Subscription::SharedPtr robot_pose_sub_; 37 | rclcpp::Publisher::SharedPtr hazard_pub_; 38 | 39 | tf2::Transform last_robot_pose_; 40 | std::mutex robot_pose_mutex_; 41 | }; 42 | 43 | } // namespace irobot_create_gz_toolbox 44 | 45 | #endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__BUMPER_HPP_ 46 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_nodes 5 | 3.0.4 6 | ROS 2 Nodes for the simulated iRobot(R) Create(R) 3 Educational Robot. 7 | Ekumen 8 | BSD 9 | 10 | ament_cmake 11 | 12 | angles 13 | boost 14 | rclcpp 15 | rclcpp_action 16 | rclcpp_components 17 | control_msgs 18 | sensor_msgs 19 | nav_msgs 20 | geometry_msgs 21 | irobot_create_msgs 22 | irobot_create_toolbox 23 | tf2 24 | tf2_geometry_msgs 25 | tf2_ros 26 | 27 | ament_lint_auto 28 | ament_cmake_cppcheck 29 | ament_cmake_cpplint 30 | ament_cmake_flake8 31 | ament_cmake_lint_cmake 32 | ament_cmake_pep257 33 | ament_cmake_uncrustify 34 | ament_cmake_xmllint 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(irobot_create_toolbox) 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(gz_math_vendor REQUIRED) 15 | find_package(gz-math REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | 18 | #### Libraries 19 | 20 | add_library(irobot_create_toolbox SHARED) 21 | target_sources( 22 | irobot_create_toolbox 23 | PRIVATE 24 | src/polar_coordinates.cpp 25 | ) 26 | target_include_directories(irobot_create_toolbox PUBLIC 27 | "$" 28 | "$") 29 | 30 | target_link_libraries(irobot_create_toolbox 31 | PUBLIC 32 | gz-math::gz-math 33 | ${rclcpp_TARGETS} 34 | ) 35 | 36 | #### Install 37 | 38 | install(TARGETS 39 | ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} 40 | ARCHIVE DESTINATION lib 41 | LIBRARY DESTINATION lib 42 | RUNTIME DESTINATION bin 43 | ) 44 | 45 | install(DIRECTORY include/ 46 | DESTINATION include/${PROJECT_NAME} 47 | ) 48 | ament_export_targets(export_${PROJECT_NAME}) 49 | ament_export_dependencies( 50 | gz_math_vendor 51 | gz-math 52 | rclcpp 53 | ) 54 | 55 | if(BUILD_TESTING) 56 | find_package(ament_lint_auto REQUIRED) 57 | ament_lint_auto_find_test_dependencies() 58 | endif() 59 | 60 | ament_package() 61 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_common_bringup 5 | 3.0.4 6 | Provides common launch and configuration scripts for a simulated iRobot(R) Create(R) 3 Educational Robot. 7 | Ekumen 8 | BSD 9 | 10 | iRobot 11 | 12 | ament_cmake 13 | ament_cmake_python 14 | 15 | irobot_create_control 16 | irobot_create_description 17 | irobot_create_nodes 18 | joint_state_publisher 19 | robot_state_publisher 20 | ros2launch 21 | rviz2 22 | urdf 23 | xacro 24 | 25 | ament_lint_auto 26 | ament_cmake_cppcheck 27 | ament_cmake_cpplint 28 | ament_cmake_flake8 29 | ament_cmake_lint_cmake 30 | ament_cmake_pep257 31 | ament_cmake_uncrustify 32 | ament_cmake_xmllint 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | irobot_create_gz_bringup 5 | 3.0.4 6 | Provides launch and configuration scripts for a Ignition simulated iRobot(R) Create(R) 3 Educational Robot. 7 | rkreinin 8 | BSD 9 | 10 | ament_cmake 11 | 12 | ros_gz_interfaces 13 | 14 | 15 | irobot_create_description 16 | irobot_create_common_bringup 17 | irobot_create_gz_toolbox 18 | 19 | 20 | gz_ros2_control 21 | ros_gz_sim 22 | ros_gz_bridge 23 | 24 | 25 | std_msgs 26 | geometry_msgs 27 | irobot_create_msgs 28 | 29 | ament_lint_auto 30 | ament_cmake_cppcheck 31 | ament_cmake_cpplint 32 | ament_cmake_flake8 33 | ament_cmake_lint_cmake 34 | ament_cmake_pep257 35 | ament_cmake_uncrustify 36 | ament_cmake_xmllint 37 | 38 | 39 | ament_cmake 40 | 41 | 42 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/offset.py: -------------------------------------------------------------------------------- 1 | import math 2 | from typing import Union 3 | 4 | from launch import LaunchContext, SomeSubstitutionsType, Substitution 5 | 6 | 7 | class RotationalOffsetX(Substitution): 8 | def __init__( 9 | self, 10 | offset: float, 11 | yaw: SomeSubstitutionsType 12 | ) -> None: 13 | self.__offset = offset 14 | self.__yaw = yaw 15 | 16 | def perform( 17 | self, 18 | context: LaunchContext = None, 19 | ) -> str: 20 | yaw = float(self.__yaw.perform(context)) 21 | return f'{self.__offset * math.cos(yaw)}' 22 | 23 | 24 | class RotationalOffsetY(Substitution): 25 | def __init__( 26 | self, 27 | offset: float, 28 | yaw: SomeSubstitutionsType 29 | ) -> None: 30 | self.__offset = offset 31 | self.__yaw = yaw 32 | 33 | def perform( 34 | self, 35 | context: LaunchContext = None, 36 | ) -> str: 37 | yaw = float(self.__yaw.perform(context)) 38 | return f'{self.__offset * math.sin(yaw)}' 39 | 40 | 41 | class OffsetParser(Substitution): 42 | def __init__( 43 | self, 44 | number: SomeSubstitutionsType, 45 | offset: Union[float, Substitution], 46 | ) -> None: 47 | self.__number = number 48 | self.__offset = offset 49 | 50 | def perform( 51 | self, 52 | context: LaunchContext = None, 53 | ) -> str: 54 | number = float(self.__number.perform(context)) 55 | if isinstance(self.__offset, Substitution): 56 | offset = float(self.__offset.perform(context)) 57 | else: 58 | offset = self.__offset 59 | return f'{number + offset}' 60 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/hazards_vector_publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__HAZARDS_VECTOR_PUBLISHER_HPP_ 5 | #define IROBOT_CREATE_NODES__HAZARDS_VECTOR_PUBLISHER_HPP_ 6 | 7 | #include 8 | #include 9 | 10 | #include "irobot_create_msgs/msg/hazard_detection.hpp" 11 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 12 | #include "rclcpp/rclcpp.hpp" 13 | 14 | namespace irobot_create_nodes 15 | { 16 | 17 | class HazardsVectorPublisher : public rclcpp::Node 18 | { 19 | public: 20 | /// \brief Constructor 21 | explicit HazardsVectorPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 22 | 23 | private: 24 | // Publish aggregated detections on timer_'s frequency 25 | rclcpp::TimerBase::SharedPtr timer_; 26 | 27 | // Detection vector publisher 28 | rclcpp::Publisher::SharedPtr publisher_; 29 | 30 | // Vector of subscriptions 31 | using HazardVectorSubscriptionPtr = 32 | std::vector::SharedPtr>; 33 | HazardVectorSubscriptionPtr subs_vector_; 34 | 35 | // Mutex to protect access to subs_vector_ from different threads 36 | std::mutex mutex_; 37 | 38 | // Topic to publish hazards vector to 39 | std::string publisher_topic_; 40 | 41 | // Topics from where hazard messages will be received from 42 | std::vector subscription_topics_; 43 | 44 | // Message containing a vector to store detected hazards 45 | irobot_create_msgs::msg::HazardDetectionVector msg_; 46 | }; 47 | 48 | } // namespace irobot_create_nodes 49 | 50 | #endif // IROBOT_CREATE_NODES__HAZARDS_VECTOR_PUBLISHER_HPP_ 51 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/sensors/mouse.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | #include "irobot_create_gz_toolbox/sensors/mouse.hpp" 10 | 11 | using irobot_create_gz_toolbox::Mouse; 12 | 13 | Mouse::Mouse(std::shared_ptr & nh) 14 | : nh_(nh), 15 | integrated_position_{0, 0, 0}, 16 | last_mouse_position_{0, 0, 0} 17 | { 18 | mouse_pose_sub_ = nh_->create_subscription( 19 | "_internal/sim_ground_truth_mouse_pose", 20 | rclcpp::SensorDataQoS(), 21 | std::bind(&Mouse::mouse_pose_callback, this, std::placeholders::_1)); 22 | 23 | mouse_pub_ = nh_->create_publisher( 24 | "mouse", rclcpp::SensorDataQoS()); 25 | } 26 | 27 | void Mouse::mouse_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg) 28 | { 29 | auto mouse_msg = irobot_create_msgs::msg::Mouse(); 30 | mouse_msg.header.stamp = nh_->now(); 31 | mouse_msg.header.frame_id = "mouse_link"; 32 | 33 | tf2::Transform mouse_pose; 34 | tf2::convert(msg->pose.pose, mouse_pose); 35 | 36 | tf2::Vector3 mouse_position = mouse_pose.getOrigin(); 37 | 38 | // First iteration 39 | if (last_mouse_position_.isZero()) { 40 | last_mouse_position_ = mouse_position; 41 | } 42 | 43 | const tf2::Vector3 position_displacement = mouse_position - last_mouse_position_; 44 | 45 | integrated_position_ += position_displacement; 46 | 47 | mouse_msg.integrated_x = integrated_position_.getX(); 48 | mouse_msg.integrated_y = integrated_position_.getY(); 49 | 50 | mouse_pub_->publish(std::move(mouse_msg)); 51 | 52 | // The pose is updated 53 | if (mouse_msg.integrated_x != 0.0 || mouse_msg.integrated_y != 0.0) { 54 | last_mouse_position_ = mouse_position; 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/sensors/ir_intensity.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | true 25 | 26 | 27 | 28 | 33 | 34 | 35 | ${namespace} 36 | ~/out:=_internal/${link_name} 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/src/mock_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Lola Segura (lsegura@irobot.com) 3 | 4 | #include "irobot_create_nodes/mock_publisher.hpp" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "rclcpp_components/register_node_macro.hpp" 12 | 13 | namespace irobot_create_nodes 14 | { 15 | using namespace std::placeholders; 16 | 17 | MockPublisher::MockPublisher(const rclcpp::NodeOptions & options) 18 | : rclcpp::Node("mock_publisher_node", options) 19 | { 20 | // Topic parameter to publish slip status to 21 | slip_status_publisher_topic_ = 22 | this->declare_parameter("slip_status_topic", "slip_status"); 23 | 24 | // Publish rate parameters in Hz 25 | const double slip_status_publish_rate = 26 | this->declare_parameter("slip_status_publish_rate", 62.0); 27 | 28 | // Define slip status publisher 29 | slip_status_publisher_ = create_publisher( 30 | slip_status_publisher_topic_, rclcpp::SensorDataQoS()); 31 | RCLCPP_INFO_STREAM(get_logger(), "Advertised mocked topic: " << slip_status_publisher_topic_); 32 | 33 | slip_status_timer_ = rclcpp::create_timer( 34 | this, 35 | this->get_clock(), 36 | rclcpp::Duration(std::chrono::duration(1 / slip_status_publish_rate)), [this]() { 37 | // Set header timestamp. 38 | this->slip_status_msg_.header.stamp = now(); 39 | 40 | // Publish topics 41 | this->slip_status_publisher_->publish(this->slip_status_msg_); 42 | }); 43 | 44 | // Set slip status header 45 | slip_status_msg_.header.frame_id = base_frame_; 46 | // Set slip status status 47 | slip_status_msg_.is_slipping = false; 48 | } 49 | 50 | } // namespace irobot_create_nodes 51 | 52 | RCLCPP_COMPONENTS_REGISTER_NODE(irobot_create_nodes::MockPublisher) 53 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/include/irobot_create_toolbox/math.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 iRobot Corporation. All Rights Reserved. 2 | // @author Alberto Soragna (asoragna@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_TOOLBOX__MATH_HPP_ 5 | #define IROBOT_CREATE_TOOLBOX__MATH_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace irobot_create_toolbox 13 | { 14 | 15 | /// \brief Convert radians to degrees 16 | template 17 | inline int Rad2Deg(T radians) {return radians / M_PI * 180;} 18 | 19 | /// \brief Wrap angle between (-pi, pi] 20 | template 21 | inline T WrapAngle(T angle) {return atan2(sin(angle), cos(angle));} 22 | 23 | /// \brief Calculate if a provided angle in radians is within the arc formed by two other angles. 24 | /// \param target angle to test 25 | /// \param angle1 first leg of the arc 26 | /// \param target second leg of the arc 27 | template 28 | bool IsAngleBetween(T target, T angle1, T angle2) 29 | { 30 | T t = WrapAngle(target); 31 | T a1 = WrapAngle(angle1); 32 | T a2 = WrapAngle(angle2); 33 | // check if it passes through zero 34 | if (a1 <= a2) { 35 | return (t >= a1) && (t <= a2); 36 | } else { 37 | return (t >= a1) || (t <= a2); 38 | } 39 | return false; 40 | } 41 | 42 | // Find the minimum range given a vector of 'ranges'. 43 | // If none was found, returns a maximum T. 44 | template 45 | T FindMinimumRange(const std::vector & ranges) 46 | { 47 | auto detection_ptr = 48 | std::min_element(std::begin(ranges), std::end(ranges)); 49 | // If a minimum range was found, return it 50 | if (detection_ptr != std::end(ranges)) { 51 | return *detection_ptr; 52 | } 53 | // Otherwise, return a maximum range 54 | return std::numeric_limits::max(); 55 | } 56 | 57 | } // namespace irobot_create_toolbox 58 | 59 | #endif // IROBOT_CREATE_TOOLBOX__MATH_HPP_ 60 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/kidnap_estimator_publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Lola Segura (lsegura@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__KIDNAP_ESTIMATOR_PUBLISHER_HPP_ 5 | #define IROBOT_CREATE_NODES__KIDNAP_ESTIMATOR_PUBLISHER_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "irobot_create_msgs/msg/hazard_detection.hpp" 13 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 14 | #include "irobot_create_msgs/msg/kidnap_status.hpp" 15 | #include "rclcpp/rclcpp.hpp" 16 | 17 | namespace irobot_create_nodes 18 | { 19 | 20 | class KidnapEstimator : public rclcpp::Node 21 | { 22 | public: 23 | /// \brief Constructor 24 | explicit KidnapEstimator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 25 | 26 | private: 27 | /// \brief Callback function 28 | void kidnap_callback(irobot_create_msgs::msg::HazardDetectionVector::SharedPtr msg); 29 | 30 | // Publish aggregated detections on timer_'s frequency 31 | rclcpp::TimerBase::SharedPtr kidnap_status_timer_; 32 | 33 | // Publisher 34 | rclcpp::Publisher::SharedPtr 35 | kidnap_status_publisher_{nullptr}; 36 | 37 | // Subscriber 38 | rclcpp::Subscription< 39 | irobot_create_msgs::msg::HazardDetectionVector>::SharedPtr kidnap_status_subscription_; 40 | 41 | // Topic to publish kidnap status to 42 | std::string kidnap_status_publisher_topic_; 43 | 44 | // Topic to subscribe to hazard detection vector 45 | std::string hazard_subscription_topic_; 46 | 47 | // Message to store the kidnap status 48 | irobot_create_msgs::msg::KidnapStatus kidnap_status_msg_; 49 | 50 | const std::size_t min_wheel_drop_count_{2}; 51 | const std::size_t min_cliff_sensor_count_{4}; 52 | 53 | const std::string base_frame_ {"base_link"}; 54 | }; 55 | 56 | } // namespace irobot_create_nodes 57 | 58 | #endif // IROBOT_CREATE_NODES__KIDNAP_ESTIMATOR_PUBLISHER_HPP_ 59 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/sensors/cliff_sensor.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 32 | 33 | 34 | ${namespace} 35 | ~/out:=_internal/${sensor_name}/event 36 | 37 | ${detection_threshold} 38 | ${sensor_name} 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/dock/ir_emitter.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | true 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ__IROBOT_CREATE_GZ_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ 7 | #define IROBOT_CREATE_GZ__IROBOT_CREATE_GZ_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | 17 | namespace gz 18 | { 19 | 20 | namespace gui 21 | { 22 | 23 | class Create3Hmi : public Plugin 24 | { 25 | Q_OBJECT 26 | 27 | // \brief Name 28 | Q_PROPERTY( 29 | QString name 30 | READ Namespace 31 | WRITE SetNamespace 32 | NOTIFY NamespaceChanged 33 | ) 34 | 35 | public: 36 | /// \brief Constructor 37 | Create3Hmi(); 38 | /// \brief Destructor 39 | virtual ~Create3Hmi(); 40 | /// \brief Called by Ignition GUI when plugin is instantiated. 41 | /// \param[in] _pluginElem XML configuration for this plugin. 42 | void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; 43 | // \brief Get the robot name as a string, for example 44 | /// '/echo' 45 | /// \return Name 46 | Q_INVOKABLE QString Namespace() const; 47 | 48 | public slots: 49 | /// \brief Callback in Qt thread when the robot name changes. 50 | /// \param[in] _name variable to indicate the robot name to 51 | /// publish the Button commands. 52 | void SetNamespace(const QString &_name); 53 | 54 | signals: 55 | /// \brief Notify that robot name has changed 56 | void NamespaceChanged(); 57 | 58 | protected slots: 59 | /// \brief Callback trigged when the button is pressed. 60 | void OnCreate3Button(const int button); 61 | 62 | private: 63 | gz::transport::Node node_; 64 | gz::transport::Node::Publisher create3_button_pub_; 65 | std::string namespace_ = ""; 66 | std::string create3_button_topic_ = "/create3_buttons"; 67 | }; 68 | 69 | } // namespace gui 70 | 71 | } // namespace gz 72 | 73 | #endif // IROBOT_CREATE_GZ__IROBOT_CREATE_GZ_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ 74 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/sensors/cliff.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "irobot_create_gz_toolbox/sensors/cliff.hpp" 11 | #include "irobot_create_toolbox/math.hpp" 12 | 13 | using irobot_create_gz_toolbox::Cliff; 14 | 15 | Cliff::Cliff(std::shared_ptr & nh) 16 | : nh_(nh), 17 | cliff_sensors_{ 18 | "front_left", 19 | "front_right", 20 | "side_left", 21 | "side_right" 22 | } 23 | { 24 | auto cliff_sub_topics = 25 | nh_->declare_parameter("cliff_subscription_topics", std::vector()); 26 | 27 | auto cliff_pub_topics = 28 | nh_->declare_parameter("cliff_publish_topics", std::vector()); 29 | 30 | for (std::string topic : cliff_sub_topics) { 31 | cliff_sub_.push_back( 32 | nh_->create_subscription( 33 | topic, 34 | rclcpp::SensorDataQoS(), 35 | std::bind(&Cliff::cliff_callback, this, std::placeholders::_1))); 36 | } 37 | 38 | for (const std::string & topic : cliff_pub_topics) { 39 | for (const std::string & sensor : cliff_sensors_) { 40 | if (topic.find(sensor) != std::string::npos) { 41 | hazard_pub_[sensor] = nh_->create_publisher< 42 | irobot_create_msgs::msg::HazardDetection>( 43 | topic, 44 | rclcpp::SensorDataQoS()); 45 | } 46 | } 47 | } 48 | } 49 | 50 | void Cliff::cliff_callback(const sensor_msgs::msg::LaserScan::SharedPtr cliff_msg) 51 | { 52 | constexpr double detection_threshold = 0.03; 53 | 54 | const double range_detection = irobot_create_toolbox::FindMinimumRange(cliff_msg->ranges); 55 | if (range_detection > detection_threshold) { 56 | auto hazard_msg = irobot_create_msgs::msg::HazardDetection(); 57 | hazard_msg.type = irobot_create_msgs::msg::HazardDetection::CLIFF; 58 | hazard_msg.header.frame_id = "base_link"; 59 | 60 | // Publish to appropriate topic 61 | for (const std::string & sensor : cliff_sensors_) { 62 | if (cliff_msg->header.frame_id.find(sensor) != std::string::npos) { 63 | hazard_pub_[sensor]->publish(hazard_msg); 64 | } 65 | } 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/pose_republisher/pose_republisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ 8 | 9 | #include 10 | 11 | #include "control_msgs/msg/dynamic_joint_state.hpp" 12 | #include "irobot_create_gz_toolbox/utils.hpp" 13 | #include "nav_msgs/msg/odometry.hpp" 14 | #include "rclcpp/rclcpp.hpp" 15 | #include "sensor_msgs/msg/joint_state.hpp" 16 | #include "tf2_msgs/msg/tf_message.hpp" 17 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 18 | 19 | namespace irobot_create_gz_toolbox 20 | { 21 | 22 | class PoseRepublisher : public rclcpp::Node 23 | { 24 | public: 25 | /// \brief Constructor 26 | PoseRepublisher(); 27 | 28 | private: 29 | void robot_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); 30 | void dock_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); 31 | void joint_state_subscriber_callback(const sensor_msgs::msg::JointState::SharedPtr msg); 32 | // Odometry publishers 33 | rclcpp::Publisher::SharedPtr robot_publisher_; 34 | rclcpp::Publisher::SharedPtr dock_publisher_; 35 | rclcpp::Publisher::SharedPtr mouse_publisher_; 36 | rclcpp::Publisher::SharedPtr ir_opcode_emitter_publisher_; 37 | rclcpp::Publisher::SharedPtr ir_opcode_receiver_publisher_; 38 | rclcpp::Publisher::SharedPtr dynamic_joint_state_publisher_; 39 | 40 | // TFMessage subscribers 41 | rclcpp::Subscription::SharedPtr robot_subscriber_; 42 | rclcpp::Subscription::SharedPtr dock_subscriber_; 43 | rclcpp::Subscription::SharedPtr joint_state_sub_; 44 | 45 | tf2::Transform last_robot_pose_; 46 | tf2::Transform last_dock_pose_; 47 | 48 | std::string robot_name_; 49 | std::string dock_name_; 50 | std::string wheel_joints_[2]; 51 | }; 52 | 53 | } // namespace irobot_create_gz_toolbox 54 | 55 | #endif // IROBOT_CREATE_GZ_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ 56 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/ir_intensity_vector_publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__IR_INTENSITY_VECTOR_PUBLISHER_HPP_ 5 | #define IROBOT_CREATE_NODES__IR_INTENSITY_VECTOR_PUBLISHER_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "irobot_create_msgs/msg/ir_intensity.hpp" 13 | #include "irobot_create_msgs/msg/ir_intensity_vector.hpp" 14 | #include "rclcpp/rclcpp.hpp" 15 | 16 | namespace irobot_create_nodes 17 | { 18 | 19 | class IrIntensityVectorPublisher : public rclcpp::Node 20 | { 21 | public: 22 | /// \brief Constructor 23 | explicit IrIntensityVectorPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 24 | 25 | private: 26 | // Publish aggregated detections on timer_'s frequency 27 | rclcpp::TimerBase::SharedPtr timer_; 28 | 29 | // Detection vector publisher 30 | std::shared_ptr> publisher_; 31 | 32 | // Vector of subscriptions 33 | using IrIntensityVectorSubscriptionPtr = 34 | std::vector::SharedPtr>; 35 | IrIntensityVectorSubscriptionPtr subs_vector_; 36 | 37 | // Mutex to protect access to subs_vector_ from different threads 38 | std::mutex mutex_; 39 | 40 | // Topic to publish IR intensity vector to 41 | std::string publisher_topic_; 42 | 43 | // Topics from where IR intensity messages will be received from 44 | std::vector subscription_topics_; 45 | 46 | // Message containing a vector to store IR intensity reasings 47 | irobot_create_msgs::msg::IrIntensityVector msg_; 48 | 49 | // Cache of last message for each frame 50 | // Each IR reading is from a separate publication so there is potential 51 | // for a race condition where frames could not come in, or come in twice 52 | // between vector publications. The cache will keep the most recent 53 | // reading from each frame and the publication will make a vector of 54 | // each frame's most recent received reading from the map of values 55 | std::map ir_intensities_; 56 | }; 57 | 58 | } // namespace irobot_create_nodes 59 | 60 | #endif // IROBOT_CREATE_NODES__IR_INTENSITY_VECTOR_PUBLISHER_HPP_ 61 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/sensors/imu.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | true 19 | ${update_rate} 20 | 21 | 22 | 23 | 24 | 0.0 25 | 0.0 26 | 27 | 28 | 29 | 30 | 0.0 31 | 0.0 32 | 33 | 34 | 35 | 36 | 0.0 37 | 0.0 38 | 39 | 40 | 41 | 42 | 43 | 44 | 0.0 45 | 0.0 46 | 47 | 48 | 49 | 50 | 0.0 51 | 0.0 52 | 53 | 54 | 55 | 56 | 0.0 57 | 0.0 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | ${namespace} 66 | ~/out:=imu 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | true 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/src/hazards_vector_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | 4 | #include "irobot_create_nodes/hazards_vector_publisher.hpp" 5 | 6 | #include 7 | #include 8 | 9 | #include "rclcpp_components/register_node_macro.hpp" 10 | 11 | namespace irobot_create_nodes 12 | { 13 | 14 | HazardsVectorPublisher::HazardsVectorPublisher(const rclcpp::NodeOptions & options) 15 | : rclcpp::Node("hazard_detection_vector_node", options) 16 | { 17 | // Topic parameter to publish hazards vector to 18 | publisher_topic_ = 19 | this->declare_parameter("publisher_topic", "hazard_detection"); 20 | 21 | // Subscription topics parameter 22 | subscription_topics_ = 23 | this->declare_parameter("subscription_topics", std::vector()); 24 | 25 | // Publish rate parameter in Hz 26 | const double publish_rate = 27 | this->declare_parameter("publish_rate", 62.0); 28 | 29 | publisher_ = create_publisher( 30 | publisher_topic_, rclcpp::SensorDataQoS().reliable()); 31 | RCLCPP_INFO_STREAM(get_logger(), "Advertised topic: " << publisher_topic_); 32 | 33 | timer_ = rclcpp::create_timer( 34 | this, 35 | this->get_clock(), 36 | rclcpp::Duration(std::chrono::duration(1 / publish_rate)), [this]() { 37 | std::lock_guard lock{this->mutex_}; 38 | 39 | // Set header timestamp. 40 | this->msg_.header.stamp = now(); 41 | 42 | // Publish detected vector. 43 | this->publisher_->publish(this->msg_); 44 | this->msg_.detections.clear(); 45 | }); 46 | 47 | // Set header frame_id. 48 | this->msg_.header.frame_id = "base_link"; 49 | 50 | // Create subscriptions 51 | for (std::string topic : subscription_topics_) { 52 | subs_vector_.push_back( 53 | (create_subscription( 54 | topic, rclcpp::SensorDataQoS(), 55 | [this](const irobot_create_msgs::msg::HazardDetection::SharedPtr msg) { 56 | std::lock_guard lock{this->mutex_}; 57 | this->msg_.detections.push_back(*msg); 58 | }))); 59 | RCLCPP_INFO_STREAM(get_logger(), "Subscription to topic: " << topic); 60 | } 61 | } 62 | 63 | } // namespace irobot_create_nodes 64 | 65 | RCLCPP_COMPONENTS_REGISTER_NODE(irobot_create_nodes::HazardsVectorPublisher) 66 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/sensors/wheel_drop.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | #include "irobot_create_gz_toolbox/sensors/wheel_drop.hpp" 10 | 11 | using irobot_create_gz_toolbox::WheelDrop; 12 | 13 | WheelDrop::WheelDrop(std::shared_ptr & nh) 14 | : nh_(nh), 15 | detection_threshold_(0.03), 16 | lower_limit_(detection_threshold_ * 0.75), 17 | upper_limit_(detection_threshold_ * 0.95), 18 | joints_{"wheel_drop_left_joint", 19 | "wheel_drop_right_joint"} 20 | { 21 | joint_state_sub_ = nh_->create_subscription( 22 | "joint_states", 23 | rclcpp::SensorDataQoS(), 24 | std::bind(&WheelDrop::joint_state_callback, this, std::placeholders::_1)); 25 | 26 | hazard_pub_[joints_[0]] = nh_->create_publisher( 27 | "_internal/wheel_drop/left_wheel/event", rclcpp::SensorDataQoS()); 28 | hazard_pub_[joints_[1]] = nh_->create_publisher( 29 | "_internal/wheel_drop/right_wheel/event", rclcpp::SensorDataQoS()); 30 | 31 | wheeldrop_detected_[joints_[0]] = false; 32 | wheeldrop_detected_[joints_[1]] = false; 33 | } 34 | 35 | void WheelDrop::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state_msg) 36 | { 37 | // Get position of each wheeldrop joint 38 | for (size_t i = 0; i < joint_state_msg->name.size(); i++) { 39 | for (auto joint : joints_) { 40 | if (joint_state_msg->name[i] == joint) { 41 | displacement_[joint] = joint_state_msg->position[i]; 42 | } 43 | } 44 | } 45 | 46 | // Check each joint for wheeldrop 47 | for (auto joint : joints_) { 48 | if (!wheeldrop_detected_[joint] && (displacement_[joint] >= upper_limit_)) { 49 | wheeldrop_detected_[joint] = true; 50 | } else if (wheeldrop_detected_[joint] && (displacement_[joint] <= lower_limit_)) { 51 | wheeldrop_detected_[joint] = false; 52 | } 53 | 54 | // Publish if wheeldrop is detected 55 | if (wheeldrop_detected_[joint]) { 56 | auto hazard_msg = irobot_create_msgs::msg::HazardDetection(); 57 | hazard_msg.type = irobot_create_msgs::msg::HazardDetection::WHEEL_DROP; 58 | hazard_msg.header.stamp = nh_->now(); 59 | hazard_msg.header.frame_id = joint; 60 | hazard_pub_[joint]->publish(std::move(hazard_msg)); 61 | } 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz_nodes.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Clearpath Robotics, Inc. 2 | # @author Roni Kreinin (rkreinin@clearpathrobotics.com) 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument 8 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 9 | from launch_ros.actions import Node 10 | 11 | ARGUMENTS = [ 12 | DeclareLaunchArgument('robot_name', default_value='create3', 13 | description='Robot name'), 14 | DeclareLaunchArgument('dock_name', default_value='standard_dock', 15 | description='Dock name') 16 | ] 17 | 18 | 19 | def generate_launch_description(): 20 | 21 | # Directories 22 | pkg_create3_gz_bringup = get_package_share_directory('irobot_create_gz_bringup') 23 | 24 | pose_republisher_params_yaml_file = PathJoinSubstitution( 25 | [pkg_create3_gz_bringup, 'config', 'pose_republisher_params.yaml']) 26 | sensors_params_yaml_file = PathJoinSubstitution( 27 | [pkg_create3_gz_bringup, 'config', 'sensors_params.yaml']) 28 | 29 | # Pose republisher 30 | pose_republisher_node = Node( 31 | package='irobot_create_gz_toolbox', 32 | name='pose_republisher_node', 33 | executable='pose_republisher_node', 34 | parameters=[pose_republisher_params_yaml_file, 35 | {'robot_name': LaunchConfiguration('robot_name')}, 36 | {'dock_name': LaunchConfiguration('dock_name')}, 37 | {'use_sim_time': True}], 38 | output='screen', 39 | ) 40 | 41 | # Sensors 42 | sensors_node = Node( 43 | package='irobot_create_gz_toolbox', 44 | name='sensors_node', 45 | executable='sensors_node', 46 | parameters=[sensors_params_yaml_file, 47 | {'use_sim_time': True}], 48 | output='screen', 49 | ) 50 | 51 | # Interface buttons 52 | interface_buttons_node = Node( 53 | package='irobot_create_gz_toolbox', 54 | name='interface_buttons_node', 55 | executable='interface_buttons_node', 56 | parameters=[{'use_sim_time': True}], 57 | output='screen', 58 | ) 59 | 60 | # Create launch description and add actions 61 | ld = LaunchDescription(ARGUMENTS) 62 | ld.add_action(pose_republisher_node) 63 | ld.add_action(sensors_node) 64 | ld.add_action(interface_buttons_node) 65 | return ld 66 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/wheel.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 1.0 39 | 1.0 40 | 1000000.0 41 | 100.0 42 | 0.0001 43 | 1.0 44 | Gazebo/DarkGrey 45 | 46 | 47 | 48 | 49 | 50 | gazebo_ros2_control/GazeboSystem 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | gz_ros2_control/GazeboSimSystem 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/wheels_publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__WHEELS_PUBLISHER_HPP_ 5 | #define IROBOT_CREATE_NODES__WHEELS_PUBLISHER_HPP_ 6 | 7 | #include 8 | #include 9 | 10 | #include "control_msgs/msg/dynamic_joint_state.hpp" 11 | #include "irobot_create_msgs/msg/wheel_ticks.hpp" 12 | #include "irobot_create_msgs/msg/wheel_vels.hpp" 13 | #include "rclcpp/rclcpp.hpp" 14 | #include "std_msgs/msg/string.hpp" 15 | 16 | namespace irobot_create_nodes 17 | { 18 | 19 | class WheelsPublisher : public rclcpp::Node 20 | { 21 | public: 22 | /// \brief Constructor 23 | explicit WheelsPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 24 | 25 | private: 26 | /// \brief Callback to be called periodically to publish the vector message 27 | void publisher_callback(); 28 | // Get vector index based on joint name. 29 | size_t get_joint_index(std::string joint_name); 30 | // Get vector index based on interface name. 31 | size_t get_interface_index(std::string interface_name, size_t joint_index); 32 | // Retrieve dynamic state values from last_joint_state_. 33 | // This is necessary because the indeces are not fixed but the names are. 34 | double get_dynamic_state_value(std::string joint_name, std::string interface_name); 35 | 36 | // Encoder parameters 37 | double encoder_resolution_; 38 | 39 | // Handling wheel ticks and wheel velocity messages 40 | rclcpp::TimerBase::SharedPtr timer_; 41 | 42 | /* 43 | * This variable holds the last message sent by the topic /dynamic_joint_states which 44 | * is published by the diff_drive_controller. This variable holds information about the 45 | * wheels' effort, velocity and position in a vector but the order in which these appear 46 | * in the vector is not guaranteed, which is why we rely on the get_dynamic_state_value() 47 | * method to retrieve information from the vectors. 48 | */ 49 | control_msgs::msg::DynamicJointState last_joint_state_; 50 | irobot_create_msgs::msg::WheelVels angular_vels_msg_; 51 | irobot_create_msgs::msg::WheelTicks wheel_ticks_msg_; 52 | rclcpp::Publisher::SharedPtr angular_vels_publisher_; 53 | rclcpp::Publisher::SharedPtr wheel_ticks_publisher_; 54 | rclcpp::Subscription::SharedPtr subscription_; 55 | 56 | // Mutex 57 | std::mutex mutex_; 58 | }; 59 | 60 | } // namespace irobot_create_nodes 61 | 62 | #endif // IROBOT_CREATE_NODES__WHEELS_PUBLISHER_HPP_ 63 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/src/ir_intensity_vector_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | 4 | #include "irobot_create_nodes/ir_intensity_vector_publisher.hpp" 5 | 6 | #include 7 | #include 8 | 9 | #include "rclcpp_components/register_node_macro.hpp" 10 | 11 | namespace irobot_create_nodes 12 | { 13 | 14 | IrIntensityVectorPublisher::IrIntensityVectorPublisher(const rclcpp::NodeOptions & options) 15 | : rclcpp::Node("ir_intensity_readings_vector_node", options) 16 | { 17 | // Topic parameter to publish IR intensity vector to 18 | publisher_topic_ = 19 | this->declare_parameter("publisher_topic", "ir_intensity"); 20 | 21 | // Subscription topics parameter 22 | subscription_topics_ = 23 | this->declare_parameter("subscription_topics", std::vector()); 24 | 25 | // Publish rate parameter in Hz 26 | const double publish_rate = 27 | this->declare_parameter("publish_rate", 62.0); 28 | 29 | publisher_ = create_publisher( 30 | publisher_topic_, rclcpp::SensorDataQoS().reliable()); 31 | RCLCPP_INFO_STREAM(get_logger(), "Advertised topic: " << publisher_topic_); 32 | 33 | timer_ = rclcpp::create_timer( 34 | this, 35 | this->get_clock(), 36 | rclcpp::Duration(std::chrono::duration(1 / publish_rate)), [this]() { 37 | std::lock_guard lock{this->mutex_}; 38 | 39 | // Set header timestamp. 40 | this->msg_.header.stamp = now(); 41 | this->msg_.readings.clear(); 42 | this->msg_.readings.reserve(this->ir_intensities_.size()); 43 | for (const auto & ir_msgs : this->ir_intensities_) { 44 | this->msg_.readings.emplace_back(ir_msgs.second); 45 | } 46 | 47 | // Publish detected vector. 48 | this->publisher_->publish(this->msg_); 49 | }); 50 | 51 | // Set header frame_id. 52 | this->msg_.header.frame_id = "base_link"; 53 | 54 | // Create subscriptions 55 | for (std::string topic : subscription_topics_) { 56 | subs_vector_.push_back( 57 | (create_subscription( 58 | topic, rclcpp::SensorDataQoS(), 59 | [this](const irobot_create_msgs::msg::IrIntensity::SharedPtr msg) { 60 | std::lock_guard lock{this->mutex_}; 61 | this->ir_intensities_[msg->header.frame_id] = *msg; 62 | }))); 63 | RCLCPP_INFO_STREAM(get_logger(), "Subscription to topic: " << topic); 64 | } 65 | } 66 | 67 | } // namespace irobot_create_nodes 68 | 69 | RCLCPP_COMPONENTS_REGISTER_NODE(irobot_create_nodes::IrIntensityVectorPublisher) 70 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Clearpath Robotics, Inc. 2 | # @author Roni Kreinin (rkreinin@clearpathrobotics.com) 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument 8 | from launch.actions import IncludeLaunchDescription 9 | from launch.launch_description_sources import PythonLaunchDescriptionSource 10 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 11 | 12 | 13 | ARGUMENTS = [ 14 | DeclareLaunchArgument('namespace', default_value='', 15 | description='Robot namespace'), 16 | DeclareLaunchArgument('use_rviz', default_value='true', 17 | choices=['true', 'false'], description='Start rviz.'), 18 | DeclareLaunchArgument('world', default_value='depot', 19 | description='Ignition World'), 20 | ] 21 | 22 | for pose_element in ['x', 'y', 'z', 'yaw']: 23 | ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', 24 | description=f'{pose_element} component of the robot pose.')) 25 | 26 | 27 | def generate_launch_description(): 28 | 29 | # Directories 30 | pkg_irobot_create_gz_bringup = get_package_share_directory( 31 | 'irobot_create_gz_bringup') 32 | 33 | # Paths 34 | ignition_launch = PathJoinSubstitution( 35 | [pkg_irobot_create_gz_bringup, 'launch', 'sim.launch.py']) 36 | robot_spawn_launch = PathJoinSubstitution( 37 | [pkg_irobot_create_gz_bringup, 'launch', 'create3_spawn.launch.py']) 38 | create3_nodes_launch = PathJoinSubstitution( 39 | [pkg_irobot_create_gz_bringup, 'launch', 'create3_gz_nodes.launch.py']) 40 | 41 | ignition = IncludeLaunchDescription( 42 | PythonLaunchDescriptionSource([ignition_launch]), 43 | launch_arguments=[ 44 | ('world', LaunchConfiguration('world')) 45 | ] 46 | ) 47 | 48 | robot_spawn = IncludeLaunchDescription( 49 | PythonLaunchDescriptionSource([robot_spawn_launch]), 50 | launch_arguments=[ 51 | ('namespace', LaunchConfiguration('namespace')), 52 | ('use_rviz', LaunchConfiguration('use_rviz')), 53 | ('x', LaunchConfiguration('x')), 54 | ('y', LaunchConfiguration('y')), 55 | ('z', LaunchConfiguration('z')), 56 | ('yaw', LaunchConfiguration('yaw'))]) 57 | 58 | create3_nodes = IncludeLaunchDescription( 59 | PythonLaunchDescriptionSource([create3_nodes_launch]), 60 | launch_arguments=[]) 61 | 62 | # Create launch description and add actions 63 | ld = LaunchDescription(ARGUMENTS) 64 | ld.add_action(ignition) 65 | ld.add_action(robot_spawn) 66 | ld.add_action(create3_nodes) 67 | return ld 68 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/sensors/ir_intensity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "irobot_create_gz_toolbox/sensors/ir_intensity.hpp" 12 | #include "irobot_create_toolbox/math.hpp" 13 | 14 | using irobot_create_gz_toolbox::IrIntensity; 15 | 16 | IrIntensity::IrIntensity(std::shared_ptr & nh) 17 | : nh_(nh), 18 | ir_intensity_sensors_{ 19 | "front_center_left", 20 | "front_center_right", 21 | "front_left", 22 | "front_right", 23 | "left", 24 | "right", 25 | "side_left" 26 | } 27 | { 28 | auto ir_scan_sub_topics = 29 | nh_->declare_parameter("ir_scan_subscription_topics", std::vector()); 30 | 31 | auto ir_intensity_pub_topics = 32 | nh_->declare_parameter("ir_intensity_publish_topics", std::vector()); 33 | 34 | for (const std::string & topic : ir_scan_sub_topics) { 35 | ir_scan_sub_.push_back( 36 | nh_->create_subscription( 37 | topic, 38 | rclcpp::SensorDataQoS(), 39 | std::bind(&IrIntensity::ir_scan_callback, this, std::placeholders::_1))); 40 | } 41 | 42 | for (const std::string & topic : ir_intensity_pub_topics) { 43 | for (const std::string & sensor : ir_intensity_sensors_) { 44 | if (topic.find(sensor) != std::string::npos) { 45 | ir_intensity_pub_[sensor] = nh_->create_publisher< 46 | irobot_create_msgs::msg::IrIntensity>( 47 | topic, 48 | rclcpp::SensorDataQoS()); 49 | } 50 | } 51 | } 52 | } 53 | 54 | void IrIntensity::ir_scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr ir_msg) 55 | { 56 | auto ir_intensity_msg = irobot_create_msgs::msg::IrIntensity(); 57 | 58 | const double detection = 59 | std::min(irobot_create_toolbox::FindMinimumRange(ir_msg->ranges), ir_msg->range_max); 60 | 61 | // IR sensor produces an exponential signal that is correlated to the distance, 62 | // that follows this formula: ir_reading = A exp(-x*B) 63 | // where: 64 | // A is a coefficient that depends on the color surface and 65 | // it can be as high as 3500 66 | // B is the decay of the signal related to the distance. 67 | // From the experiments B ~ 26.831568 68 | const double scaled_detection = 3500 * std::exp(detection * (-2 * M_E / ir_msg->range_max)); 69 | ir_intensity_msg.value = static_cast( 70 | scaled_detection); 71 | 72 | // Publish to appropriate topic 73 | for (const std::string & sensor : ir_intensity_sensors_) { 74 | if (ir_msg->header.frame_id.find(sensor) != std::string::npos) { 75 | ir_intensity_pub_[sensor]->publish(ir_intensity_msg); 76 | } 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | # @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | # 4 | # Launch Create(R) 3 state publishers. 5 | 6 | from ament_index_python.packages import get_package_share_directory 7 | from launch import LaunchDescription 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import Command, PathJoinSubstitution 10 | from launch.substitutions.launch_configuration import LaunchConfiguration 11 | from launch_ros.actions import Node 12 | 13 | 14 | ARGUMENTS = [ 15 | DeclareLaunchArgument('gazebo', default_value='classic', 16 | choices=['classic', 'ignition'], 17 | description='Which gazebo simulator to use'), 18 | DeclareLaunchArgument('visualize_rays', default_value='false', 19 | choices=['true', 'false'], 20 | description='Enable/disable ray visualization'), 21 | DeclareLaunchArgument('namespace', default_value='', 22 | description='Robot namespace'), 23 | ] 24 | 25 | 26 | def generate_launch_description(): 27 | pkg_create3_description = get_package_share_directory('irobot_create_description') 28 | xacro_file = PathJoinSubstitution([pkg_create3_description, 'urdf', 'create3.urdf.xacro']) 29 | gazebo_simulator = LaunchConfiguration('gazebo') 30 | visualize_rays = LaunchConfiguration('visualize_rays') 31 | namespace = LaunchConfiguration('namespace') 32 | 33 | robot_state_publisher = Node( 34 | package='robot_state_publisher', 35 | executable='robot_state_publisher', 36 | name='robot_state_publisher', 37 | output='screen', 38 | parameters=[ 39 | {'use_sim_time': True}, 40 | {'robot_description': 41 | Command( 42 | ['xacro', ' ', xacro_file, ' ', 43 | 'gazebo:=', gazebo_simulator, ' ', 44 | 'visualize_rays:=', visualize_rays, ' ', 45 | 'namespace:=', namespace])}, 46 | ], 47 | remappings=[ 48 | ('/tf', 'tf'), 49 | ('/tf_static', 'tf_static') 50 | ] 51 | ) 52 | 53 | joint_state_publisher = Node( 54 | package='joint_state_publisher', 55 | executable='joint_state_publisher', 56 | name='joint_state_publisher', 57 | output='screen', 58 | parameters=[{'use_sim_time': True}], 59 | remappings=[ 60 | ('/tf', 'tf'), 61 | ('/tf_static', 'tf_static') 62 | ] 63 | ) 64 | 65 | # Define LaunchDescription variable 66 | ld = LaunchDescription(ARGUMENTS) 67 | 68 | # Add nodes to LaunchDescription 69 | ld.add_action(joint_state_publisher) 70 | ld.add_action(robot_state_publisher) 71 | 72 | return ld 73 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_control/config/control.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | controller_manager: 3 | ros__parameters: 4 | update_rate: 1000 # Hz 5 | 6 | joint_state_broadcaster: 7 | type: joint_state_broadcaster/JointStateBroadcaster 8 | 9 | diffdrive_controller: 10 | type: diff_drive_controller/DiffDriveController 11 | 12 | diffdrive_controller: 13 | ros__parameters: 14 | use_sim_time: True 15 | left_wheel_names: ["left_wheel_joint"] 16 | right_wheel_names: ["right_wheel_joint"] 17 | 18 | wheel_separation: 0.233 19 | wheel_radius: 0.03575 20 | 21 | # Multiplier applied to the wheel separation parameter. 22 | # This is used to account for a difference between the robot model and a real robot. 23 | wheel_separation_multiplier: 1.0 24 | 25 | # Multipliers applied to the wheel radius parameter. 26 | # This is used to account for a difference between the robot model and a real robot. 27 | left_wheel_radius_multiplier: 1.0 28 | right_wheel_radius_multiplier: 1.0 29 | 30 | publish_rate: 62.0 31 | odom_frame_id: odom 32 | base_frame_id: base_link 33 | pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] 34 | twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] 35 | 36 | open_loop: false 37 | enable_odom_tf: true 38 | 39 | cmd_vel_timeout: 0.5 40 | use_stamped_vel: false 41 | 42 | # Preserve turning radius when limiting speed/acceleration/jerk 43 | preserve_turning_radius: true 44 | 45 | # Publish limited velocity 46 | publish_cmd: true 47 | 48 | # Publish wheel data 49 | publish_wheel_data: true 50 | 51 | # Velocity and acceleration limits in cartesian 52 | # These limits do not factor in per wheel limits 53 | # that might be exceeded when linear and angular are combined 54 | # Whenever a min_* is unspecified, default to -max_* 55 | linear.x.has_velocity_limits: true 56 | linear.x.has_acceleration_limits: true 57 | linear.x.has_jerk_limits: false 58 | # This is max if system is safety_override "full" 59 | # motion_control_node will bound this to 0.306 if safety enabled (as is by default) 60 | linear.x.max_velocity: 0.46 61 | linear.x.min_velocity: -0.46 62 | linear.x.max_acceleration: 0.9 63 | # Not using jerk limits yet 64 | # linear.x.max_jerk: 0.0 65 | # linear.x.min_jerk: 0.0 66 | 67 | angular.z.has_velocity_limits: true 68 | angular.z.has_acceleration_limits: true 69 | angular.z.has_jerk_limits: false 70 | angular.z.max_velocity: 1.9 71 | angular.z.min_velocity: -1.9 72 | # Using 0.9 linear for each wheel, assuming one wheel accel to .9 73 | # and other to -.9 with wheelbase leads to 7.725 rad/s^2 74 | angular.z.max_acceleration: 7.725 75 | angular.z.min_acceleration: -7.725 76 | # Not using jerk limits yet 77 | # angular.z.max_jerk: 0.0 78 | # angular.z.min_jerk: 0.0 79 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/wheel_drop.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | false 40 | true 41 | 43 | ${spring_stiffness_nm} 44 | ${detection_threshold} 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | $(arg namespace) 56 | ~/out:=_internal/wheel_drop/${name}_wheel/event 57 | 58 | ${update_rate} 59 | ${detection_threshold} 60 | ${wd_joint_name} 61 | ${wd_link_name} 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Lola Segura (lsegura@irobot.com) 3 | 4 | #include "irobot_create_nodes/kidnap_estimator_publisher.hpp" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "rclcpp_components/register_node_macro.hpp" 11 | 12 | namespace irobot_create_nodes 13 | { 14 | KidnapEstimator::KidnapEstimator(const rclcpp::NodeOptions & options) 15 | : rclcpp::Node("kidnap_estimator", options) 16 | { 17 | // Topic parameter to publish kidnap status to 18 | kidnap_status_publisher_topic_ = 19 | this->declare_parameter("kidnap_status_topic", "kidnap_status"); 20 | 21 | // Subscriber topics 22 | hazard_subscription_topic_ = 23 | this->declare_parameter("hazard_topic", "hazard_detection"); 24 | 25 | // Define kidnap status publisher 26 | kidnap_status_publisher_ = create_publisher( 27 | kidnap_status_publisher_topic_, rclcpp::SensorDataQoS().reliable()); 28 | RCLCPP_INFO_STREAM(get_logger(), "Advertised topic: " << kidnap_status_publisher_topic_); 29 | 30 | // Subscription to the hazard detection vector 31 | kidnap_status_subscription_ = create_subscription( 32 | hazard_subscription_topic_, rclcpp::SensorDataQoS(), 33 | std::bind(&KidnapEstimator::kidnap_callback, this, std::placeholders::_1)); 34 | RCLCPP_INFO_STREAM(get_logger(), "Subscription to topic: " << hazard_subscription_topic_); 35 | 36 | // Set kidnap status header 37 | kidnap_status_msg_.header.frame_id = base_frame_; 38 | } 39 | 40 | void KidnapEstimator::kidnap_callback(irobot_create_msgs::msg::HazardDetectionVector::SharedPtr msg) 41 | { 42 | const auto hazard_vector = msg->detections; 43 | const std::size_t wheel_drop_count = std::count_if( 44 | hazard_vector.begin(), hazard_vector.end(), [](auto hazard_vector) { 45 | return hazard_vector.header.frame_id == "wheel_drop_left" || 46 | hazard_vector.header.frame_id == "wheel_drop_right"; 47 | }); 48 | 49 | const std::size_t cliff_sensor_count = std::count_if( 50 | hazard_vector.begin(), hazard_vector.end(), [](auto hazard_vector) { 51 | return hazard_vector.header.frame_id == "cliff_side_left" || 52 | hazard_vector.header.frame_id == "cliff_side_right" || 53 | hazard_vector.header.frame_id == "cliff_front_left" || 54 | hazard_vector.header.frame_id == "cliff_front_right"; 55 | }); 56 | 57 | // Set header timestamp. 58 | kidnap_status_msg_.header.stamp = now(); 59 | // Set kidnap status. The robot is kidnapped when both wheel drops 60 | // and four cliff sensors are triggered. 61 | kidnap_status_msg_.is_kidnapped = wheel_drop_count >= min_wheel_drop_count_ && 62 | cliff_sensor_count >= min_cliff_sensor_count_; 63 | 64 | // Publish topics 65 | kidnap_status_publisher_->publish(kidnap_status_msg_); 66 | } 67 | 68 | } // namespace irobot_create_nodes 69 | 70 | RCLCPP_COMPONENTS_REGISTER_NODE(irobot_create_nodes::KidnapEstimator) 71 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include "Create3Hmi.hh" 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace gz 18 | { 19 | 20 | namespace gui 21 | { 22 | 23 | Create3Hmi::Create3Hmi() 24 | : Plugin() 25 | { 26 | this->create3_button_pub_ = gz::transport::Node::Publisher(); 27 | this->create3_button_pub_ = 28 | this->node_.Advertise < gz::msgs::Int32 > (this->create3_button_topic_); 29 | } 30 | 31 | Create3Hmi::~Create3Hmi() 32 | { 33 | } 34 | 35 | void Create3Hmi::LoadConfig(const tinyxml2::XMLElement * _pluginElem) 36 | { 37 | if (this->title.empty()) { 38 | this->title = "Create3 HMI"; 39 | } 40 | 41 | if (_pluginElem) 42 | { 43 | auto namespaceElem = _pluginElem->FirstChildElement("namespace"); 44 | if (nullptr != namespaceElem && nullptr != namespaceElem->GetText()) 45 | this->SetNamespace(namespaceElem->GetText()); 46 | } 47 | 48 | this->connect( 49 | this, SIGNAL(AddMsg(QString)), this, SLOT(OnAddMsg(QString)), 50 | Qt::QueuedConnection); 51 | } 52 | 53 | void Create3Hmi::OnCreate3Button(const int button) 54 | { 55 | gz::msgs::Int32 button_msg; 56 | 57 | button_msg.set_data(button); 58 | 59 | if (!this->create3_button_pub_.Publish(button_msg)) { 60 | gzerr << "gz::msgs::Int32 message couldn't be published at topic: " << 61 | this->create3_button_topic_ << std::endl; 62 | } 63 | } 64 | 65 | QString Create3Hmi::Namespace() const 66 | { 67 | return QString::fromStdString(this->namespace_); 68 | } 69 | 70 | void Create3Hmi::SetNamespace(const QString &_name) 71 | { 72 | this->namespace_ = _name.toStdString(); 73 | this->create3_button_topic_ = this->namespace_ + "/create3_buttons"; 74 | 75 | gzlog << "A new robot name has been entered, publishing on topic: '" << 76 | this->create3_button_topic_ << " ' " <create3_button_pub_ = gz::transport::Node::Publisher(); 80 | this->create3_button_pub_ = 81 | this->node_.Advertise< gz::msgs::Int32 > 82 | (this->create3_button_topic_); 83 | if (!this->create3_button_pub_) 84 | { 85 | App()->findChild()->notifyWithDuration( 86 | QString::fromStdString("Error when advertising topic: " + 87 | this->create3_button_topic_), 4000); 88 | gzerr << "Error when advertising topic: " << 89 | this->create3_button_topic_ << std::endl; 90 | }else { 91 | App()->findChild()->notifyWithDuration( 92 | QString::fromStdString("Advertising topic: '" + 93 | this->create3_button_topic_ + "'"), 4000); 94 | } 95 | this->NamespaceChanged(); 96 | } 97 | 98 | } // namespace gui 99 | 100 | } // namespace gz 101 | 102 | // Register this plugin 103 | GZ_ADD_PLUGIN( 104 | gz::gui::Create3Hmi, 105 | gz::gui::Plugin) 106 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | # @author Emiliano Javier Borghi Orue (creativa_eborghi@irobot.com) 3 | # 4 | # Launch standard docking station state publishers. 5 | 6 | from ament_index_python.packages import get_package_share_directory 7 | from launch import LaunchDescription 8 | from launch.actions import DeclareLaunchArgument 9 | from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution 10 | from launch_ros.actions import Node 11 | 12 | ARGUMENTS = [ 13 | DeclareLaunchArgument('gazebo', default_value='classic', 14 | choices=['classic', 'ignition'], 15 | description='Which gazebo simulation to use'), 16 | DeclareLaunchArgument('visualize_rays', default_value='true', 17 | choices=['true', 'false'], 18 | description='Enable/disable ray visualization'), 19 | DeclareLaunchArgument('namespace', default_value='', 20 | description='Robot namespace'), 21 | ] 22 | 23 | 24 | def generate_launch_description(): 25 | # Directory 26 | pkg_create3_description = get_package_share_directory('irobot_create_description') 27 | # Path 28 | dock_xacro_file = PathJoinSubstitution( 29 | [pkg_create3_description, 'urdf', 'dock', 'standard_dock.urdf.xacro']) 30 | 31 | # Launch Configurations 32 | visualize_rays = LaunchConfiguration('visualize_rays') 33 | gazebo_simulator = LaunchConfiguration('gazebo') 34 | namespace = LaunchConfiguration('namespace') 35 | 36 | state_publisher = Node( 37 | package='robot_state_publisher', 38 | executable='robot_state_publisher', 39 | name='dock_state_publisher', 40 | output='screen', 41 | parameters=[ 42 | {'use_sim_time': True}, 43 | {'robot_description': 44 | Command( 45 | ['xacro', ' ', dock_xacro_file, ' ', 46 | 'gazebo:=', gazebo_simulator, ' ', 47 | 'namespace:=', namespace, ' ', 48 | 'visualize_rays:=', visualize_rays])}, 49 | ], 50 | remappings=[ 51 | ('robot_description', 'standard_dock_description'), 52 | ('/tf', 'tf'), 53 | ('/tf_static', 'tf_static') 54 | ], 55 | ) 56 | 57 | tf_odom_std_dock_link_publisher = Node( 58 | package='tf2_ros', 59 | executable='static_transform_publisher', 60 | name='tf_odom_std_dock_link_publisher', 61 | arguments=['0.157', '0', '0', 62 | # According to documentation (http://wiki.ros.org/tf2_ros): 63 | # the order is yaw, pitch, roll 64 | '3.141592', '0', '0', 65 | 'odom', 'std_dock_link'], 66 | remappings=[ 67 | ('/tf', 'tf'), 68 | ('/tf_static', 'tf_static') 69 | ], 70 | output='screen', 71 | ) 72 | 73 | # Define LaunchDescription variable 74 | ld = LaunchDescription(ARGUMENTS) 75 | # Add nodes to LaunchDescription 76 | ld.add_action(state_publisher) 77 | ld.add_action(tf_odom_std_dock_link_publisher) 78 | 79 | return ld 80 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package irobot_create_nodes 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.4 (2024-10-23) 6 | ------------------ 7 | 8 | 3.0.3 (2024-09-28) 9 | ------------------ 10 | 11 | 3.0.2 (2024-08-26) 12 | ------------------ 13 | 14 | 3.0.1 (2024-08-25) 15 | ------------------ 16 | 17 | 3.0.0 (2024-08-25) 18 | ------------------ 19 | * Jazzy updates (`#229 `_) 20 | * Wheel Ticks calculation code equation correction (`#222 `_) 21 | * Contributors: Adnan Saood, Alberto Soragna, Chris Iverach-Brereton 22 | 23 | 2.1.0 (2023-05-15) 24 | ------------------ 25 | * Multi robot support (`#207 `_) 26 | * Contributors: Roni Kreinin 27 | 28 | 2.0.0 (2023-01-19) 29 | ------------------ 30 | * Update to ROS 2 Humble (`#197 `_) 31 | * Update message names to https://github.com/iRobotEducation/irobot_create_msgs/pull/10 32 | * rename dock topic into dock_status 33 | * comment ign_ros2_control dependency as it must be built from sources 34 | Co-authored-by: Francisco Martín Rico 35 | * add missing dependency to irobot-create-common-bringup (`#186 `_) 36 | * Contributors: Alberto Soragna, Francisco Martín Rico 37 | 38 | 1.0.1 (2022-04-12) 39 | ------------------ 40 | * add boost as explicit dependency and remove unnneded boost usage (`#175 `_) 41 | * add boost as explicit dependency and remove unnneded boost usage 42 | * do not search for boost component in Boost library 43 | 44 | 1.0.0 (2022-03-31) 45 | ------------------ 46 | * Add audio action and move UI elements to ui_mgr node (`#172 `_) 47 | * Add audio notes sequence action and move UI elements from mock to UI mgr node 48 | * fix linter error 49 | * add missing yaml file 50 | * fix flake8 error 51 | * Removed reliable QoS from subscriptions (`#164 `_) 52 | * Update dependencies for build from source, throttle action feedback, add stub for audio command (`#161 `_) 53 | * Update dependencies for build from source 54 | * Add stub for cmd_audio and throttle action feedback 55 | * fix packages from PR feedback 56 | * Reliable QoS for publishers (`#158 `_) 57 | * reliable QoS for publishers 58 | * reliable QoS for publishers 59 | * Fixing tests 60 | * Split `irobot_create_toolbox` (`#153 `_) 61 | * rename irobot_create_toolbox into irobot_create_nodes 62 | * move common utilities to irobot_create_toolbox 63 | * register irobot_create_nodes as rclcpp_components 64 | * update readme 65 | * use new node names in parameter files 66 | * remove declare parameter utility and fix linter tests 67 | * Contributors: Alberto Soragna, Justin Kearns, Santti4go, roni-kreinin 68 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/launch/sim.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Clearpath Robotics, Inc. 2 | # @author Roni Kreinin (rkreinin@clearpathrobotics.com) 3 | 4 | import os 5 | 6 | from pathlib import Path 7 | 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | from launch import LaunchDescription 11 | from launch.actions import DeclareLaunchArgument 12 | from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable 13 | from launch.launch_description_sources import PythonLaunchDescriptionSource 14 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 15 | from launch_ros.actions import Node 16 | 17 | 18 | ARGUMENTS = [ 19 | DeclareLaunchArgument('use_sim_time', default_value='true', 20 | choices=['true', 'false'], 21 | description='use_sim_time'), 22 | DeclareLaunchArgument('world', default_value='depot', 23 | description='Ignition World'), 24 | ] 25 | 26 | 27 | def generate_launch_description(): 28 | 29 | # Directories 30 | pkg_irobot_create_gz_bringup = get_package_share_directory( 31 | 'irobot_create_gz_bringup') 32 | pkg_irobot_create_gz_plugins = get_package_share_directory( 33 | 'irobot_create_gz_plugins') 34 | pkg_irobot_create_description = get_package_share_directory( 35 | 'irobot_create_description') 36 | pkg_ros_gz_sim = get_package_share_directory( 37 | 'ros_gz_sim') 38 | 39 | # Set Ignition resource path 40 | gz_resource_path = SetEnvironmentVariable( 41 | name='GZ_SIM_RESOURCE_PATH', 42 | value=':'.join([ 43 | os.path.join(pkg_irobot_create_gz_bringup, 'worlds'), 44 | str(Path(pkg_irobot_create_description).parent.resolve()) 45 | ]) 46 | ) 47 | 48 | gz_gui_plugin_path = SetEnvironmentVariable( 49 | name='GZ_GUI_PLUGIN_PATH', 50 | value=':'.join([ 51 | os.path.join(pkg_irobot_create_gz_plugins, 'lib') 52 | ]) 53 | ) 54 | 55 | # Paths 56 | gz_sim_launch = PathJoinSubstitution( 57 | [pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py']) 58 | 59 | # Ignition gazebo 60 | gz_sim = IncludeLaunchDescription( 61 | PythonLaunchDescriptionSource([gz_sim_launch]), 62 | launch_arguments=[( 63 | 'gz_args', [ 64 | LaunchConfiguration('world'), 65 | '.sdf', 66 | ' -v 4', 67 | ' --gui-config ', 68 | PathJoinSubstitution( 69 | [pkg_irobot_create_gz_bringup, 'gui', 'create3', 'gui.config'] 70 | ) 71 | ] 72 | )] 73 | ) 74 | 75 | # clock bridge 76 | clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', 77 | name='clock_bridge', 78 | output='screen', 79 | arguments=[ 80 | '/clock' + '@rosgraph_msgs/msg/Clock' + '[gz.msgs.Clock' 81 | ]) 82 | 83 | # Create launch description and add actions 84 | ld = LaunchDescription(ARGUMENTS) 85 | ld.add_action(gz_resource_path) 86 | ld.add_action(gz_gui_plugin_path) 87 | ld.add_action(gz_sim) 88 | ld.add_action(clock_bridge) 89 | return ld 90 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include "irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp" 7 | #include 8 | 9 | using irobot_create_gz_toolbox::InterfaceButtons; 10 | 11 | InterfaceButtons::InterfaceButtons() 12 | : rclcpp::Node("sensors_node") 13 | { 14 | interface_buttons_sub_ = this->create_subscription( 15 | "_internal/create3_buttons", 16 | rclcpp::SensorDataQoS(), 17 | std::bind(&InterfaceButtons::create3_buttons_callback, this, std::placeholders::_1)); 18 | 19 | interface_buttons_pub_ = this->create_publisher( 20 | "interface_buttons", 21 | rclcpp::SensorDataQoS()); 22 | 23 | interface_buttons_ = std::make_unique(); 24 | } 25 | 26 | // Convert Int32 message to InterfaceButtons message 27 | void InterfaceButtons::create3_buttons_callback( 28 | const std_msgs::msg::Int32::SharedPtr 29 | create3_buttons_msg) 30 | { 31 | // User control not allowed when in STOP or RECOIL response 32 | switch (create3_buttons_msg->data) { 33 | // All buttons released 34 | case Create3Buttons::NONE: 35 | { 36 | if (interface_buttons_->button_1.is_pressed) { 37 | interface_buttons_->button_1.last_pressed_duration = 38 | this->get_clock()->now() - 39 | interface_buttons_->button_1.last_start_pressed_time; 40 | interface_buttons_->button_1.is_pressed = false; 41 | } 42 | if (interface_buttons_->button_power.is_pressed) { 43 | interface_buttons_->button_power.last_pressed_duration = 44 | this->get_clock()->now() - 45 | interface_buttons_->button_power.last_start_pressed_time; 46 | interface_buttons_->button_power.is_pressed = false; 47 | } 48 | if (interface_buttons_->button_2.is_pressed) { 49 | interface_buttons_->button_2.last_pressed_duration = 50 | this->get_clock()->now() - 51 | interface_buttons_->button_2.last_start_pressed_time; 52 | interface_buttons_->button_2.is_pressed = false; 53 | } 54 | break; 55 | } 56 | case Create3Buttons::BUTTON_1: 57 | { 58 | interface_buttons_->button_1.is_pressed = true; 59 | interface_buttons_->button_1.last_start_pressed_time = this->get_clock()->now(); 60 | break; 61 | } 62 | case Create3Buttons::BUTTON_POWER: 63 | { 64 | interface_buttons_->button_power.is_pressed = true; 65 | interface_buttons_->button_power.last_start_pressed_time = this->get_clock()->now(); 66 | break; 67 | } 68 | case Create3Buttons::BUTTON_2: 69 | { 70 | interface_buttons_->button_2.is_pressed = true; 71 | interface_buttons_->button_2.last_start_pressed_time = this->get_clock()->now(); 72 | break; 73 | } 74 | default: 75 | { 76 | RCLCPP_ERROR( 77 | this->get_logger(), "Invalid create3 button %d", 78 | create3_buttons_msg->data); 79 | break; 80 | } 81 | } 82 | // Make copy of interface_buttons_ to publish 83 | auto interface_buttons_msg = *interface_buttons_; 84 | interface_buttons_pub_->publish(interface_buttons_msg); 85 | } 86 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_opcode.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_OPCODE_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_OPCODE_HPP_ 8 | 9 | #include 10 | #include 11 | 12 | #include "irobot_create_msgs/msg/ir_opcode.hpp" 13 | #include "irobot_create_msgs/msg/dock_status.hpp" 14 | #include "nav_msgs/msg/odometry.hpp" 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 17 | 18 | #include "irobot_create_gz_toolbox/utils.hpp" 19 | #include "irobot_create_toolbox/polar_coordinates.hpp" 20 | 21 | namespace irobot_create_gz_toolbox 22 | { 23 | 24 | class IrOpcode 25 | { 26 | public: 27 | explicit IrOpcode(std::shared_ptr & nh); 28 | virtual ~IrOpcode() {} 29 | 30 | private: 31 | // Robot receiver parameters 32 | // Index 0: Sensor 0, the omnidirectional receiver 33 | // Index 1: Sensor 1, is the forward facing receiver 34 | struct SensorParams 35 | { 36 | double fov; 37 | double range; 38 | }; 39 | 40 | // Dock emitter parameters 41 | const double DOCK_BUOYS_FOV_ = 50 * M_PI / 180; // Convert to radians 42 | const double DOCK_BUOY_FOV_RATIO_ = 0.6; // Red, green Buoy is 0.6 times the total fov. 43 | const double DOCK_BUOYS_RANGE_ = 1.0; 44 | const double DOCK_HALO_RANGE_ = 0.6096; 45 | 46 | // Docked thresholds 47 | const double DOCKED_DISTANCE = 0.075; // Max distance in meters. 48 | const double DOCKED_YAW = M_PI / 30.0; // Max Yaw between dock and robot in radians. 49 | 50 | // Convert cartesian point to polar point 51 | irobot_create_toolbox::PolarCoordinate EmitterCartesianPointToReceiverPolarPoint( 52 | const tf2::Vector3 & 53 | emitter_point); 54 | irobot_create_toolbox::PolarCoordinate ReceiverCartesianPointToEmitterPolarPoint( 55 | const tf2::Vector3 & 56 | receiver_point); 57 | 58 | // Check dock visibility and return the associated opcode 59 | int CheckBuoysDetection(const double fov, const double range); 60 | int CheckForceFieldDetection(const double fov, const double range); 61 | 62 | // Publish sensors 63 | void PublishSensors(const std::array detected_opcodes); 64 | 65 | // Subscription callbacks 66 | void emitter_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); 67 | void receiver_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); 68 | void robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); 69 | void dock_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); 70 | 71 | std::shared_ptr nh_; 72 | 73 | rclcpp::TimerBase::SharedPtr ir_opcode_timer_; 74 | rclcpp::TimerBase::SharedPtr dock_status_timer_; 75 | 76 | rclcpp::Publisher::SharedPtr ir_opcode_pub_; 77 | rclcpp::Publisher::SharedPtr dock_pub_; 78 | 79 | rclcpp::Subscription::SharedPtr emitter_pose_sub_; 80 | rclcpp::Subscription::SharedPtr receiver_pose_sub_; 81 | 82 | std::array sensors_; 83 | std::array detected_forcefield_opcodes_; 84 | std::array detected_buoys_opcodes_; 85 | 86 | tf2::Transform last_emitter_pose_; 87 | tf2::Transform last_receiver_pose_; 88 | 89 | std::mutex emitter_pose_mutex_; 90 | std::mutex receiver_pose_mutex_; 91 | 92 | bool is_docked_ = false; 93 | bool is_dock_visible_ = false; 94 | }; 95 | 96 | } // namespace irobot_create_gz_toolbox 97 | 98 | #endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_OPCODE_HPP_ 99 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_sim/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package irobot_create_ignition_sim 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.4 (2024-10-23) 6 | ------------------ 7 | 8 | 3.0.3 (2024-09-28) 9 | ------------------ 10 | 11 | 3.0.2 (2024-08-26) 12 | ------------------ 13 | 14 | 3.0.1 (2024-08-25) 15 | ------------------ 16 | 17 | 3.0.0 (2024-08-25) 18 | ------------------ 19 | * Jazzy updates (`#229 `_) 20 | * Contributors: Chris Iverach-Brereton 21 | 22 | 2.1.0 (2023-05-15) 23 | ------------------ 24 | 25 | 2.0.0 (2023-01-19) 26 | ------------------ 27 | * fix dependency tree of create3_sim packages (`#177 `_) 28 | * Contributors: Alberto Soragna 29 | 30 | 1.0.1 (2022-04-12) 31 | ------------------ 32 | * add boost as explicit dependency and remove unnneded boost usage (`#175 `_) 33 | * add boost as explicit dependency and remove unnneded boost usage 34 | * do not search for boost component in Boost library 35 | 36 | 1.0.0 (2022-03-31) 37 | ------------------ 38 | * Split `irobot_create_toolbox` (`#153 `_) 39 | * rename irobot_create_toolbox into irobot_create_nodes 40 | * move common utilities to irobot_create_toolbox 41 | * register irobot_create_nodes as rclcpp_components 42 | * update readme 43 | * use new node names in parameter files 44 | * remove declare parameter utility and fix linter tests 45 | * Ignition Gazebo support (`#144 `_) 46 | * Updated URDF to work with both gazebo classic and ignition: 47 | - Added gazebo arg to create3.urdf.xacro and standard_dock.urdf.xacro. This arg sets which gazebo version to use. 48 | - Added ray sensor macro which creates the correct sensor given the gazebo arg 49 | - Added ignition plugins when gazebo=ignition 50 | - Adjusted front caster position to better align with the create3 model 51 | - Adjusted wheeldrop spring stiffness to compensate for the front caster position change 52 | * Launch joint_state_publisher and diffdrive_controller only in classic 53 | * Use joint_state_publisher for both ignition and classic 54 | Cleaned up gazebo launch arg 55 | Adjusted front caster position 56 | * Simulation -> simulator 57 | * Added irobot_create_ignition packages 58 | * Fixed some linter warnings 59 | * Removed joint state publisher from ros_ign_bridge 60 | * - Reorganized packages 61 | - Shifted center of gravity of create3 forwards by 22.8 mm 62 | - Added min/max velocity and acceleration to diff drive plugin 63 | * Moved README.md to irobot_create_gazebo 64 | Created new README.md for irobot_create_ignition 65 | * Update README.md 66 | * Added ignition edifice repos for source installation 67 | * Create README.md 68 | * Added missing dependency 69 | * Renamed dock to standard_dock 70 | Center of gravity offset only applied in ignition for now 71 | * Fixed Linter errors 72 | * Updated README to have installation and example instructions for both Ignition and Classic 73 | Moved .repos files to the root of the repository 74 | * Ignition and Gazebo packages are now optional and only built if the required dependencies are installed 75 | * Made ros_ign_interfaces optional in irobot_create_ignition_bringup 76 | * fix license and minor changes to CMake and README 77 | * Interface buttons mock publisher not used in Ignition sim 78 | * Fixed linter errors 79 | Co-authored-by: Alberto Soragna 80 | * Contributors: Alberto Soragna, roni-kreinin 81 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.qml: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | import QtQuick 2.9 7 | import QtQuick.Controls 2.2 8 | import QtQuick.Controls.Material 2.1 9 | import QtQuick.Controls.Styles 1.4 10 | import QtQuick.Layouts 1.3 11 | import "qrc:/qml" 12 | 13 | Rectangle 14 | { 15 | id: widgetRectangle 16 | color: "white" 17 | anchors.fill: parent 18 | focus: true 19 | Layout.minimumWidth: 400 20 | Layout.minimumHeight: 225 21 | 22 | Rectangle 23 | { 24 | id: create3ButtonsRectangle 25 | border.width: 2 26 | anchors.top: widgetRectangle.top 27 | anchors.left: widgetRectangle.left 28 | focus: true 29 | height: 175 30 | width: 400 31 | 32 | // Robot namespace input 33 | Label { 34 | id: namespaceLabel 35 | text: "Namespace:" 36 | Layout.fillWidth: true 37 | Layout.margins: 10 38 | anchors.top: create3ButtonsRectangle.top 39 | anchors.topMargin: 10 40 | anchors.left: create3ButtonsRectangle.left 41 | anchors.leftMargin: 10 42 | } 43 | 44 | TextField { 45 | id: nameField 46 | width: 175 47 | Layout.fillWidth: true 48 | Layout.margins: 10 49 | text: Create3Hmi.namespace 50 | placeholderText: qsTr("Robot namespace") 51 | anchors.top: namespaceLabel.bottom 52 | anchors.topMargin: 5 53 | anchors.left: create3ButtonsRectangle.left 54 | anchors.leftMargin: 10 55 | onEditingFinished: { 56 | Create3Hmi.SetNamespace(text) 57 | } 58 | } 59 | 60 | // Button inputs 61 | ToolButton { 62 | id: create3Button1 63 | anchors.verticalCenter: create3ButtonPower.verticalCenter 64 | anchors.right: create3ButtonPower.left 65 | anchors.rightMargin: 15 66 | checkable: true 67 | checked: true 68 | contentItem: Image { 69 | fillMode: Image.Pad 70 | horizontalAlignment: Image.AlignHCenter 71 | verticalAlignment: Image.AlignVCenter 72 | source: "images/One Dot.png" 73 | sourceSize.width: 36; 74 | sourceSize.height: 36; 75 | } 76 | onPressed: { Create3Hmi.OnCreate3Button(1); } 77 | onReleased: { Create3Hmi.OnCreate3Button(0); } 78 | } 79 | 80 | ToolButton { 81 | id: create3ButtonPower 82 | anchors.bottom: create3ButtonsRectangle.bottom 83 | anchors.bottomMargin: 15 84 | anchors.horizontalCenter: create3ButtonsRectangle.horizontalCenter 85 | checkable: true 86 | checked: true 87 | contentItem: Image { 88 | fillMode: Image.Pad 89 | horizontalAlignment: Image.AlignHCenter 90 | verticalAlignment: Image.AlignVCenter 91 | source: "images/Power.png" 92 | sourceSize.width: 72; 93 | sourceSize.height: 72; 94 | } 95 | onPressed: { Create3Hmi.OnCreate3Button(2); } 96 | onReleased: { Create3Hmi.OnCreate3Button(0); } 97 | } 98 | 99 | ToolButton { 100 | id: create3Button2 101 | anchors.verticalCenter: create3ButtonPower.verticalCenter 102 | anchors.left: create3ButtonPower.right 103 | anchors.leftMargin: 15 104 | checkable: true 105 | checked: true 106 | contentItem: Image { 107 | fillMode: Image.Pad 108 | horizontalAlignment: Image.AlignHCenter 109 | verticalAlignment: Image.AlignVCenter 110 | source: "images/Two Dots.png" 111 | sourceSize.width: 36; 112 | sourceSize.height: 36; 113 | } 114 | onPressed: { Create3Hmi.OnCreate3Button(3); } 115 | onReleased: { Create3Hmi.OnCreate3Button(0); } 116 | } 117 | } 118 | } 119 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #ifndef IROBOT_CREATE_GZ_TOOLBOX__UTILS_HPP_ 7 | #define IROBOT_CREATE_GZ_TOOLBOX__UTILS_HPP_ 8 | 9 | #include 10 | #include 11 | 12 | #include "nav_msgs/msg/odometry.hpp" 13 | #include "rclcpp/rclcpp.hpp" 14 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 15 | #include "tf2_msgs/msg/tf_message.hpp" 16 | 17 | namespace irobot_create_gz_toolbox 18 | { 19 | 20 | namespace utils 21 | { 22 | 23 | // Get yaw from tf2 Transform 24 | inline double tf2_transform_to_yaw(const tf2::Transform tf) 25 | { 26 | auto tf_m = tf2::Matrix3x3(tf.getRotation()); 27 | double tf_r, tf_p, tf_y; 28 | tf_m.getRPY(tf_r, tf_p, tf_y); 29 | return tf_y; 30 | } 31 | 32 | // Get object position wrt frame 33 | inline tf2::Vector3 object_wrt_frame(tf2::Transform object, tf2::Transform frame) 34 | { 35 | return frame.inverseTimes(object).getOrigin(); 36 | } 37 | 38 | // Get global transform of a static link connected to base frame 39 | inline tf2::Transform static_link_wrt_global_frame( 40 | tf2::Transform static_link, 41 | tf2::Transform base_frame) 42 | { 43 | tf2::Transform global_pose(tf2::Transform::getIdentity()); 44 | 45 | double base_y = tf2_transform_to_yaw(base_frame); 46 | 47 | // Rotate static link frame by the base yaw 48 | global_pose.getOrigin().setX( 49 | cos(base_y) * static_link.getOrigin().getX() - 50 | sin(base_y) * static_link.getOrigin().getY()); 51 | global_pose.getOrigin().setY( 52 | sin(base_y) * static_link.getOrigin().getX() + 53 | cos(base_y) * static_link.getOrigin().getY()); 54 | 55 | // Static link pose is relative to base pose, so add the base pose to get global pose 56 | global_pose.setOrigin(global_pose.getOrigin() + base_frame.getOrigin()); 57 | // Get global rotation 58 | tf2::Quaternion rotation; 59 | rotation.setRPY(0.0, 0.0, tf2_transform_to_yaw(global_pose) + tf2_transform_to_yaw(base_frame)); 60 | global_pose.setRotation(rotation); 61 | 62 | return global_pose; 63 | } 64 | 65 | inline nav_msgs::msg::Odometry::UniquePtr tf_message_to_odom( 66 | const tf2_msgs::msg::TFMessage::SharedPtr msg, uint16_t i) 67 | { 68 | auto odom_msg = std::make_unique(); 69 | 70 | odom_msg->header = msg->transforms[i].header; 71 | odom_msg->child_frame_id = msg->transforms[i].child_frame_id; 72 | 73 | odom_msg->pose.pose.position.x = msg->transforms[i].transform.translation.x; 74 | odom_msg->pose.pose.position.y = msg->transforms[i].transform.translation.y; 75 | odom_msg->pose.pose.position.z = msg->transforms[i].transform.translation.z; 76 | 77 | odom_msg->pose.pose.orientation.x = msg->transforms[i].transform.rotation.x; 78 | odom_msg->pose.pose.orientation.y = msg->transforms[i].transform.rotation.y; 79 | odom_msg->pose.pose.orientation.z = msg->transforms[i].transform.rotation.z; 80 | odom_msg->pose.pose.orientation.w = msg->transforms[i].transform.rotation.w; 81 | 82 | return odom_msg; 83 | } 84 | 85 | inline void tf2_transform_to_pose(const tf2::Transform tf, geometry_msgs::msg::Pose & pose) 86 | { 87 | pose.position.set__x(tf.getOrigin().getX()); 88 | pose.position.set__y(tf.getOrigin().getY()); 89 | pose.position.set__z(tf.getOrigin().getZ()); 90 | 91 | pose.orientation.set__x(tf.getRotation().getX()); 92 | pose.orientation.set__y(tf.getRotation().getY()); 93 | pose.orientation.set__z(tf.getRotation().getZ()); 94 | pose.orientation.set__w(tf.getRotation().getW()); 95 | } 96 | 97 | 98 | } // namespace utils 99 | 100 | } // namespace irobot_create_gz_toolbox 101 | 102 | #endif // IROBOT_CREATE_GZ_TOOLBOX__UTILS_HPP_ 103 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 0.1 44 | 0.1 45 | 0.01 46 | 0.001 47 | 10 48 | Gazebo/DarkGrey 49 | 50 | true 51 | ${update_rate} 52 | 53 | ${link_name}_collision 54 | 55 | 56 | /bumper_contact 57 | 58 | 59 | ${namespace}/bumper_contact 60 | 61 | 62 | 63 | 64 | 65 | 66 | ${namespace} 67 | ~/out:=_internal/bumper/event 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | true 76 | 77 | 78 | 80 | 81 | 82 | 83 | 85 | 86 | 87 | 88 | 90 | 91 | 92 | 93 | 95 | 96 | 97 | 98 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_toolbox/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package irobot_create_toolbox 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.4 (2024-10-23) 6 | ------------------ 7 | * change gz_math_vendor from buildtool dependency to regular dependency 8 | * Contributors: Alberto Soragna 9 | 10 | 3.0.3 (2024-09-28) 11 | ------------------ 12 | * Generalize gz vendor use and modernize CMake (`#232 `_) 13 | * Contributors: Jose Luis Rivero 14 | 15 | 3.0.2 (2024-08-26) 16 | ------------------ 17 | 18 | 3.0.1 (2024-08-25) 19 | ------------------ 20 | 21 | 3.0.0 (2024-08-25) 22 | ------------------ 23 | * Jazzy updates (`#229 `_) 24 | * Contributors: Alberto Soragna, Chris Iverach-Brereton 25 | 26 | 2.1.0 (2023-05-15) 27 | ------------------ 28 | 29 | 2.0.0 (2023-01-19) 30 | ------------------ 31 | 32 | 1.0.1 (2022-04-12) 33 | ------------------ 34 | * add boost as explicit dependency and remove unnneded boost usage (`#175 `_) 35 | * add boost as explicit dependency and remove unnneded boost usage 36 | * do not search for boost component in Boost library 37 | 38 | 1.0.0 (2022-03-31) 39 | ------------------ 40 | * Split `irobot_create_toolbox` (`#153 `_) 41 | * rename irobot_create_toolbox into irobot_create_nodes 42 | * move common utilities to irobot_create_toolbox 43 | * register irobot_create_nodes as rclcpp_components 44 | * update readme 45 | * use new node names in parameter files 46 | * remove declare parameter utility and fix linter tests 47 | * Ignition Gazebo support (`#144 `_) 48 | * Updated URDF to work with both gazebo classic and ignition: 49 | - Added gazebo arg to create3.urdf.xacro and standard_dock.urdf.xacro. This arg sets which gazebo version to use. 50 | - Added ray sensor macro which creates the correct sensor given the gazebo arg 51 | - Added ignition plugins when gazebo=ignition 52 | - Adjusted front caster position to better align with the create3 model 53 | - Adjusted wheeldrop spring stiffness to compensate for the front caster position change 54 | * Launch joint_state_publisher and diffdrive_controller only in classic 55 | * Use joint_state_publisher for both ignition and classic 56 | Cleaned up gazebo launch arg 57 | Adjusted front caster position 58 | * Simulation -> simulator 59 | * Added irobot_create_ignition packages 60 | * Fixed some linter warnings 61 | * Removed joint state publisher from ros_ign_bridge 62 | * - Reorganized packages 63 | - Shifted center of gravity of create3 forwards by 22.8 mm 64 | - Added min/max velocity and acceleration to diff drive plugin 65 | * Moved README.md to irobot_create_gazebo 66 | Created new README.md for irobot_create_ignition 67 | * Update README.md 68 | * Added ignition edifice repos for source installation 69 | * Create README.md 70 | * Added missing dependency 71 | * Renamed dock to standard_dock 72 | Center of gravity offset only applied in ignition for now 73 | * Fixed Linter errors 74 | * Updated README to have installation and example instructions for both Ignition and Classic 75 | Moved .repos files to the root of the repository 76 | * Ignition and Gazebo packages are now optional and only built if the required dependencies are installed 77 | * Made ros_ign_interfaces optional in irobot_create_ignition_bringup 78 | * fix license and minor changes to CMake and README 79 | * Interface buttons mock publisher not used in Ignition sim 80 | * Fixed linter errors 81 | Co-authored-by: Alberto Soragna 82 | * Contributors: Alberto Soragna, roni-kreinin 83 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/src/sensors/bumper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Clearpath Robotics, Inc. 3 | * @author Roni Kreinin (rkreinin@clearpathrobotics.com) 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | #include "irobot_create_gz_toolbox/sensors/bumper.hpp" 10 | #include "irobot_create_gz_toolbox/utils.hpp" 11 | #include "irobot_create_toolbox/math.hpp" 12 | #include "irobot_create_toolbox/polar_coordinates.hpp" 13 | #include "irobot_create_toolbox/sensors/bumpers.hpp" 14 | 15 | using irobot_create_gz_toolbox::Bumper; 16 | 17 | Bumper::Bumper(std::shared_ptr & nh) 18 | : nh_(nh) 19 | { 20 | bumper_sub_ = nh_->create_subscription( 21 | "bumper_contact", 22 | rclcpp::SensorDataQoS(), 23 | std::bind(&Bumper::bumper_callback, this, std::placeholders::_1)); 24 | 25 | robot_pose_sub_ = nh_->create_subscription( 26 | "sim_ground_truth_pose", 27 | rclcpp::SensorDataQoS(), 28 | std::bind(&Bumper::robot_pose_callback, this, std::placeholders::_1)); 29 | 30 | hazard_pub_ = nh_->create_publisher( 31 | "_internal/bumper/event", rclcpp::SensorDataQoS()); 32 | } 33 | 34 | void Bumper::bumper_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr bumper_contact_msg) 35 | { 36 | tf2::Transform robot_pose(tf2::Transform::getIdentity()); 37 | { 38 | const std::lock_guard lock(robot_pose_mutex_); 39 | robot_pose = last_robot_pose_; 40 | } 41 | 42 | // Get rotation of robot in RPY 43 | auto robot_q = robot_pose.getRotation(); 44 | auto robot_m = tf2::Matrix3x3(robot_q); 45 | double robot_r, robot_p, robot_y; 46 | robot_m.getRPY(robot_r, robot_p, robot_y); 47 | 48 | // The bumper may be in contact with multiple objects simultaneously 49 | for (const auto & contact : bumper_contact_msg->contacts) { 50 | uint16_t count = 0; 51 | geometry_msgs::msg::Vector3 average_position; 52 | 53 | for (const auto position : contact.positions) { 54 | average_position.x += position.x; 55 | average_position.y += position.y; 56 | average_position.z += position.z; 57 | count++; 58 | } 59 | 60 | if (count != 0) { 61 | // Get average 62 | average_position.x /= count; 63 | average_position.y /= count; 64 | average_position.z /= count; 65 | } 66 | 67 | tf2::Transform average_pose; 68 | average_pose.setOrigin( 69 | tf2::Vector3( 70 | average_position.x, 71 | average_position.y, 72 | average_position.z)); 73 | 74 | tf2::Vector3 contact_point = utils::object_wrt_frame(average_pose, robot_pose); 75 | gz::math::Vector2d cartesian_coord = {contact_point[0], contact_point[1]}; 76 | auto azimuth = irobot_create_toolbox::toPolar(cartesian_coord).azimuth; 77 | 78 | // Find contact zone 79 | const auto iter = std::find_if( 80 | irobot_create_toolbox::sensors::BUMPER_ZONES_MAP.begin(), 81 | irobot_create_toolbox::sensors::BUMPER_ZONES_MAP.end(), 82 | [this, azimuth](const auto & zone) -> bool { 83 | return irobot_create_toolbox::IsAngleBetween( 84 | zone.second.left_limit, zone.second.right_limit, azimuth); 85 | }); 86 | 87 | if (iter != irobot_create_toolbox::sensors::BUMPER_ZONES_MAP.end()) { 88 | auto hazard_msg = irobot_create_msgs::msg::HazardDetection(); 89 | hazard_msg.type = irobot_create_msgs::msg::HazardDetection::BUMP; 90 | hazard_msg.header.frame_id = iter->second.name; 91 | hazard_msg.header.stamp = nh_->now(); 92 | hazard_pub_->publish(std::move(hazard_msg)); 93 | } 94 | } 95 | } 96 | 97 | void Bumper::robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg) 98 | { 99 | const std::lock_guard lock(robot_pose_mutex_); 100 | tf2::convert(msg->pose.pose, last_robot_pose_); 101 | } 102 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/behaviors_scheduler.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Alberto Soragna (asoragna@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__MOTION_CONTROL__BEHAVIORS_SCHEDULER_HPP_ 5 | #define IROBOT_CREATE_NODES__MOTION_CONTROL__BEHAVIORS_SCHEDULER_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "boost/optional.hpp" 12 | #include "geometry_msgs/msg/twist_stamped.hpp" 13 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 14 | #include "tf2/LinearMath/Transform.h" 15 | 16 | namespace irobot_create_nodes 17 | { 18 | 19 | struct RobotState 20 | { 21 | tf2::Transform pose; 22 | irobot_create_msgs::msg::HazardDetectionVector hazards; 23 | }; 24 | 25 | /** 26 | * @brief This class manages the execution of a single behavior at the time. 27 | */ 28 | class BehaviorsScheduler 29 | { 30 | public: 31 | using optional_output_t = boost::optional; 32 | using run_behavior_func_t = std::function; 33 | using is_done_func_t = std::function; 34 | using cleanup_func_t = std::function; 35 | 36 | // A behavior implementation needs to provide the following data in order to be used 37 | struct BehaviorsData 38 | { 39 | // This function is executed each iteration to generate new velocity commands 40 | run_behavior_func_t run_func; 41 | // This function is executed at the end of each iteration to know if we need to run again 42 | is_done_func_t is_done_func; 43 | // This function is executed when a behavior is stopped externally 44 | cleanup_func_t cleanup_func; 45 | // Whether a new behavior should stop this behavior (true) or if behavior should prevent 46 | // new ones from running till finished (false) 47 | bool stop_on_new_behavior; 48 | // Whether backup limits should be applied to behavior 49 | bool apply_backup_limits; 50 | }; 51 | 52 | BehaviorsScheduler() 53 | { 54 | } 55 | 56 | bool set_behavior(const BehaviorsData & data) 57 | { 58 | std::unique_lock lock(mutex_); 59 | 60 | if (!data.run_func || !data.is_done_func) { 61 | return false; 62 | } 63 | 64 | // If already has behavior, only take new behavior if old behavior is stop_on_new_behavior 65 | // and new behavior is not 66 | if (has_behavior_) { 67 | if (!current_behavior_.stop_on_new_behavior) { 68 | // Reject new behavior to keep running current 69 | return false; 70 | } else if (current_behavior_.cleanup_func) { 71 | // If behavior has to cleanup before moving onto another, run func 72 | current_behavior_.cleanup_func(); 73 | } 74 | } 75 | 76 | has_behavior_ = true; 77 | current_behavior_ = data; 78 | return true; 79 | } 80 | 81 | bool has_behavior() 82 | { 83 | return has_behavior_; 84 | } 85 | 86 | bool apply_backup_limits() 87 | { 88 | std::unique_lock lock(mutex_); 89 | return current_behavior_.apply_backup_limits; 90 | } 91 | 92 | bool stop_on_new_behavior() 93 | { 94 | std::unique_lock lock(mutex_); 95 | return current_behavior_.stop_on_new_behavior; 96 | } 97 | 98 | optional_output_t run_behavior(const RobotState & current_state) 99 | { 100 | if (!has_behavior_) { 101 | return optional_output_t(); 102 | } 103 | 104 | std::unique_lock lock(mutex_); 105 | optional_output_t output = current_behavior_.run_func(current_state); 106 | 107 | if (current_behavior_.is_done_func()) { 108 | has_behavior_ = false; 109 | } 110 | 111 | return output; 112 | } 113 | 114 | private: 115 | std::mutex mutex_; 116 | std::atomic has_behavior_ {false}; 117 | BehaviorsData current_behavior_; 118 | }; 119 | 120 | } // namespace irobot_create_nodes 121 | #endif // IROBOT_CREATE_NODES__MOTION_CONTROL__BEHAVIORS_SCHEDULER_HPP_ 122 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/wall_follow_behavior.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Justin Kearns (jkearns@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__MOTION_CONTROL__WALL_FOLLOW_BEHAVIOR_HPP_ 5 | #define IROBOT_CREATE_NODES__MOTION_CONTROL__WALL_FOLLOW_BEHAVIOR_HPP_ 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "irobot_create_msgs/action/wall_follow.hpp" 15 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 16 | #include "irobot_create_msgs/msg/ir_intensity_vector.hpp" 17 | #include "irobot_create_nodes/motion_control/behaviors_scheduler.hpp" 18 | #include "irobot_create_nodes/motion_control/wall_follow_states.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "rclcpp_action/rclcpp_action.hpp" 21 | 22 | namespace irobot_create_nodes 23 | { 24 | 25 | /** 26 | * @brief This class allows to create and manage the WallFollow action 27 | * server. Uses a simplistic wall follow implementation that is not intended 28 | * to exactly mimic the behavior of the robot. The robot will follow tighter 29 | * than this simple implementation. 30 | */ 31 | class WallFollowBehavior 32 | { 33 | public: 34 | WallFollowBehavior( 35 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, 36 | rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, 37 | rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, 38 | rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, 39 | rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, 40 | std::shared_ptr behavior_scheduler); 41 | ~WallFollowBehavior() = default; 42 | 43 | void hazard_vector_callback(irobot_create_msgs::msg::HazardDetectionVector::ConstSharedPtr msg); 44 | 45 | private: 46 | bool wall_follow_behavior_is_done(); 47 | 48 | void ir_intensity_callback(irobot_create_msgs::msg::IrIntensityVector::ConstSharedPtr msg); 49 | 50 | /// Get velocity command to follow wall given robot position and sensor readings 51 | BehaviorsScheduler::optional_output_t get_next_servo_cmd(const RobotState & current_state); 52 | 53 | rclcpp_action::GoalResponse handle_wall_follow_goal( 54 | const rclcpp_action::GoalUUID & uuid, 55 | std::shared_ptr goal); 56 | 57 | rclcpp_action::CancelResponse handle_wall_follow_cancel( 58 | const std::shared_ptr< 59 | rclcpp_action::ServerGoalHandle> goal_handle); 60 | 61 | void handle_wall_follow_accepted( 62 | const std::shared_ptr< 63 | rclcpp_action::ServerGoalHandle> goal_handle); 64 | 65 | BehaviorsScheduler::optional_output_t execute_wall_follow( 66 | const std::shared_ptr< 67 | rclcpp_action::ServerGoalHandle> goal_handle, 68 | const RobotState & current_state); 69 | 70 | rclcpp_action::Server::SharedPtr 71 | wall_follow_action_server_; 72 | 73 | rclcpp::Subscription::SharedPtr ir_intensity_sub_; 74 | 75 | rclcpp::Logger logger_; 76 | rclcpp::Clock::SharedPtr clock_ {nullptr}; 77 | std::shared_ptr behavior_scheduler_ {nullptr}; 78 | std::atomic wf_running_ {false}; 79 | std::atomic wf_engaged_ {false}; 80 | std::atomic wf_side_ {0}; 81 | rclcpp::Duration wf_end_duration_; 82 | rclcpp::Time wf_start_time_; 83 | std::mutex sensor_mutex_; 84 | irobot_create_msgs::msg::IrIntensityVector last_ir_intensity_; 85 | std::shared_ptr wf_state_mgr_ {nullptr}; 86 | rclcpp::Time last_feedback_time_; 87 | const rclcpp::Duration report_feedback_interval_ {std::chrono::seconds(3)}; 88 | }; 89 | 90 | } // namespace irobot_create_nodes 91 | #endif // IROBOT_CREATE_NODES__MOTION_CONTROL__WALL_FOLLOW_BEHAVIOR_HPP_ 92 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_control/launch/include/control.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | # @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) 3 | # 4 | # Launch Create(R) 3 with diffdrive controller in Gazebo and optionally also in RViz. 5 | 6 | from ament_index_python.packages import get_package_share_directory 7 | from launch import LaunchDescription 8 | from launch.actions import DeclareLaunchArgument, RegisterEventHandler 9 | from launch.conditions import IfCondition 10 | from launch.event_handlers import OnProcessExit 11 | from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PathJoinSubstitution 12 | from launch_ros.actions import Node 13 | 14 | ARGUMENTS = [ 15 | DeclareLaunchArgument('namespace', default_value='', 16 | description='Robot namespace'), 17 | ] 18 | 19 | 20 | def generate_launch_description(): 21 | pkg_create3_control = get_package_share_directory('irobot_create_control') 22 | 23 | namespace = LaunchConfiguration('namespace') 24 | 25 | control_params_file = PathJoinSubstitution( 26 | [pkg_create3_control, 'config', 'control.yaml']) 27 | 28 | diffdrive_controller_node = Node( 29 | package='controller_manager', 30 | executable='spawner', 31 | namespace=namespace, # Namespace is not pushed when used in EventHandler 32 | parameters=[control_params_file], 33 | arguments=[ 34 | 'diffdrive_controller', 35 | '-c', 36 | 'controller_manager', 37 | '--controller-manager-timeout', 38 | '30' 39 | ], 40 | output='screen', 41 | ) 42 | 43 | joint_state_broadcaster_spawner = Node( 44 | package='controller_manager', 45 | executable='spawner', 46 | arguments=[ 47 | 'joint_state_broadcaster', 48 | '-c', 49 | 'controller_manager', 50 | '--controller-manager-timeout', 51 | '30' 52 | ], 53 | output='screen', 54 | ) 55 | 56 | # Ensure diffdrive_controller_node starts after joint_state_broadcaster_spawner 57 | diffdrive_controller_callback = RegisterEventHandler( 58 | event_handler=OnProcessExit( 59 | target_action=joint_state_broadcaster_spawner, 60 | on_exit=[diffdrive_controller_node], 61 | ) 62 | ) 63 | 64 | # Static transform from /odom to odom 65 | # See https://github.com/ros-controls/ros2_controllers/pull/533 66 | tf_namespaced_odom_publisher = Node( 67 | package='tf2_ros', 68 | executable='static_transform_publisher', 69 | name='tf_namespaced_odom_publisher', 70 | arguments=['0', '0', '0', 71 | '0', '0', '0', 72 | 'odom', [namespace, '/odom']], 73 | remappings=[ 74 | ('/tf', 'tf'), 75 | ('/tf_static', 'tf_static') 76 | ], 77 | output='screen', 78 | condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')) 79 | ) 80 | 81 | # Static transform from /base_link to base_link 82 | tf_namespaced_base_link_publisher = Node( 83 | package='tf2_ros', 84 | executable='static_transform_publisher', 85 | name='tf_namespaced_base_link_publisher', 86 | arguments=['0', '0', '0', 87 | '0', '0', '0', 88 | [namespace, '/base_link'], 'base_link'], 89 | remappings=[ 90 | ('/tf', 'tf'), 91 | ('/tf_static', 'tf_static') 92 | ], 93 | output='screen', 94 | condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')) 95 | ) 96 | 97 | ld = LaunchDescription(ARGUMENTS) 98 | 99 | ld.add_action(joint_state_broadcaster_spawner) 100 | ld.add_action(diffdrive_controller_callback) 101 | ld.add_action(tf_namespaced_odom_publisher) 102 | ld.add_action(tf_namespaced_base_link_publisher) 103 | 104 | return ld 105 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | 39 | 40 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | ${namespace} 64 | ~/out:=ir_opcode 65 | 66 | ${update_rate} 67 | ${robot_model_name} 68 | ${receiver_link_name} 69 | ${dock_model_name} 70 | ${emitter_link_name} 71 | ${sensor_0_fov} 72 | ${sensor_0_range} 73 | ${sensor_1_fov} 74 | ${sensor_1_range} 75 | 76 | 77 | 78 | 79 | 80 | true 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_plugins/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package irobot_create_ignition_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.4 (2024-10-23) 6 | ------------------ 7 | 8 | 3.0.3 (2024-09-28) 9 | ------------------ 10 | * Generalize gz vendor use and modernize CMake (`#232 `_) 11 | * Contributors: Jose Luis Rivero 12 | 13 | 3.0.2 (2024-08-26) 14 | ------------------ 15 | * replace gz-gui8 with gz_gui_vendor 16 | back to what was used before, but as depend rather than buildtool_depend 17 | * Contributors: Alberto Soragna 18 | 19 | 3.0.1 (2024-08-25) 20 | ------------------ 21 | * replace gz_gui_vendor with gz-gui8 (`#230 `_) 22 | * Contributors: Alberto Soragna 23 | 24 | 3.0.0 (2024-08-25) 25 | ------------------ 26 | * Jazzy updates (`#229 `_) 27 | * Contributors: Chris Iverach-Brereton 28 | 29 | 2.1.0 (2023-05-15) 30 | ------------------ 31 | * Multi robot support (`#207 `_) 32 | * Contributors: Roni Kreinin 33 | 34 | 2.0.0 (2023-01-19) 35 | ------------------ 36 | * Update to ROS 2 Humble (`#197 `_) 37 | * Update message names to https://github.com/iRobotEducation/irobot_create_msgs/pull/10 38 | * rename dock topic into dock_status 39 | * comment ign_ros2_control dependency as it must be built from sources 40 | Co-authored-by: Francisco Martín Rico 41 | * add missing dependency to irobot-create-common-bringup (`#186 `_) 42 | * Contributors: Alberto Soragna, Francisco Martín Rico 43 | 44 | 1.0.1 (2022-04-12) 45 | ------------------ 46 | * add boost as explicit dependency and remove unnneded boost usage (`#175 `_) 47 | * add boost as explicit dependency and remove unnneded boost usage 48 | * do not search for boost component in Boost library 49 | 50 | 1.0.0 (2022-03-31) 51 | ------------------ 52 | * Ignition Gazebo support (`#144 `_) 53 | * Updated URDF to work with both gazebo classic and ignition: 54 | - Added gazebo arg to create3.urdf.xacro and standard_dock.urdf.xacro. This arg sets which gazebo version to use. 55 | - Added ray sensor macro which creates the correct sensor given the gazebo arg 56 | - Added ignition plugins when gazebo=ignition 57 | - Adjusted front caster position to better align with the create3 model 58 | - Adjusted wheeldrop spring stiffness to compensate for the front caster position change 59 | * Launch joint_state_publisher and diffdrive_controller only in classic 60 | * Use joint_state_publisher for both ignition and classic 61 | Cleaned up gazebo launch arg 62 | Adjusted front caster position 63 | * Simulation -> simulator 64 | * Added irobot_create_ignition packages 65 | * Fixed some linter warnings 66 | * Removed joint state publisher from ros_ign_bridge 67 | * - Reorganized packages 68 | - Shifted center of gravity of create3 forwards by 22.8 mm 69 | - Added min/max velocity and acceleration to diff drive plugin 70 | * Moved README.md to irobot_create_gazebo 71 | Created new README.md for irobot_create_ignition 72 | * Update README.md 73 | * Added ignition edifice repos for source installation 74 | * Create README.md 75 | * Added missing dependency 76 | * Renamed dock to standard_dock 77 | Center of gravity offset only applied in ignition for now 78 | * Fixed Linter errors 79 | * Updated README to have installation and example instructions for both Ignition and Classic 80 | Moved .repos files to the root of the repository 81 | * Ignition and Gazebo packages are now optional and only built if the required dependencies are installed 82 | * Made ros_ign_interfaces optional in irobot_create_ignition_bringup 83 | * fix license and minor changes to CMake and README 84 | * Interface buttons mock publisher not used in Ignition sim 85 | * Fixed linter errors 86 | Co-authored-by: Alberto Soragna 87 | * Contributors: roni-kreinin 88 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/robot_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Lola Segura (lsegura@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__ROBOT_STATE_HPP_ 5 | #define IROBOT_CREATE_NODES__ROBOT_STATE_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "geometry_msgs/msg/pose.hpp" 13 | #include "irobot_create_msgs/msg/dock_status.hpp" 14 | #include "irobot_create_msgs/msg/hazard_detection.hpp" 15 | #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" 16 | #include "irobot_create_msgs/msg/stop_status.hpp" 17 | #include "irobot_create_msgs/msg/wheel_vels.hpp" 18 | #include "nav_msgs/msg/odometry.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "sensor_msgs/msg/battery_state.hpp" 21 | 22 | namespace irobot_create_nodes 23 | { 24 | 25 | class RobotState : public rclcpp::Node 26 | { 27 | public: 28 | /// \brief Constructor 29 | explicit RobotState(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 30 | 31 | private: 32 | // Callback functions 33 | void dock_callback(irobot_create_msgs::msg::DockStatus::SharedPtr msg); 34 | void stop_callback(nav_msgs::msg::Odometry::SharedPtr msg); 35 | 36 | double get_docked_charge_percentage(const rclcpp::Time & at_time); 37 | double get_undocked_charge_percentage(const rclcpp::Time & at_time); 38 | 39 | // Publish aggregated detections on timer_'s frequency 40 | rclcpp::TimerBase::SharedPtr battery_state_timer_; 41 | rclcpp::TimerBase::SharedPtr stop_status_timer_; 42 | 43 | // Publishers 44 | rclcpp::Publisher::SharedPtr battery_state_publisher_{nullptr}; 45 | rclcpp::Publisher::SharedPtr stop_status_publisher_{nullptr}; 46 | 47 | // Subscribers 48 | rclcpp::Subscription::SharedPtr dock_subscription_; 49 | rclcpp::Subscription::SharedPtr stop_status_subscription_; 50 | 51 | // Topic to publish battery state to 52 | std::string battery_state_publisher_topic_; 53 | // Topic to publish stop status to 54 | std::string stop_status_publisher_topic_; 55 | 56 | // Topic to subscribe to dock 57 | std::string dock_subscription_topic_; 58 | // Topic to subscribe to wheel vels vector 59 | std::string wheel_vels_subscription_topic_; 60 | 61 | // Message to store the stop status 62 | irobot_create_msgs::msg::StopStatus stop_status_msg_; 63 | // Message to store the battery state 64 | sensor_msgs::msg::BatteryState battery_state_msg_; 65 | 66 | double linear_velocity_tolerance{std::numeric_limits::max()}; 67 | double angular_velocity_tolerance{std::numeric_limits::max()}; 68 | std::atomic is_stopped_{true}; 69 | std::atomic is_docked_{false}; 70 | // Fields to help populate battery_state 71 | const double idle_current_ {-0.404}; 72 | const double drive_current_ {-0.526}; 73 | const double charge_current_ {0.9}; 74 | const double full_charge_current_ {-0.15}; 75 | const double full_batter_state_voltage_ {16.474}; 76 | std::mutex battery_charge_timings_mutex_; 77 | rclcpp::Time transitioned_to_stopped_; 78 | rclcpp::Time transitioned_to_drive_; 79 | rclcpp::Time transitioned_to_undocked_; 80 | rclcpp::Time transitioned_to_docked_; 81 | rclcpp::Duration off_dock_drive_time_ {std::chrono::seconds(0)}; 82 | rclcpp::Duration off_dock_idle_time_ {std::chrono::seconds(0)}; 83 | std::atomic last_docked_charge_percentage_ {1.0}; 84 | const double charge_rate_percent_per_second_ {0.00012658291225191914}; 85 | const double driving_drain_percentage_per_second {0.00008875}; 86 | const double idle_drain_percentage_per_second {0.00005634}; 87 | const double battery_voltage_range_high_ {6}; 88 | const double battery_voltage_range_middle_ {3.27}; 89 | const double battery_capacity_ {2.046}; 90 | const double battery_default_temp_ {27.0}; 91 | 92 | double battery_full_charge_percentage; 93 | double battery_high_percentage_limit; 94 | double undocked_charge_limit; 95 | 96 | const std::string base_frame_ {"base_link"}; 97 | }; 98 | 99 | } // namespace irobot_create_nodes 100 | 101 | #endif // IROBOT_CREATE_NODES__ROBOT_STATE_HPP_ 102 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # iRobot® Create® 3 Simulator 2 | 3 | [![Testing](https://github.com/iRobotSTEM/create3_sim/actions/workflows/ci.yml/badge.svg)](https://github.com/iRobotSTEM/create3_sim/actions/workflows/ci.yml) [![License](https://img.shields.io/github/license/iRobotEducation/create3_sim)](https://github.com/iRobotEducation/create3_sim/blob/main/LICENSE) 4 | 5 | This is a ROS 2 simulation stack for the [iRobot® Create® 3](https://edu.irobot.com/create3) robot. 6 | Only Gazebo Harmonic is supported. 7 | 8 | Have a look at the [Create® 3 documentation](https://iroboteducation.github.io/create3_docs/) for more details on the ROS 2 interfaces exposed by the robot. 9 | 10 | ## Prerequisites 11 | 12 | Required dependencies: 13 | 14 | 1. [ROS 2 Jazzy](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) 15 | 2. ROS 2 dev tools: 16 | - [colcon-common-extensions](https://pypi.org/project/colcon-common-extensions/) 17 | - [rosdep](https://pypi.org/project/rosdep/): Used to install dependencies when building from sources 18 | - [vcs](https://pypi.org/project/vcstool/): Automates cloning of git repositories declared on a YAML file. 19 | 20 | Besides the aforementioned dependencies you will also need Gazebo Harmonic. 21 | 22 | #### Gazebo Harmonic 23 | 24 | ```bash 25 | sudo apt-get update && sudo apt-get install wget 26 | sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' 27 | wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - 28 | sudo apt-get update && sudo apt-get install ros-gz 29 | ``` 30 | 31 | ## Build 32 | 33 | - Create a workspace if you don't already have one: 34 | 35 | ```bash 36 | mkdir -p ~/create3_ws/src 37 | ``` 38 | 39 | - Clone this repository into the src directory from above. 40 | - Clone the [`irobot_create_msgs` repository](https://github.com/iRobotEducation/irobot_create_msgs) into the workspace 41 | 42 | - Navigate to the workspace and install ROS 2 dependencies with: 43 | 44 | ```bash 45 | cd ~/create3_ws 46 | sudo apt-get update 47 | rosdep install --from-path src -yi 48 | ``` 49 | 50 | - Build the workspace with: 51 | 52 | ```bash 53 | colcon build --symlink-install 54 | source install/local_setup.bash 55 | ``` 56 | 57 | ## Run 58 | 59 | #### Gazebo Harmonic 60 | 61 | Create® 3 can be spawned in a demo world in Gazebo and monitored through RViz with 62 | 63 | ```bash 64 | ros2 launch irobot_create_gz_bringup create3_gz.launch.py 65 | ``` 66 | 67 | The spawn point can be changed with the `x`, `y`, `z` and `yaw` launch arguments: 68 | 69 | ```bash 70 | ros2 launch irobot_create_gz_bringup create3_gz.launch.py x:=1.0 y:=0.5 yaw:=1.5707 71 | ``` 72 | 73 | ##### Namespacing 74 | 75 | A namespace can be applied to the robot using the `namespace` launch argument: 76 | 77 | ```bash 78 | ros2 launch irobot_create_gz_bringup create3_gz.launch.py namespace:=my_robot 79 | ``` 80 | 81 | Multiple robots can be spawned with unique namespaces: 82 | 83 | ```bash 84 | ros2 launch irobot_create_gz_bringup create3_gz.launch.py namespace:=robot1 85 | ros2 launch irobot_create_gz_bringup create3_spawn.launch.py namespace:=robot2 x:=1.0 86 | ``` 87 | 88 | > :warning: `create3_gz.launch.py` should only be used once as it launches the Ignition simulator itself. Additional robots should be spawned with `create3_spawn.launch.py`. Namespaces and spawn points should be unique for each robot. 89 | 90 | ## Package layout 91 | 92 | This repository contains packages for both the Classic and Ignition Gazebo simulators: 93 | 94 | - `irobot_create_common` Packages common to both Classic and Ignition 95 | - `irobot_create_common_bringup` Launch files and configurations 96 | - `irobot_create_control` Launch control nodes 97 | - `irobot_create_description` URDF and mesh files describing the robot 98 | - `irobot_create_nodes` Nodes for simulating robot topics and motion control 99 | - `irobot_create_toolbox` Tools and helpers for creating nodes and plugins 100 | 101 | - `irobot_create_gz` Packages used for the Gazebo Harmonic Simulator 102 | - `irobot_create_gz_bringup` Launch files and configurations 103 | - `irobot_create_gz_plugins` GUI plugins 104 | - `irobot_create_gz_sim` Metapackage 105 | - `irobot_create_gz_toolbox` Sensor and interface nodes 106 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(irobot_create_gz_toolbox) 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(ament_cmake_python REQUIRED) 21 | find_package(control_msgs REQUIRED) 22 | find_package(geometry_msgs REQUIRED) 23 | find_package(sensor_msgs REQUIRED) 24 | find_package(nav_msgs REQUIRED) 25 | find_package(irobot_create_msgs REQUIRED) 26 | find_package(rclcpp REQUIRED) 27 | find_package(rclcpp_action REQUIRED) 28 | find_package(tf2 REQUIRED) 29 | find_package(tf2_geometry_msgs REQUIRED) 30 | find_package(tf2_ros REQUIRED) 31 | find_package(irobot_create_toolbox REQUIRED) 32 | find_package(ros_gz_interfaces QUIET) 33 | 34 | if(NOT ros_gz_interfaces_FOUND) 35 | message(WARNING "ros_gz_interfaces not found. Skipping ${PROJECT_NAME} package.") 36 | ament_package() 37 | return() 38 | endif() 39 | 40 | # Libraries 41 | 42 | set(dependencies 43 | control_msgs 44 | geometry_msgs 45 | sensor_msgs 46 | irobot_create_msgs 47 | nav_msgs 48 | rclcpp 49 | rclcpp_action 50 | tf2 51 | tf2_geometry_msgs 52 | tf2_ros 53 | ros_gz_interfaces 54 | irobot_create_toolbox 55 | ) 56 | 57 | # Pose republisher 58 | add_library(irobot_create_gz_pose_republisher_lib SHARED) 59 | target_sources( 60 | irobot_create_gz_pose_republisher_lib 61 | PRIVATE 62 | src/pose_republisher/pose_republisher_node.cpp 63 | ) 64 | target_include_directories(irobot_create_gz_pose_republisher_lib PUBLIC include) 65 | ament_target_dependencies(irobot_create_gz_pose_republisher_lib 66 | ${dependencies} 67 | ) 68 | 69 | # Sensors 70 | add_library(irobot_create_gz_sensors_lib SHARED) 71 | target_sources( 72 | irobot_create_gz_sensors_lib 73 | PRIVATE 74 | src/sensors/sensors_node.cpp 75 | src/sensors/bumper.cpp 76 | src/sensors/cliff.cpp 77 | src/sensors/ir_intensity.cpp 78 | src/sensors/ir_opcode.cpp 79 | src/sensors/mouse.cpp 80 | src/sensors/wheel_drop.cpp 81 | ) 82 | target_include_directories(irobot_create_gz_sensors_lib PUBLIC include) 83 | ament_target_dependencies(irobot_create_gz_sensors_lib 84 | ${dependencies} 85 | ) 86 | 87 | # Interface buttons 88 | add_library(irobot_create_gz_interface_buttons_lib SHARED) 89 | target_sources( 90 | irobot_create_gz_interface_buttons_lib 91 | PRIVATE 92 | src/interface_buttons/interface_buttons_node.cpp 93 | ) 94 | target_include_directories(irobot_create_gz_interface_buttons_lib PUBLIC include) 95 | ament_target_dependencies(irobot_create_gz_interface_buttons_lib 96 | ${dependencies} 97 | ) 98 | 99 | set(libraries_names 100 | irobot_create_gz_pose_republisher_lib 101 | irobot_create_gz_sensors_lib 102 | irobot_create_gz_interface_buttons_lib 103 | ) 104 | 105 | # Executables 106 | 107 | # Pose republisher node 108 | add_executable(pose_republisher_node) 109 | target_sources( 110 | pose_republisher_node 111 | PRIVATE 112 | src/pose_republisher/pose_republisher_main.cpp 113 | ) 114 | target_link_libraries(pose_republisher_node irobot_create_gz_pose_republisher_lib) 115 | 116 | # Sensors node 117 | add_executable(sensors_node) 118 | target_sources( 119 | sensors_node 120 | PRIVATE 121 | src/sensors/sensors_main.cpp 122 | ) 123 | target_link_libraries(sensors_node irobot_create_gz_sensors_lib) 124 | 125 | # Interface buttons node 126 | add_executable(interface_buttons_node) 127 | target_sources( 128 | interface_buttons_node 129 | PRIVATE 130 | src/interface_buttons/interface_buttons_main.cpp 131 | ) 132 | target_link_libraries(interface_buttons_node irobot_create_gz_interface_buttons_lib) 133 | 134 | set(executables_names 135 | pose_republisher_node 136 | sensors_node 137 | interface_buttons_node 138 | ) 139 | 140 | # Install 141 | 142 | install(TARGETS ${libraries_names} 143 | ARCHIVE DESTINATION lib 144 | LIBRARY DESTINATION lib 145 | RUNTIME DESTINATION bin 146 | ) 147 | 148 | install(TARGETS ${executables_names} 149 | DESTINATION lib/${PROJECT_NAME} 150 | ) 151 | 152 | install(DIRECTORY include/ 153 | DESTINATION include 154 | ) 155 | 156 | if(BUILD_TESTING) 157 | find_package(ament_lint_auto REQUIRED) 158 | ament_lint_auto_find_test_dependencies() 159 | endif() 160 | 161 | ament_export_include_directories(include) 162 | ament_export_libraries(${libraries_names}) 163 | ament_export_dependencies(${dependencies}) 164 | ament_package() 165 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_toolbox/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package irobot_create_ignition_toolbox 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.4 (2024-10-23) 6 | ------------------ 7 | 8 | 3.0.3 (2024-09-28) 9 | ------------------ 10 | 11 | 3.0.2 (2024-08-26) 12 | ------------------ 13 | 14 | 3.0.1 (2024-08-25) 15 | ------------------ 16 | 17 | 3.0.0 (2024-08-25) 18 | ------------------ 19 | * Jazzy updates (`#229 `_) 20 | * Contributors: Chris Iverach-Brereton 21 | 22 | 2.1.0 (2023-05-15) 23 | ------------------ 24 | * Multi robot support (`#207 `_) 25 | * Contributors: Roni Kreinin 26 | 27 | 2.0.0 (2023-01-19) 28 | ------------------ 29 | * Update to ROS 2 Humble (`#197 `_) 30 | * Update message names to https://github.com/iRobotEducation/irobot_create_msgs/pull/10 31 | * rename dock topic into dock_status 32 | * comment ign_ros2_control dependency as it must be built from sources 33 | Co-authored-by: Francisco Martín Rico 34 | * add missing dependency to irobot-create-common-bringup (`#186 `_) 35 | * Renamed Ignition Toolbox libraries (`#178 `_) 36 | * Renamed Ignition Toolbox libraries 37 | * Pre-pend library names with irobot_create_ignition 38 | * Contributors: Alberto Soragna, Francisco Martín Rico, roni-kreinin 39 | 40 | 1.0.1 (2022-04-12) 41 | ------------------ 42 | * add boost as explicit dependency and remove unnneded boost usage (`#175 `_) 43 | * add boost as explicit dependency and remove unnneded boost usage 44 | * do not search for boost component in Boost library 45 | 46 | 1.0.0 (2022-03-31) 47 | ------------------ 48 | * Update dependencies for build from source, throttle action feedback, add stub for audio command (`#161 `_) 49 | * Update dependencies for build from source 50 | * Add stub for cmd_audio and throttle action feedback 51 | * fix packages from PR feedback 52 | * Split `irobot_create_toolbox` (`#153 `_) 53 | * rename irobot_create_toolbox into irobot_create_nodes 54 | * move common utilities to irobot_create_toolbox 55 | * register irobot_create_nodes as rclcpp_components 56 | * update readme 57 | * use new node names in parameter files 58 | * remove declare parameter utility and fix linter tests 59 | * Ignition Gazebo support (`#144 `_) 60 | * Updated URDF to work with both gazebo classic and ignition: 61 | - Added gazebo arg to create3.urdf.xacro and standard_dock.urdf.xacro. This arg sets which gazebo version to use. 62 | - Added ray sensor macro which creates the correct sensor given the gazebo arg 63 | - Added ignition plugins when gazebo=ignition 64 | - Adjusted front caster position to better align with the create3 model 65 | - Adjusted wheeldrop spring stiffness to compensate for the front caster position change 66 | * Launch joint_state_publisher and diffdrive_controller only in classic 67 | * Use joint_state_publisher for both ignition and classic 68 | Cleaned up gazebo launch arg 69 | Adjusted front caster position 70 | * Simulation -> simulator 71 | * Added irobot_create_ignition packages 72 | * Fixed some linter warnings 73 | * Removed joint state publisher from ros_ign_bridge 74 | * - Reorganized packages 75 | - Shifted center of gravity of create3 forwards by 22.8 mm 76 | - Added min/max velocity and acceleration to diff drive plugin 77 | * Moved README.md to irobot_create_gazebo 78 | Created new README.md for irobot_create_ignition 79 | * Update README.md 80 | * Added ignition edifice repos for source installation 81 | * Create README.md 82 | * Added missing dependency 83 | * Renamed dock to standard_dock 84 | Center of gravity offset only applied in ignition for now 85 | * Fixed Linter errors 86 | * Updated README to have installation and example instructions for both Ignition and Classic 87 | Moved .repos files to the root of the repository 88 | * Ignition and Gazebo packages are now optional and only built if the required dependencies are installed 89 | * Made ros_ign_interfaces optional in irobot_create_ignition_bringup 90 | * fix license and minor changes to CMake and README 91 | * Interface buttons mock publisher not used in Ignition sim 92 | * Fixed linter errors 93 | Co-authored-by: Alberto Soragna 94 | * Contributors: Alberto Soragna, Justin Kearns, roni-kreinin 95 | -------------------------------------------------------------------------------- /irobot_create_gz/irobot_create_gz_bringup/gui/create3/gui.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 3D View 7 | docked 8 | false 9 | 10 | ogre2 11 | scene 12 | 0.4 0.4 0.4 13 | 0.8 0.8 0.8 14 | -6 0 6 0 0.5 0 15 | 16 | 17 | 18 | false 19 | floating 20 | 21 | 22 | 23 | 24 | false 25 | floating 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | World control 34 | false 35 | false 36 | 72 37 | 1 38 | 39 | floating 40 | 41 | 42 | 43 | 44 | 45 | 46 | true 47 | true 48 | false 49 | 50 | 51 | 52 | 53 | 54 | World stats 55 | false 56 | false 57 | 110 58 | 290 59 | 1 60 | 61 | floating 62 | 63 | 64 | 65 | 66 | 67 | 68 | true 69 | true 70 | true 71 | true 72 | 73 | 74 | 75 | 76 | 77 | Transform control 78 | 79 | 80 | 81 | 82 | false 83 | 250 84 | 50 85 | floating 86 | false 87 | #03a9f4 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | false 99 | 250 100 | 50 101 | floating 102 | false 103 | #03a9f4 104 | 105 | 106 | 107 | 108 | 109 | 110 | true 111 | docked 112 | 200 113 | true 114 | 115 | 116 | 117 | 118 | 119 | /cmd_vel 120 | 121 | true 122 | docked 123 | true 124 | 125 | 126 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package irobot_create_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.4 (2024-10-23) 6 | ------------------ 7 | 8 | 3.0.3 (2024-09-28) 9 | ------------------ 10 | 11 | 3.0.2 (2024-08-26) 12 | ------------------ 13 | 14 | 3.0.1 (2024-08-25) 15 | ------------------ 16 | 17 | 3.0.0 (2024-08-25) 18 | ------------------ 19 | * Add dependencies to irobot_create_description (`#219 `_) 20 | * Contributors: Alberto Soragna, Gaël Écorchard 21 | 22 | 2.1.0 (2023-05-15) 23 | ------------------ 24 | * Multi robot support (`#207 `_) 25 | * Description fixes (`#200 `_) 26 | * Remapped odom topic (`#206 `_) 27 | * Contributors: Roni Kreinin, roni-kreinin 28 | 29 | 2.0.0 (2023-01-19) 30 | ------------------ 31 | * Update to ROS 2 Humble (`#197 `_) 32 | * Update message names to https://github.com/iRobotEducation/irobot_create_msgs/pull/10 33 | * rename dock topic into dock_status 34 | * comment ign_ros2_control dependency as it must be built from sources 35 | Co-authored-by: Francisco Martín Rico 36 | * add missing dependency to irobot-create-common-bringup (`#186 `_) 37 | * Contributors: Alberto Soragna, Francisco Martín Rico 38 | 39 | 1.0.1 (2022-04-12) 40 | ------------------ 41 | * add boost as explicit dependency and remove unnneded boost usage (`#175 `_) 42 | * add boost as explicit dependency and remove unnneded boost usage 43 | * do not search for boost component in Boost library 44 | 45 | 1.0.0 (2022-03-31) 46 | ------------------ 47 | * Added irobot_create_control dependency to irobot_create_description (`#171 `_) 48 | * Use package:// to reference meshes (`#168 `_) 49 | * Set GAZEBO_MODEL_URI to empty string to prevent model downloads. 50 | Added /usr/share/gazebo-11/models/ to GAZEBO_MODEL_PATH to use local ground plane and sun models. 51 | Using package:// instead of file:// for mesh paths. 52 | * Append to GAZEBO_MODEL_PATH 53 | * Added pose publisher (`#154 `_) 54 | * Use ign_ros2_control (`#148 `_) 55 | * Use ign_ros2_control 56 | * Removed remapping 57 | * Documentation 58 | * Ignition Gazebo support (`#144 `_) 59 | * Updated URDF to work with both gazebo classic and ignition: 60 | - Added gazebo arg to create3.urdf.xacro and standard_dock.urdf.xacro. This arg sets which gazebo version to use. 61 | - Added ray sensor macro which creates the correct sensor given the gazebo arg 62 | - Added ignition plugins when gazebo=ignition 63 | - Adjusted front caster position to better align with the create3 model 64 | - Adjusted wheeldrop spring stiffness to compensate for the front caster position change 65 | * Launch joint_state_publisher and diffdrive_controller only in classic 66 | * Use joint_state_publisher for both ignition and classic 67 | Cleaned up gazebo launch arg 68 | Adjusted front caster position 69 | * Simulation -> simulator 70 | * Added irobot_create_ignition packages 71 | * Fixed some linter warnings 72 | * Removed joint state publisher from ros_ign_bridge 73 | * - Reorganized packages 74 | - Shifted center of gravity of create3 forwards by 22.8 mm 75 | - Added min/max velocity and acceleration to diff drive plugin 76 | * Moved README.md to irobot_create_gazebo 77 | Created new README.md for irobot_create_ignition 78 | * Update README.md 79 | * Added ignition edifice repos for source installation 80 | * Create README.md 81 | * Added missing dependency 82 | * Renamed dock to standard_dock 83 | Center of gravity offset only applied in ignition for now 84 | * Fixed Linter errors 85 | * Updated README to have installation and example instructions for both Ignition and Classic 86 | Moved .repos files to the root of the repository 87 | * Ignition and Gazebo packages are now optional and only built if the required dependencies are installed 88 | * Made ros_ign_interfaces optional in irobot_create_ignition_bringup 89 | * fix license and minor changes to CMake and README 90 | * Interface buttons mock publisher not used in Ignition sim 91 | * Fixed linter errors 92 | Co-authored-by: Alberto Soragna 93 | * Contributors: Alejandro Hernández Cordero, roni-kreinin 94 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_description/urdf/dock/standard_dock.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 53 | 54 | 55 | 56 | 62 | 64 | 65 | 66 | 67 | 73 | 74 | 75 | 76 | 77 | 83 | 84 | 85 | 86 | 87 | 88 | true 89 | 90 | 91 | 92 | $(arg namespace) 93 | odom:=sim_ground_truth_dock_pose 94 | 95 | ${link_name} 96 | world 97 | 1 98 | 0 0 0 99 | 0.0 0.0 0.0 100 | 0.0 101 | 102 | 103 | 104 | 105 | 106 | 107 | true 108 | 109 | 110 | true 111 | true 112 | true 113 | 62 114 | 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/ui_mgr.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 iRobot Corporation. All Rights Reserved. 2 | // @author Lola Segura (lsegura@irobot.com) 3 | 4 | #ifndef IROBOT_CREATE_NODES__UI_MGR_HPP_ 5 | #define IROBOT_CREATE_NODES__UI_MGR_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "irobot_create_msgs/action/audio_note_sequence.hpp" 13 | #include "irobot_create_msgs/action/led_animation.hpp" 14 | #include "irobot_create_msgs/msg/audio_note_vector.hpp" 15 | #include "irobot_create_msgs/msg/button.hpp" 16 | #include "irobot_create_msgs/msg/interface_buttons.hpp" 17 | #include "irobot_create_msgs/msg/led_color.hpp" 18 | #include "irobot_create_msgs/msg/lightring_leds.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "rclcpp_action/rclcpp_action.hpp" 21 | 22 | namespace irobot_create_nodes 23 | { 24 | 25 | class UIMgr : public rclcpp::Node 26 | { 27 | public: 28 | /// \brief Constructor 29 | explicit UIMgr(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 30 | 31 | // Callback functions 32 | void lightring_callback(irobot_create_msgs::msg::LightringLeds::SharedPtr msg); 33 | void audio_callback(irobot_create_msgs::msg::AudioNoteVector::SharedPtr msg); 34 | 35 | private: 36 | rclcpp_action::GoalResponse handle_led_animation_goal( 37 | const rclcpp_action::GoalUUID & uuid, 38 | std::shared_ptr goal); 39 | rclcpp_action::CancelResponse handle_led_animation_cancel( 40 | const std::shared_ptr< 41 | rclcpp_action::ServerGoalHandle> goal_handle); 42 | void handle_led_animation_accepted( 43 | const std::shared_ptr< 44 | rclcpp_action::ServerGoalHandle> goal_handle); 45 | void execute_led_animation( 46 | const std::shared_ptr< 47 | rclcpp_action::ServerGoalHandle> goal_handle); 48 | 49 | rclcpp_action::GoalResponse handle_audio_note_sequence_goal( 50 | const rclcpp_action::GoalUUID & uuid, 51 | std::shared_ptr goal); 52 | rclcpp_action::CancelResponse handle_audio_note_sequence_cancel( 53 | const std::shared_ptr< 54 | rclcpp_action::ServerGoalHandle> goal_handle); 55 | void handle_audio_note_sequence_accepted( 56 | const std::shared_ptr< 57 | rclcpp_action::ServerGoalHandle> goal_handle); 58 | void execute_audio_note_sequence( 59 | const std::shared_ptr< 60 | rclcpp_action::ServerGoalHandle> goal_handle); 61 | 62 | // Publish aggregated detections on timer_'s frequency 63 | rclcpp::TimerBase::SharedPtr buttons_timer_; 64 | 65 | // Publishers 66 | std::shared_ptr< 67 | rclcpp::Publisher> buttons_publisher_{nullptr}; 68 | 69 | // Subscribers 70 | rclcpp::Subscription::SharedPtr lightring_subscription_; 71 | rclcpp::Subscription::SharedPtr audio_subscription_; 72 | 73 | // Actions 74 | rclcpp_action::Server::SharedPtr 75 | led_animation_action_server_; 76 | rclcpp_action::Server::SharedPtr 77 | audio_note_sequence_action_server_; 78 | 79 | // Gazebo simulator being used 80 | std::string gazebo_; 81 | 82 | // Topic to publish interface buttons to 83 | std::string buttons_publisher_topic_; 84 | 85 | // Topic to subscribe to light ring vector 86 | std::string lightring_subscription_topic_; 87 | // Topic to subscribe to audio note vector 88 | std::string audio_subscription_topic_; 89 | 90 | // Message to store the interface buttons 91 | irobot_create_msgs::msg::InterfaceButtons buttons_msg_; 92 | 93 | const std::string base_frame_ {"base_link"}; 94 | std::mutex led_animation_params_mutex_; 95 | rclcpp::Duration led_animation_end_duration_; 96 | rclcpp::Time led_animation_start_time_; 97 | rclcpp::Time last_animation_feedback_time_; 98 | const rclcpp::Duration report_animation_feedback_interval_ {std::chrono::seconds(3)}; 99 | std::mutex audio_note_sequence_params_mutex_; 100 | rclcpp::Duration audio_note_sequence_end_duration_; 101 | rclcpp::Time audio_note_sequence_start_time_; 102 | rclcpp::Time last_audio_note_feedback_time_; 103 | int32_t audio_iterations_; 104 | rclcpp::Duration audio_notes_duration_; 105 | const rclcpp::Duration report_audio_note_feedback_interval_ {std::chrono::seconds(3)}; 106 | }; 107 | 108 | } // namespace irobot_create_nodes 109 | 110 | #endif // IROBOT_CREATE_NODES__UI_MGR_HPP_ 111 | --------------------------------------------------------------------------------