From 71cda803f89c34356bbd5fcc3a05bb85b4195f6f Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton <59611394+civerachb-cpr@users.noreply.github.com> Date: Fri, 16 Aug 2024 16:07:00 -0400 Subject: [PATCH] Jazzy updates (#229) * Remove the COLCON_IGNORE, fix some namespace issues with the Create3 plugin * Initial bulk renaming ignition -> gz, ign -> gz * Rename files & folders ignition -> gz * Add python setup files * Copypasta * Remove setup files (they're no longer necessary after all?), add ament_cmake_python dependency * Remove substitution deprecations * Add IfCondition wrappers * Missing import * Rename gazebo launch file * Update launch file * Update the GUI config with fixed camera position, add 3d scene manager * Enable the plugins, fix the path envars * Remove commented-out parameters * Initial migration from Twist to TwistStamped. Headers are not initalized; just the message type is changed so far * Override the backup safety for now; it seems to be clobbering the undock behaviour * Update the gz_ros_bridge to use stamped messages for cmd_vel * Don't allow the dock/undock actions to be preempted * Add the InteractiveViewControl so we can move the camera around inside Gazebo * Reduce the width of wall 12 so the robot's default spawn location doesn't clip into the wall * Add launch instructions to the readme * Add note about how to change initial pose * Fix logging for the buttons plugin * Revert accidental change to changelog * Revert changes to CMakeLists * Set the time in the stamped twist message headers * Revert get_default_velocity_cmd to have no parameters as it's used externally, set the stamp outside that function (since we may create the command, do some math, and then publish it later). Add an unstamped velocity command input * Revert additional changelog modifications * Add dependency on gz_gui_vendor (provides gz-gui8) * Add dependency on gz_math_vendor (provides gz-math7) * Update CI for Jazzy * Make gz-gui8 required in cmake * Update CI versions to latest * Linter fixes --- .github/workflows/ci.yml | 12 ++-- README.md | 38 +++++------ .../launch/create3_nodes.launch.py | 5 +- .../irobot_create_common_bringup/package.xml | 1 + .../launch/include/control.py | 8 +-- .../motion_control/behaviors_scheduler.hpp | 4 +- .../motion_control/drive_goal_behaviors.hpp | 12 ++-- .../motion_control/simple_goal_controller.hpp | 18 ++--- .../motion_control_node.hpp | 13 ++-- .../src/kidnap_estimator_publisher.cpp | 8 +-- .../src/motion_control/docking_behavior.cpp | 4 +- .../motion_control/drive_goal_behaviors.cpp | 49 +++++++------- .../src/motion_control/reflex_behavior.cpp | 12 ++-- .../motion_control/wall_follow_behavior.cpp | 6 +- .../src/motion_control/wall_follow_states.cpp | 4 +- .../src/motion_control_node.cpp | 62 +++++++++++++----- .../irobot_create_toolbox/package.xml | 1 + .../irobot_create_gz_bringup}/CHANGELOG.rst | 0 .../irobot_create_gz_bringup}/CMakeLists.txt | 2 +- .../irobot_create_gz_bringup/README.md | 22 +++++++ .../config/pose_republisher_params.yaml | 0 .../config/sensors_params.yaml | 0 .../gui/create3/gui.config | 40 +++++++---- .../launch/create3_gz.launch.py | 15 +++-- .../launch/create3_gz_nodes.launch.py | 12 ++-- .../launch/create3_ros_gz_bridge.launch.py | 6 +- .../launch/create3_spawn.launch.py | 12 ++-- .../launch/sim.launch.py | 59 +++++++++-------- .../irobot_create_gz_bringup}/package.xml | 4 +- .../worlds/depot.sdf | 0 .../irobot_create_gz_bringup}/worlds/maze.sdf | 6 +- .../irobot_create_gz_plugins}/CHANGELOG.rst | 0 .../irobot_create_gz_plugins}/CMakeLists.txt | 4 +- .../Create3Hmi/CMakeLists.txt | 2 + .../Create3Hmi/Create3Hmi.cc | 6 +- .../Create3Hmi/Create3Hmi.config | 0 .../Create3Hmi/Create3Hmi.hh | 12 ++-- .../Create3Hmi/Create3Hmi.qml | 0 .../Create3Hmi/Create3Hmi.qrc | 0 .../Create3Hmi/images/One Dot.png | Bin .../Create3Hmi/images/Power.png | Bin .../Create3Hmi/images/Two Dots.png | Bin .../irobot_create_gz_plugins/README.md | 1 + .../irobot_create_gz_plugins}/package.xml | 3 +- .../irobot_create_gz_sim}/CHANGELOG.rst | 0 .../irobot_create_gz_sim}/CMakeLists.txt | 2 +- .../irobot_create_gz_sim}/package.xml | 8 +-- .../irobot_create_gz_toolbox}/CHANGELOG.rst | 0 .../irobot_create_gz_toolbox}/CMakeLists.txt | 38 +++++------ .../interface_buttons_node.hpp | 10 +-- .../pose_republisher/pose_republisher.hpp | 12 ++-- .../sensors/bumper.hpp | 10 +-- .../sensors/cliff.hpp | 10 +-- .../sensors/ir_intensity.hpp | 10 +-- .../sensors/ir_opcode.hpp | 12 ++-- .../sensors/mouse.hpp | 10 +-- .../sensors/sensors_node.hpp | 46 +++++++++++++ .../sensors/wheel_drop.hpp | 10 +-- .../irobot_create_gz_toolbox}/utils.hpp | 10 +-- .../irobot_create_gz_toolbox}/package.xml | 2 +- .../interface_buttons_main.cpp | 4 +- .../interface_buttons_node.cpp | 4 +- .../pose_republisher_main.cpp | 4 +- .../pose_republisher_node.cpp | 4 +- .../src/sensors/bumper.cpp | 6 +- .../src/sensors/cliff.cpp | 4 +- .../src/sensors/ir_intensity.cpp | 4 +- .../src/sensors/ir_opcode.cpp | 8 +-- .../src/sensors/mouse.cpp | 4 +- .../src/sensors/sensors_main.cpp | 4 +- .../src/sensors/sensors_node.cpp | 4 +- .../src/sensors/wheel_drop.cpp | 4 +- .../irobot_create_ignition_bringup/README.md | 1 - .../COLCON_IGNORE | 0 .../irobot_create_ignition_plugins/README.md | 1 - .../sensors/sensors_node.hpp | 46 ------------- 76 files changed, 420 insertions(+), 335 deletions(-) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/CHANGELOG.rst (100%) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/CMakeLists.txt (93%) create mode 100644 irobot_create_gz/irobot_create_gz_bringup/README.md rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/config/pose_republisher_params.yaml (100%) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/config/sensors_params.yaml (100%) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/gui/create3/gui.config (78%) rename irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py => irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz.launch.py (78%) rename irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py => irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz_nodes.launch.py (82%) rename irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py => irobot_create_gz/irobot_create_gz_bringup/launch/create3_ros_gz_bridge.launch.py (97%) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/launch/create3_spawn.launch.py (93%) rename irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py => irobot_create_gz/irobot_create_gz_bringup/launch/sim.launch.py (52%) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/package.xml (93%) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/worlds/depot.sdf (100%) rename {irobot_create_ignition/irobot_create_ignition_bringup => irobot_create_gz/irobot_create_gz_bringup}/worlds/maze.sdf (99%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/CHANGELOG.rst (100%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/CMakeLists.txt (87%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/CMakeLists.txt (96%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/Create3Hmi.cc (92%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/Create3Hmi.config (100%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/Create3Hmi.hh (77%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/Create3Hmi.qml (100%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/Create3Hmi.qrc (100%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/images/One Dot.png (100%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/images/Power.png (100%) rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/Create3Hmi/images/Two Dots.png (100%) create mode 100644 irobot_create_gz/irobot_create_gz_plugins/README.md rename {irobot_create_ignition/irobot_create_ignition_plugins => irobot_create_gz/irobot_create_gz_plugins}/package.xml (90%) rename {irobot_create_ignition/irobot_create_ignition_sim => irobot_create_gz/irobot_create_gz_sim}/CHANGELOG.rst (100%) rename {irobot_create_ignition/irobot_create_ignition_sim => irobot_create_gz/irobot_create_gz_sim}/CMakeLists.txt (70%) rename {irobot_create_ignition/irobot_create_ignition_sim => irobot_create_gz/irobot_create_gz_sim}/package.xml (71%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/CHANGELOG.rst (100%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/CMakeLists.txt (70%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/interface_buttons/interface_buttons_node.hpp (70%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/pose_republisher/pose_republisher.hpp (81%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/sensors/bumper.hpp (79%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/sensors/cliff.hpp (76%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/sensors/ir_intensity.hpp (73%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/sensors/ir_opcode.hpp (89%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/sensors/mouse.hpp (76%) create mode 100644 irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/sensors_node.hpp rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/sensors/wheel_drop.hpp (76%) rename {irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox}/utils.hpp (92%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/package.xml (96%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/interface_buttons/interface_buttons_main.cpp (56%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/interface_buttons/interface_buttons_node.cpp (95%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/pose_republisher/pose_republisher_main.cpp (57%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/pose_republisher/pose_republisher_node.cpp (98%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/bumper.cpp (95%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/cliff.cpp (94%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/ir_intensity.cpp (95%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/ir_opcode.cpp (96%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/mouse.cpp (93%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/sensors_main.cpp (60%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/sensors_node.cpp (83%) rename {irobot_create_ignition/irobot_create_ignition_toolbox => irobot_create_gz/irobot_create_gz_toolbox}/src/sensors/wheel_drop.cpp (94%) delete mode 100644 irobot_create_ignition/irobot_create_ignition_bringup/README.md delete mode 100644 irobot_create_ignition/irobot_create_ignition_plugins/COLCON_IGNORE delete mode 100644 irobot_create_ignition/irobot_create_ignition_plugins/README.md delete mode 100644 irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/sensors_node.hpp diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 173df627..3699c3df 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -10,17 +10,17 @@ on: jobs: build_and_test: name: build_and_test - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: Set Ignition Version run: | - echo "IGNITION_VERSION=edifice" >> $GITHUB_ENV + echo "GZ_VERSION=harmonic" >> $GITHUB_ENV - uses: actions/checkout@v2.3.4 - - uses: ros-tooling/setup-ros@0.7.1 + - uses: ros-tooling/setup-ros@0.7.8 with: - required-ros-distributions: humble - - uses: ros-tooling/action-ros-ci@0.3.5 + required-ros-distributions: jazzy + - uses: ros-tooling/action-ros-ci@0.3.14 id: action_ros_ci_step with: - target-ros2-distro: humble + target-ros2-distro: jazzy import-token: ${{ secrets.REPO_TOKEN }} diff --git a/README.md b/README.md index 56bb2019..0a02369b 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ [![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) This is a ROS 2 simulation stack for the [iRobot® Create® 3](https://edu.irobot.com/create3) robot. -Both Ignition Gazebo and Classic Gazebo are supported. +Only Gazebo Harmonic is supported. 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. @@ -17,19 +17,15 @@ Required dependencies: - [rosdep](https://pypi.org/project/rosdep/): Used to install dependencies when building from sources - [vcs](https://pypi.org/project/vcstool/): Automates cloning of git repositories declared on a YAML file. -Besides the aforementioned dependencies you will also need at least one among Ignition Gazebo and Classic Gazebo +Besides the aforementioned dependencies you will also need Gazebo Harmonic. -#### Classic Gazebo - -Install [Gazebo 11](http://gazebosim.org/tutorials?tut=install_ubuntu) - -#### Ignition Fortress +#### Gazebo Harmonic ```bash sudo apt-get update && sudo apt-get install wget sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - -sudo apt-get update && sudo apt-get install ignition-fortress +sudo apt-get update && sudo apt-get install ros-gz ``` ## Build @@ -60,18 +56,18 @@ source install/local_setup.bash ## Run -#### Ignition Gazebo +#### Gazebo Harmonic -Create® 3 can be spawned in a demo world in Ignition and monitored through RViz with +Create® 3 can be spawned in a demo world in Gazebo and monitored through RViz with ```bash -ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py +ros2 launch irobot_create_gz_bringup create3_gz.launch.py ``` The spawn point can be changed with the `x`, `y`, `z` and `yaw` launch arguments: ```bash -ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py x:=1.0 y:=0.5 yaw:=1.5707 +ros2 launch irobot_create_gz_bringup create3_gz.launch.py x:=1.0 y:=0.5 yaw:=1.5707 ``` ##### Namespacing @@ -79,17 +75,17 @@ ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py x:=1.0 y:= A namespace can be applied to the robot using the `namespace` launch argument: ```bash -ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py namespace:=my_robot +ros2 launch irobot_create_gz_bringup create3_gz.launch.py namespace:=my_robot ``` Multiple robots can be spawned with unique namespaces: ```bash -ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py namespace:=robot1 -ros2 launch irobot_create_ignition_bringup create3_spawn.launch.py namespace:=robot2 x:=1.0 +ros2 launch irobot_create_gz_bringup create3_gz.launch.py namespace:=robot1 +ros2 launch irobot_create_gz_bringup create3_spawn.launch.py namespace:=robot2 x:=1.0 ``` -> :warning: `create3_ignition.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. +> :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. ## Package layout @@ -102,8 +98,8 @@ This repository contains packages for both the Classic and Ignition Gazebo simul - `irobot_create_nodes` Nodes for simulating robot topics and motion control - `irobot_create_toolbox` Tools and helpers for creating nodes and plugins -- `irobot_create_ignition` Packages used for the Ignition Gazebo Simulator - - `irobot_create_ignition_bringup` Launch files and configurations - - `irobot_create_ignition_plugins` GUI plugins - - `irobot_create_ignition_sim` Metapackage - - `irobot_create_ignition_toolbox` Sensor and interface nodes +- `irobot_create_gz` Packages used for the Gazebo Harmonic Simulator + - `irobot_create_gz_bringup` Launch files and configurations + - `irobot_create_gz_plugins` GUI plugins + - `irobot_create_gz_sim` Metapackage + - `irobot_create_gz_toolbox` Sensor and interface nodes diff --git a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py index ca2833b5..bafd600c 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py @@ -75,7 +75,10 @@ def generate_launch_description(): package='irobot_create_nodes', name='motion_control', executable='motion_control', - parameters=[{'use_sim_time': True}], + parameters=[{ + 'use_sim_time': True, + 'safety_override': 'backup_only' + }], output='screen', remappings=[ ('/tf', 'tf'), diff --git a/irobot_create_common/irobot_create_common_bringup/package.xml b/irobot_create_common/irobot_create_common_bringup/package.xml index a0043bae..0c3f9a38 100644 --- a/irobot_create_common/irobot_create_common_bringup/package.xml +++ b/irobot_create_common/irobot_create_common_bringup/package.xml @@ -10,6 +10,7 @@ iRobot ament_cmake + ament_cmake_python irobot_create_control irobot_create_description diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index 89cba524..26aa2497 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -6,9 +6,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PathJoinSubstitution from launch_ros.actions import Node ARGUMENTS = [ @@ -63,7 +63,7 @@ def generate_launch_description(): ('/tf_static', 'tf_static') ], output='screen', - condition=LaunchConfigurationNotEquals('namespace', '') + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')) ) # Static transform from /base_link to base_link @@ -79,7 +79,7 @@ def generate_launch_description(): ('/tf_static', 'tf_static') ], output='screen', - condition=LaunchConfigurationNotEquals('namespace', '') + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')) ) ld = LaunchDescription(ARGUMENTS) diff --git a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/behaviors_scheduler.hpp b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/behaviors_scheduler.hpp index 9f732e2e..2463b819 100644 --- a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/behaviors_scheduler.hpp +++ b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/behaviors_scheduler.hpp @@ -9,7 +9,7 @@ #include #include "boost/optional.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "irobot_create_msgs/msg/hazard_detection_vector.hpp" #include "tf2/LinearMath/Transform.h" @@ -28,7 +28,7 @@ struct RobotState class BehaviorsScheduler { public: - using optional_output_t = boost::optional; + using optional_output_t = boost::optional; using run_behavior_func_t = std::function; using is_done_func_t = std::function; using cleanup_func_t = std::function; diff --git a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/drive_goal_behaviors.hpp b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/drive_goal_behaviors.hpp index ef3bd746..f874be7f 100644 --- a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/drive_goal_behaviors.hpp +++ b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/drive_goal_behaviors.hpp @@ -10,7 +10,7 @@ #include #include -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "irobot_create_nodes/motion_control/behaviors_scheduler.hpp" #include "irobot_create_msgs/action/drive_arc.hpp" #include "irobot_create_msgs/action/drive_distance.hpp" @@ -23,7 +23,7 @@ namespace irobot_create_nodes { -geometry_msgs::msg::Twist get_default_velocity_cmd(); +geometry_msgs::msg::TwistStamped get_default_velocity_cmd(); geometry_msgs::msg::PoseStamped get_current_pose_stamped( const rclcpp::Time & current_time, const tf2::Transform & pose); @@ -253,7 +253,7 @@ class DriveArcBehavior : public DriveGoalBaseBehavior first_iter_; - geometry_msgs::msg::Twist arc_velocity_cmd_; + geometry_msgs::msg::TwistStamped arc_velocity_cmd_; float translate_speed_; const float slow_angle_dist_ {0.4}; const float converge_angle_dist_ {M_PI / 360.0f}; @@ -292,7 +292,7 @@ class DriveDistanceBehavior float goal_travel_; tf2::Vector3 start_position_; std::atomic first_iter_; - geometry_msgs::msg::Twist drive_velocity_cmd_; + geometry_msgs::msg::TwistStamped drive_velocity_cmd_; float translate_speed_; const float slow_translate_dist_ {0.1}; const float converge_translate_dist_ {0.005}; @@ -328,7 +328,7 @@ class RotateAngleBehavior : public DriveGoalBaseBehavior first_iter_; - geometry_msgs::msg::Twist rotate_velocity_cmd_; + geometry_msgs::msg::TwistStamped rotate_velocity_cmd_; float servo_speed_; float max_rot_speed_radps_; const float slow_angle_dist_ {0.6}; @@ -369,7 +369,7 @@ class NavigateToPositionBehavior float remain_angle_travel_; int8_t start_sign_; std::atomic first_iter_; - geometry_msgs::msg::Twist rotate_velocity_cmd_; + geometry_msgs::msg::TwistStamped rotate_velocity_cmd_; float servo_speed_; float max_rot_speed_radps_; const float apply_ang_correction_thresh_ {0.05}; diff --git a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/simple_goal_controller.hpp b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/simple_goal_controller.hpp index 33be9d44..54bbc934 100644 --- a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/simple_goal_controller.hpp +++ b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/simple_goal_controller.hpp @@ -94,7 +94,7 @@ class SimpleGoalController gp.x - current_position.getX(), gp.y - current_position.getY()); if (dist_to_goal <= gp.radius) { - servo_vel = geometry_msgs::msg::Twist(); + servo_vel = geometry_msgs::msg::TwistStamped(); navigate_state_ = NavigateStates::GO_TO_GOAL_POSITION; } else { double ang = diff_angle(gp, current_position, current_angle); @@ -103,11 +103,11 @@ class SimpleGoalController ang = angles::normalize_angle(ang + M_PI); } bound_rotation(ang); - servo_vel = geometry_msgs::msg::Twist(); + servo_vel = geometry_msgs::msg::TwistStamped(); if (std::abs(ang) < TO_GOAL_ANGLE_CONVERGED) { navigate_state_ = NavigateStates::GO_TO_GOAL_POSITION; } else { - servo_vel->angular.z = ang; + servo_vel->twist.angular.z = ang; } } break; @@ -124,7 +124,7 @@ class SimpleGoalController // Angle is 180 from travel direction abs_ang = angles::normalize_angle(abs_ang + M_PI); } - servo_vel = geometry_msgs::msg::Twist(); + servo_vel = geometry_msgs::msg::TwistStamped(); // If robot is close enough to goal, move to final stage if (dist_to_goal < goal_points_.front().radius) { navigate_state_ = NavigateStates::GOAL_ANGLE; @@ -140,9 +140,9 @@ class SimpleGoalController if (gp.drive_backwards) { translate_velocity *= -1; } - servo_vel->linear.x = translate_velocity; + servo_vel->twist.linear.x = translate_velocity; if (abs_ang > GO_TO_GOAL_APPLY_ROTATION_ANGLE) { - servo_vel->angular.z = ang; + servo_vel->twist.angular.z = ang; } } break; @@ -153,12 +153,12 @@ class SimpleGoalController angles::shortest_angular_distance(current_angle, goal_points_.front().theta); bound_rotation(ang); if (std::abs(ang) > GOAL_ANGLE_CONVERGED) { - servo_vel = geometry_msgs::msg::Twist(); - servo_vel->angular.z = ang; + servo_vel = geometry_msgs::msg::TwistStamped(); + servo_vel->twist.angular.z = ang; } else { goal_points_.pop_front(); if (goal_points_.size() > 0) { - servo_vel = geometry_msgs::msg::Twist(); + servo_vel = geometry_msgs::msg::TwistStamped(); navigate_state_ = NavigateStates::ANGLE_TO_GOAL; } } diff --git a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp index 279ad381..59ab536c 100644 --- a/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp +++ b/irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control_node.hpp @@ -13,6 +13,7 @@ #include "tf2_ros/transform_listener.h" #include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "irobot_create_msgs/msg/kidnap_status.hpp" #include "irobot_create_msgs/msg/hazard_detection.hpp" #include "irobot_create_msgs/msg/wheel_status.hpp" @@ -82,7 +83,8 @@ class MotionControlNode : public rclcpp::Node void hazard_vector_callback(irobot_create_msgs::msg::HazardDetectionVector::ConstSharedPtr msg); /// \brief Callback for new velocity commands - void commanded_velocity_callback(geometry_msgs::msg::Twist::ConstSharedPtr msg); + void commanded_velocity_callback(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + void commanded_velocity_unstamped_callback(geometry_msgs::msg::Twist::ConstSharedPtr msg); /// \brief Callback for robot odometry void robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); @@ -91,7 +93,7 @@ class MotionControlNode : public rclcpp::Node void kidnap_callback(irobot_create_msgs::msg::KidnapStatus::ConstSharedPtr msg); /// \brief Given command, bound by max speed, checking each wheel - void bound_command_by_limits(geometry_msgs::msg::Twist & cmd); + void bound_command_by_limits(geometry_msgs::msg::TwistStamped & cmd); /// These robot services live in ui-mgr node on robot, but here for convenience enum ResponseStatus : bool @@ -106,10 +108,11 @@ class MotionControlNode : public rclcpp::Node rclcpp::Subscription::SharedPtr hazard_detection_sub_; - rclcpp::Subscription::SharedPtr teleop_subscription_; + rclcpp::Subscription::SharedPtr teleop_subscription_; + rclcpp::Subscription::SharedPtr teleop_unstamped_subscription_; rclcpp::Subscription::SharedPtr odom_pose_sub_; rclcpp::Subscription::SharedPtr kidnap_sub_; - rclcpp::Publisher::SharedPtr cmd_vel_out_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_out_pub_; rclcpp::Publisher::SharedPtr backup_limit_hazard_pub_; rclcpp::Publisher::SharedPtr wheel_status_pub_; rclcpp::TimerBase::SharedPtr control_timer_ {nullptr}; @@ -128,7 +131,7 @@ class MotionControlNode : public rclcpp::Node std::shared_ptr wall_follow_behavior_ {nullptr}; std::mutex mutex_; - geometry_msgs::msg::Twist last_teleop_cmd_; + geometry_msgs::msg::TwistStamped last_teleop_cmd_; rclcpp::Time last_teleop_ts_; rclcpp::Duration wheels_stop_threshold_; std::atomic allow_speed_param_change_ {false}; diff --git a/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp b/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp index 9f1deab1..c08ed4e1 100644 --- a/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp +++ b/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp @@ -43,15 +43,15 @@ void KidnapEstimator::kidnap_callback(irobot_create_msgs::msg::HazardDetectionVe const std::size_t wheel_drop_count = std::count_if( hazard_vector.begin(), hazard_vector.end(), [](auto hazard_vector) { return hazard_vector.header.frame_id == "wheel_drop_left" || - hazard_vector.header.frame_id == "wheel_drop_right"; + hazard_vector.header.frame_id == "wheel_drop_right"; }); const std::size_t cliff_sensor_count = std::count_if( hazard_vector.begin(), hazard_vector.end(), [](auto hazard_vector) { return hazard_vector.header.frame_id == "cliff_side_left" || - hazard_vector.header.frame_id == "cliff_side_right" || - hazard_vector.header.frame_id == "cliff_front_left" || - hazard_vector.header.frame_id == "cliff_front_right"; + hazard_vector.header.frame_id == "cliff_side_right" || + hazard_vector.header.frame_id == "cliff_front_left" || + hazard_vector.header.frame_id == "cliff_front_right"; }); // Set header timestamp. diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control/docking_behavior.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control/docking_behavior.cpp index d4a574da..16459fe5 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control/docking_behavior.cpp +++ b/irobot_create_common/irobot_create_nodes/src/motion_control/docking_behavior.cpp @@ -154,7 +154,7 @@ void DockingBehavior::handle_dock_servo_accepted( BehaviorsScheduler::BehaviorsData data; data.run_func = std::bind(&DockingBehavior::execute_dock_servo, this, goal_handle, _1); data.is_done_func = std::bind(&DockingBehavior::docking_behavior_is_done, this); - data.stop_on_new_behavior = true; + data.stop_on_new_behavior = false; data.apply_backup_limits = false; const bool ret = behavior_scheduler_->set_behavior(data); @@ -291,7 +291,7 @@ void DockingBehavior::handle_undock_accepted( BehaviorsScheduler::BehaviorsData data; data.run_func = std::bind(&DockingBehavior::execute_undock, this, goal_handle, _1); data.is_done_func = std::bind(&DockingBehavior::docking_behavior_is_done, this); - data.stop_on_new_behavior = true; + data.stop_on_new_behavior = false; data.apply_backup_limits = false; const bool ret = behavior_scheduler_->set_behavior(data); diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp index bd5df683..0d38cab2 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp +++ b/irobot_create_common/irobot_create_nodes/src/motion_control/drive_goal_behaviors.cpp @@ -14,15 +14,15 @@ namespace irobot_create_nodes { //// Helper functions //// -geometry_msgs::msg::Twist get_default_velocity_cmd() +geometry_msgs::msg::TwistStamped get_default_velocity_cmd() { - geometry_msgs::msg::Twist default_cmd; - default_cmd.linear.x = 0; - default_cmd.linear.y = 0; - default_cmd.linear.z = 0; - default_cmd.angular.x = 0; - default_cmd.angular.y = 0; - default_cmd.angular.z = 0; + geometry_msgs::msg::TwistStamped default_cmd; + default_cmd.twist.linear.x = 0; + default_cmd.twist.linear.y = 0; + default_cmd.twist.linear.z = 0; + default_cmd.twist.angular.x = 0; + default_cmd.twist.angular.y = 0; + default_cmd.twist.angular.z = 0; return default_cmd; } geometry_msgs::msg::PoseStamped get_current_pose_stamped( @@ -74,10 +74,11 @@ void DriveArcBehavior::initialize_goal(const irobot_create_msgs::action::DriveAr const std::lock_guard lock(drive_arc_params_mutex_); arc_velocity_cmd_ = get_default_velocity_cmd(); - arc_velocity_cmd_.linear.x = max_speed; - arc_velocity_cmd_.angular.z = std::copysign(max_speed / std::abs(goal.radius), goal.angle); + arc_velocity_cmd_.header.stamp = clock_->now(); + arc_velocity_cmd_.twist.linear.x = max_speed; + arc_velocity_cmd_.twist.angular.z = std::copysign(max_speed / std::abs(goal.radius), goal.angle); if (goal.translate_direction == irobot_create_msgs::action::DriveArc::Goal::TRANSLATE_BACKWARD) { - arc_velocity_cmd_.linear.x *= -1.0f; + arc_velocity_cmd_.twist.linear.x *= -1.0f; } remain_angle_travel_ = goal.angle; start_sign_ = std::copysign(1, remain_angle_travel_); @@ -114,13 +115,13 @@ bool DriveArcBehavior::iterate_on_goal( } float scale = abs_remain_angle_travel / slow_angle_dist_; scale = std::max(scale, min_percent_); - output->linear.x = arc_velocity_cmd_.linear.x * scale; - output->angular.z = arc_velocity_cmd_.angular.z * scale; - float abs_linear = std::abs(output->linear.x); + output->twist.linear.x = arc_velocity_cmd_.twist.linear.x * scale; + output->twist.angular.z = arc_velocity_cmd_.twist.angular.z * scale; + float abs_linear = std::abs(output->twist.linear.x); if (abs_linear < min_controllable_) { float up_scale = min_controllable_ / abs_linear; - output->linear.x *= up_scale; - output->angular.z *= up_scale; + output->twist.linear.x *= up_scale; + output->twist.angular.z *= up_scale; } } } @@ -167,12 +168,13 @@ void DriveDistanceBehavior::initialize_goal( first_iter_ = true; const std::lock_guard lock(drive_distance_params_mutex_); drive_velocity_cmd_ = get_default_velocity_cmd(); + drive_velocity_cmd_.header.stamp = clock_->now(); goal_travel_ = goal.distance; travel_distance_sq_ = goal_travel_ * goal_travel_; remaining_travel_ = goal_travel_; float max_speed = std::min(translate_speed_, goal.max_translation_speed); RCLCPP_INFO(logger_, "DriveDistance with distance %f, max_speed %f", goal.distance, max_speed); - drive_velocity_cmd_.linear.x = std::copysign(max_speed, goal_travel_); + drive_velocity_cmd_.twist.linear.x = std::copysign(max_speed, goal_travel_); } bool DriveDistanceBehavior::iterate_on_goal( @@ -204,8 +206,8 @@ bool DriveDistanceBehavior::iterate_on_goal( } abs_remaining = std::max(abs_remaining, min_translate_vel_); float remain_vel = std::copysign(abs_remaining, goal_travel_); - if (std::abs(remain_vel) < std::abs(output->linear.x)) { - output->linear.x = remain_vel; + if (std::abs(remain_vel) < std::abs(output->twist.linear.x)) { + output->twist.linear.x = remain_vel; } } } @@ -252,11 +254,12 @@ void RotateAngleBehavior::initialize_goal( first_iter_ = true; const std::lock_guard lock(rotate_angle_params_mutex_); rotate_velocity_cmd_ = get_default_velocity_cmd(); + rotate_velocity_cmd_.header.stamp = clock_->now(); remain_angle_travel_ = goal.angle; start_sign_ = std::copysign(1, remain_angle_travel_); float max_speed = std::min(max_rot_speed_radps_, goal.max_rotation_speed); RCLCPP_INFO(logger_, "RotateAngle with angle %f, max_speed %f", goal.angle, max_speed); - rotate_velocity_cmd_.angular.z = std::copysign(max_speed, remain_angle_travel_); + rotate_velocity_cmd_.twist.angular.z = std::copysign(max_speed, remain_angle_travel_); } bool RotateAngleBehavior::iterate_on_goal( @@ -287,8 +290,8 @@ bool RotateAngleBehavior::iterate_on_goal( return true; } double slow_speed = std::max(abs_remain_angle_travel, min_angular_vel_); - if (slow_speed < std::abs(output->angular.z)) { - output->angular.z = std::copysign(slow_speed, remain_angle_travel_); + if (slow_speed < std::abs(output->twist.angular.z)) { + output->twist.angular.z = std::copysign(slow_speed, remain_angle_travel_); } } } @@ -428,7 +431,7 @@ bool NavigateToPositionBehavior::iterate_on_goal( // Look for deviation from goal while driving straight and apply small correction float goal_ang_diff = angle_to_goal(current_pose, goal_pose_); if (std::abs(goal_ang_diff) > apply_ang_correction_thresh_) { - output->angular.z += goal_ang_diff; + output->twist.angular.z += goal_ang_diff; } } } diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control/reflex_behavior.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control/reflex_behavior.cpp index cc975265..726415f8 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control/reflex_behavior.cpp +++ b/irobot_create_common/irobot_create_nodes/src/motion_control/reflex_behavior.cpp @@ -254,23 +254,23 @@ BehaviorsScheduler::optional_output_t ReflexBehavior::execute_reflex( running_reflex_ = false; } else if (drive_dir != DriveAwayDirection::NO_HAZARD_TO_ESCAPE) { // Pick velocity to escape hazards based on hazard types and locations - servo_cmd = geometry_msgs::msg::Twist(); + servo_cmd = geometry_msgs::msg::TwistStamped(); switch (drive_dir) { case DriveAwayDirection::PURE_BACKUP: { - servo_cmd->linear.x = BACKUP_X_VELOCITY; + servo_cmd->twist.linear.x = BACKUP_X_VELOCITY; break; } case DriveAwayDirection::ARC_CLOCKWISE: { - servo_cmd->linear.x = ARC_X_VELOCITY; - servo_cmd->angular.z = -ARC_ANGULAR_VELOCITY; + servo_cmd->twist.linear.x = ARC_X_VELOCITY; + servo_cmd->twist.angular.z = -ARC_ANGULAR_VELOCITY; break; } case DriveAwayDirection::ARC_COUNTER_CLOCKWISE: { - servo_cmd->linear.x = ARC_X_VELOCITY; - servo_cmd->angular.z = ARC_ANGULAR_VELOCITY; + servo_cmd->twist.linear.x = ARC_X_VELOCITY; + servo_cmd->twist.angular.z = ARC_ANGULAR_VELOCITY; break; } case DriveAwayDirection::NO_HAZARD_TO_ESCAPE: // this code path is covered above diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_behavior.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_behavior.cpp index 79e5149a..49f12b21 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_behavior.cpp +++ b/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_behavior.cpp @@ -208,9 +208,9 @@ BehaviorsScheduler::optional_output_t WallFollowBehavior::get_next_servo_cmd( current_state.pose, last_ir_intensity_, active_hazard_frames, wf_vel_cmd); } - BehaviorsScheduler::optional_output_t servo_cmd = geometry_msgs::msg::Twist(); - servo_cmd->linear.x = wf_vel_cmd.translate; - servo_cmd->angular.z = wf_vel_cmd.rotate; + BehaviorsScheduler::optional_output_t servo_cmd = geometry_msgs::msg::TwistStamped(); + servo_cmd->twist.linear.x = wf_vel_cmd.translate; + servo_cmd->twist.angular.z = wf_vel_cmd.rotate; return servo_cmd; } diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_states.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_states.cpp index f16b1261..02b55ab5 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_states.cpp +++ b/irobot_create_common/irobot_create_nodes/src/motion_control/wall_follow_states.cpp @@ -166,8 +166,8 @@ bool ObstacleInFront::get_next_velocity( { // Summarize IR sensors auto obstacle_in_sensors = [this](const std::vector & sensors, - const irobot_create_msgs::msg::IrIntensityVector & ir_intensity, - int16_t min_obs_threshold, int16_t & obs_val) -> bool { + const irobot_create_msgs::msg::IrIntensityVector & ir_intensity, + int16_t min_obs_threshold, int16_t & obs_val) -> bool { for (const auto & frame : sensors) { auto frame_it = std::find_if( ir_intensity.readings.begin(), diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp index fe0a142a..4ba32c2a 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp +++ b/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp @@ -111,9 +111,12 @@ MotionControlNode::MotionControlNode(const rclcpp::NodeOptions & options) std::bind(&MotionControlNode::hazard_vector_callback, this, _1)); // Create subscription to let other applications drive the robot - teleop_subscription_ = this->create_subscription( + teleop_subscription_ = this->create_subscription( "cmd_vel", rclcpp::SensorDataQoS(), std::bind(&MotionControlNode::commanded_velocity_callback, this, _1)); + teleop_unstamped_subscription_ = this->create_subscription( + "cmd_vel_unstamped", rclcpp::SensorDataQoS(), + std::bind(&MotionControlNode::commanded_velocity_unstamped_callback, this, _1)); odom_pose_sub_ = this->create_subscription( "odom", rclcpp::SensorDataQoS(), @@ -123,8 +126,8 @@ MotionControlNode::MotionControlNode(const rclcpp::NodeOptions & options) "kidnap_status", rclcpp::SensorDataQoS(), std::bind(&MotionControlNode::kidnap_callback, this, _1)); - cmd_vel_out_pub_ = this->create_publisher( - "diffdrive_controller/cmd_vel_unstamped", rclcpp::SystemDefaultsQoS()); + cmd_vel_out_pub_ = this->create_publisher( + "diffdrive_controller/cmd_vel", rclcpp::SystemDefaultsQoS()); backup_limit_hazard_pub_ = this->create_publisher( "_internal/backup_limit", rclcpp::SensorDataQoS().reliable()); @@ -257,7 +260,7 @@ void MotionControlNode::control_robot() // Create a null command if we don't have anything. // We also disable reflexes because the robot is in an idle state. if (!command) { - command = geometry_msgs::msg::Twist(); + command = geometry_msgs::msg::TwistStamped(); } else if (apply_backup_limits) { bound_command_by_limits(*command); } @@ -274,7 +277,7 @@ void MotionControlNode::control_robot() if (backup_printed_ && !backup_buffer_low_) { backup_printed_ = false; } - auto cmd_out_msg = std::make_unique(); + auto cmd_out_msg = std::make_unique(); if (!e_stop_engaged_) { *cmd_out_msg = *command; } @@ -330,7 +333,8 @@ void MotionControlNode::hazard_vector_callback( reflex_behavior_->update_hazards(current_state_); } -void MotionControlNode::commanded_velocity_callback(geometry_msgs::msg::Twist::ConstSharedPtr msg) +void MotionControlNode::commanded_velocity_callback( + geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { if (scheduler_->has_behavior()) { const auto time_now = this->now(); @@ -349,6 +353,31 @@ void MotionControlNode::commanded_velocity_callback(geometry_msgs::msg::Twist::C last_teleop_ts_ = this->now(); } + +void MotionControlNode::commanded_velocity_unstamped_callback( + geometry_msgs::msg::Twist::ConstSharedPtr msg) +{ + geometry_msgs::msg::TwistStamped stamped_msg; + stamped_msg.twist = *msg; + stamped_msg.header.stamp = this->get_clock()->now(); + + if (scheduler_->has_behavior()) { + const auto time_now = this->now(); + if (time_now - auto_override_print_ts_ > repeat_print_) { + auto_override_print_ts_ = time_now; + RCLCPP_WARN( + this->get_logger(), + "Ignoring velocities commanded while an autonomous behavior is running!"); + } + return; + } + + const std::lock_guard lock(mutex_); + + last_teleop_cmd_ = stamped_msg; + last_teleop_ts_ = this->now(); +} + void MotionControlNode::robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg) { const std::lock_guard lock(current_state_mutex_); @@ -368,21 +397,22 @@ void MotionControlNode::reset_last_teleop_cmd() const std::lock_guard lock(mutex_); last_teleop_cmd_ = get_default_velocity_cmd(); + last_teleop_cmd_.header.stamp = this->get_clock()->now(); last_teleop_ts_ = this->now(); } -void MotionControlNode::bound_command_by_limits(geometry_msgs::msg::Twist & cmd) +void MotionControlNode::bound_command_by_limits(geometry_msgs::msg::TwistStamped & cmd) { - if (std::abs(cmd.angular.z) > GYRO_MAX_ROTATE_SPEED_RAD_S) { - cmd.angular.z = std::copysign(GYRO_MAX_ROTATE_SPEED_RAD_S, cmd.angular.z); + if (std::abs(cmd.twist.angular.z) > GYRO_MAX_ROTATE_SPEED_RAD_S) { + cmd.twist.angular.z = std::copysign(GYRO_MAX_ROTATE_SPEED_RAD_S, cmd.twist.angular.z); } if (safety_override_mode_ == SafetyOverrideMode::NONE && backup_buffer_ <= 0.0 && - cmd.linear.x < 0.0) + cmd.twist.linear.x < 0.0) { // Robot has run out of room to backup - cmd.linear.x = 0.0; - cmd.angular.z = 0.0; + cmd.twist.linear.x = 0.0; + cmd.twist.angular.z = 0.0; const auto time_now = this->now(); if (!backup_printed_) { backup_printed_ = true; @@ -392,8 +422,8 @@ void MotionControlNode::bound_command_by_limits(geometry_msgs::msg::Twist & cmd) safety_override_param_name_.c_str()); } } else { - double left_vel = cmd.linear.x - cmd.angular.z * wheel_base_ / 2.0; - double right_vel = cmd.angular.z * wheel_base_ + left_vel; + double left_vel = cmd.twist.linear.x - cmd.twist.angular.z * wheel_base_ / 2.0; + double right_vel = cmd.twist.angular.z * wheel_base_ + left_vel; const double max_vel = std::max(std::abs(left_vel), std::abs(right_vel)); if (max_vel > 0 && max_vel > max_speed_) { const double scale = max_speed_ / max_vel; @@ -401,8 +431,8 @@ void MotionControlNode::bound_command_by_limits(geometry_msgs::msg::Twist & cmd) left_vel *= scale; right_vel *= scale; // Convert back to cartesian - cmd.linear.x = (left_vel + right_vel) / 2.0; - cmd.angular.z = (right_vel - left_vel) / wheel_base_; + cmd.twist.linear.x = (left_vel + right_vel) / 2.0; + cmd.twist.angular.z = (right_vel - left_vel) / wheel_base_; } } } diff --git a/irobot_create_common/irobot_create_toolbox/package.xml b/irobot_create_common/irobot_create_toolbox/package.xml index 9fa5d0a5..9b8858ee 100644 --- a/irobot_create_common/irobot_create_toolbox/package.xml +++ b/irobot_create_common/irobot_create_toolbox/package.xml @@ -8,6 +8,7 @@ BSD ament_cmake + gz_math_vendor rclcpp diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/CHANGELOG.rst b/irobot_create_gz/irobot_create_gz_bringup/CHANGELOG.rst similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_bringup/CHANGELOG.rst rename to irobot_create_gz/irobot_create_gz_bringup/CHANGELOG.rst diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/CMakeLists.txt b/irobot_create_gz/irobot_create_gz_bringup/CMakeLists.txt similarity index 93% rename from irobot_create_ignition/irobot_create_ignition_bringup/CMakeLists.txt rename to irobot_create_gz/irobot_create_gz_bringup/CMakeLists.txt index 88633218..0c01a074 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/CMakeLists.txt +++ b/irobot_create_gz/irobot_create_gz_bringup/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(irobot_create_ignition_bringup) +project(irobot_create_gz_bringup) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/irobot_create_gz/irobot_create_gz_bringup/README.md b/irobot_create_gz/irobot_create_gz_bringup/README.md new file mode 100644 index 00000000..8b23bc13 --- /dev/null +++ b/irobot_create_gz/irobot_create_gz_bringup/README.md @@ -0,0 +1,22 @@ +# turtlebot4_gz + +To launch the simulation, run +```bash +ros2 launch irobot_create_gz_bringup create3_gz.launch.py +``` + +Wait for the simulation environment to load completely, then press the orange `play` button in the +lower-left corner to start the simulation. The robot starts docked on its charger. + +## Worlds + +The default simulation world is the `depot` environment. To chage worlds, use the `world` +argument. Supported worlds are: +- `depot` (default) +- `maze` + +## Robot spawn location + +By default the robot spawns at the origin. To change the robot's spawn location, use the +`x`, `y`, `z` and `yaw` arguments. Note that changing `z` may result in the robot spawning +in the air or below the ground plane, depending in the value used. \ No newline at end of file diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml b/irobot_create_gz/irobot_create_gz_bringup/config/pose_republisher_params.yaml similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml rename to irobot_create_gz/irobot_create_gz_bringup/config/pose_republisher_params.yaml diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/config/sensors_params.yaml b/irobot_create_gz/irobot_create_gz_bringup/config/sensors_params.yaml similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_bringup/config/sensors_params.yaml rename to irobot_create_gz/irobot_create_gz_bringup/config/sensors_params.yaml diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config b/irobot_create_gz/irobot_create_gz_bringup/gui/create3/gui.config similarity index 78% rename from irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config rename to irobot_create_gz/irobot_create_gz_bringup/gui/create3/gui.config index 4f650410..d07f693c 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config +++ b/irobot_create_gz/irobot_create_gz_bringup/gui/create3/gui.config @@ -1,16 +1,31 @@ - - - - 3D View - false - docked - - ogre2 - scene - -0.0176 -0.737 0.017 0 -0.135 1.64 - + + + + 3D View + docked + false + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + false + floating + + + + + false + floating + + + @@ -19,7 +34,6 @@ false false 72 - 121 1 floating @@ -91,7 +105,6 @@ - diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz.launch.py similarity index 78% rename from irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py rename to irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz.launch.py index b7baa641..67642025 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py +++ b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz.launch.py @@ -27,14 +27,16 @@ def generate_launch_description(): # Directories - pkg_irobot_create_ignition_bringup = get_package_share_directory( - 'irobot_create_ignition_bringup') + pkg_irobot_create_gz_bringup = get_package_share_directory( + 'irobot_create_gz_bringup') # Paths ignition_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'ignition.launch.py']) + [pkg_irobot_create_gz_bringup, 'launch', 'sim.launch.py']) robot_spawn_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_spawn.launch.py']) + [pkg_irobot_create_gz_bringup, 'launch', 'create3_spawn.launch.py']) + create3_nodes_launch = PathJoinSubstitution( + [pkg_irobot_create_gz_bringup, 'launch', 'create3_gz_nodes.launch.py']) ignition = IncludeLaunchDescription( PythonLaunchDescriptionSource([ignition_launch]), @@ -53,8 +55,13 @@ def generate_launch_description(): ('z', LaunchConfiguration('z')), ('yaw', LaunchConfiguration('yaw'))]) + create3_nodes = IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[]) + # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) ld.add_action(ignition) ld.add_action(robot_spawn) + ld.add_action(create3_nodes) return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz_nodes.launch.py similarity index 82% rename from irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py rename to irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz_nodes.launch.py index a2662d46..e0d72710 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py +++ b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_gz_nodes.launch.py @@ -19,16 +19,16 @@ def generate_launch_description(): # Directories - pkg_create3_ignition_bringup = get_package_share_directory('irobot_create_ignition_bringup') + pkg_create3_gz_bringup = get_package_share_directory('irobot_create_gz_bringup') pose_republisher_params_yaml_file = PathJoinSubstitution( - [pkg_create3_ignition_bringup, 'config', 'pose_republisher_params.yaml']) + [pkg_create3_gz_bringup, 'config', 'pose_republisher_params.yaml']) sensors_params_yaml_file = PathJoinSubstitution( - [pkg_create3_ignition_bringup, 'config', 'sensors_params.yaml']) + [pkg_create3_gz_bringup, 'config', 'sensors_params.yaml']) # Pose republisher pose_republisher_node = Node( - package='irobot_create_ignition_toolbox', + package='irobot_create_gz_toolbox', name='pose_republisher_node', executable='pose_republisher_node', parameters=[pose_republisher_params_yaml_file, @@ -40,7 +40,7 @@ def generate_launch_description(): # Sensors sensors_node = Node( - package='irobot_create_ignition_toolbox', + package='irobot_create_gz_toolbox', name='sensors_node', executable='sensors_node', parameters=[sensors_params_yaml_file, @@ -50,7 +50,7 @@ def generate_launch_description(): # Interface buttons interface_buttons_node = Node( - package='irobot_create_ignition_toolbox', + package='irobot_create_gz_toolbox', name='interface_buttons_node', executable='interface_buttons_node', parameters=[{'use_sim_time': True}], diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_ros_gz_bridge.launch.py similarity index 97% rename from irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py rename to irobot_create_gz/irobot_create_gz_bringup/launch/create3_ros_gz_bridge.launch.py index ced37aae..66df2026 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_ros_gz_bridge.launch.py @@ -57,15 +57,15 @@ def generate_launch_description(): }], arguments=[ [namespace, - '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'], + '/cmd_vel' + '@geometry_msgs/msg/TwistStamped' + '[ignition.msgs.Twist'], ['/model/', robot_name, '/cmd_vel' + - '@geometry_msgs/msg/Twist' + + '@geometry_msgs/msg/TwistStamped' + ']ignition.msgs.Twist'] ], remappings=[ ([namespace, '/cmd_vel'], 'cmd_vel'), (['/model/', robot_name, '/cmd_vel'], - 'diffdrive_controller/cmd_vel_unstamped') + 'diffdrive_controller/cmd_vel') ]) # Pose bridge diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_spawn.launch.py similarity index 93% rename from irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py rename to irobot_create_gz/irobot_create_gz_bringup/launch/create3_spawn.launch.py index 73cf5372..92bce524 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_gz/irobot_create_gz_bringup/launch/create3_spawn.launch.py @@ -43,16 +43,16 @@ def generate_launch_description(): # Directories pkg_irobot_create_common_bringup = get_package_share_directory( 'irobot_create_common_bringup') - pkg_irobot_create_ignition_bringup = get_package_share_directory( - 'irobot_create_ignition_bringup') + pkg_irobot_create_gz_bringup = get_package_share_directory( + 'irobot_create_gz_bringup') # Paths ros_gz_bridge_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ros_ignition_bridge.launch.py']) + [pkg_irobot_create_gz_bringup, 'launch', 'create3_ros_gz_bridge.launch.py']) create3_nodes_launch = PathJoinSubstitution( [pkg_irobot_create_common_bringup, 'launch', 'create3_nodes.launch.py']) - create3_ignition_nodes_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ignition_nodes.launch.py']) + create3_gz_nodes_launch = PathJoinSubstitution( + [pkg_irobot_create_gz_bringup, 'launch', 'create3_gz_nodes.launch.py']) robot_description_launch = PathJoinSubstitution( [pkg_irobot_create_common_bringup, 'launch', 'robot_description.launch.py']) dock_description_launch = PathJoinSubstitution( @@ -141,7 +141,7 @@ def generate_launch_description(): # Create 3 Ignition nodes IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), + PythonLaunchDescriptionSource([create3_gz_nodes_launch]), launch_arguments=[ ('robot_name', robot_name), ('dock_name', dock_name), diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py b/irobot_create_gz/irobot_create_gz_bringup/launch/sim.launch.py similarity index 52% rename from irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py rename to irobot_create_gz/irobot_create_gz_bringup/launch/sim.launch.py index db4eac4d..28e64bdc 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py +++ b/irobot_create_gz/irobot_create_gz_bringup/launch/sim.launch.py @@ -27,44 +27,49 @@ def generate_launch_description(): # Directories - pkg_irobot_create_ignition_bringup = get_package_share_directory( - 'irobot_create_ignition_bringup') - #pkg_irobot_create_ignition_plugins = get_package_share_directory( - # 'irobot_create_ignition_plugins') + pkg_irobot_create_gz_bringup = get_package_share_directory( + 'irobot_create_gz_bringup') + pkg_irobot_create_gz_plugins = get_package_share_directory( + 'irobot_create_gz_plugins') pkg_irobot_create_description = get_package_share_directory( 'irobot_create_description') pkg_ros_gz_sim = get_package_share_directory( 'ros_gz_sim') # Set Ignition resource path - ign_resource_path = SetEnvironmentVariable(name='GZ_SIM_RESOURCE_PATH', - value=[os.path.join( - pkg_irobot_create_ignition_bringup, - 'worlds'), ':' + - str(Path( - pkg_irobot_create_description). - parent.resolve())]) + gz_resource_path = SetEnvironmentVariable( + name='GZ_SIM_RESOURCE_PATH', + value=':'.join([ + os.path.join(pkg_irobot_create_gz_bringup, 'worlds'), + str(Path(pkg_irobot_create_description).parent.resolve()) + ]) + ) - #gz_gui_plugin_path = SetEnvironmentVariable(name='GZ_GUI_PLUGIN_PATH', - # value=[os.path.join( - # pkg_irobot_create_ignition_plugins, - # 'lib')]) + gz_gui_plugin_path = SetEnvironmentVariable( + name='GZ_GUI_PLUGIN_PATH', + value=':'.join([ + os.path.join(pkg_irobot_create_gz_plugins, 'lib') + ]) + ) # Paths gz_sim_launch = PathJoinSubstitution( [pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py']) # Ignition gazebo - ignition_gazebo = IncludeLaunchDescription( + gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource([gz_sim_launch]), - launch_arguments=[ - ('ign_args', [LaunchConfiguration('world'), - '.sdf', - ' -v 4', - ' --gui-config ', - PathJoinSubstitution([pkg_irobot_create_ignition_bringup, - 'gui', 'create3', 'gui.config'])]) - ] + launch_arguments=[( + 'gz_args', [ + LaunchConfiguration('world'), + '.sdf', + ' -v 4', + ' --gui-config ', + PathJoinSubstitution( + [pkg_irobot_create_gz_bringup, 'gui', 'create3', 'gui.config'] + ) + ] + )] ) # clock bridge @@ -77,8 +82,8 @@ def generate_launch_description(): # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) - ld.add_action(ign_resource_path) - #ld.add_action(gz_gui_plugin_path) - ld.add_action(ignition_gazebo) + ld.add_action(gz_resource_path) + ld.add_action(gz_gui_plugin_path) + ld.add_action(gz_sim) ld.add_action(clock_bridge) return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/package.xml b/irobot_create_gz/irobot_create_gz_bringup/package.xml similarity index 93% rename from irobot_create_ignition/irobot_create_ignition_bringup/package.xml rename to irobot_create_gz/irobot_create_gz_bringup/package.xml index a54a7455..c035a30a 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/package.xml +++ b/irobot_create_gz/irobot_create_gz_bringup/package.xml @@ -1,7 +1,7 @@ - irobot_create_ignition_bringup + irobot_create_gz_bringup 2.1.0 Provides launch and configuration scripts for a Ignition simulated iRobot(R) Create(R) 3 Educational Robot. rkreinin @@ -14,7 +14,7 @@ irobot_create_description irobot_create_common_bringup - irobot_create_ignition_toolbox + irobot_create_gz_toolbox gz_ros2_control diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf b/irobot_create_gz/irobot_create_gz_bringup/worlds/depot.sdf similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf rename to irobot_create_gz/irobot_create_gz_bringup/worlds/depot.sdf diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/worlds/maze.sdf b/irobot_create_gz/irobot_create_gz_bringup/worlds/maze.sdf similarity index 99% rename from irobot_create_ignition/irobot_create_ignition_bringup/worlds/maze.sdf rename to irobot_create_gz/irobot_create_gz_bringup/worlds/maze.sdf index 4a4492d1..3a3a10a7 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/worlds/maze.sdf +++ b/irobot_create_gz/irobot_create_gz_bringup/worlds/maze.sdf @@ -362,19 +362,19 @@ - 0 5 0 0 0 0 + 0 5.5 0 0 0 0 true - 2 10 1 + 2 9 1 - 2 10 1 + 2 9 1 diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/CHANGELOG.rst b/irobot_create_gz/irobot_create_gz_plugins/CHANGELOG.rst similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_plugins/CHANGELOG.rst rename to irobot_create_gz/irobot_create_gz_plugins/CHANGELOG.rst diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/CMakeLists.txt b/irobot_create_gz/irobot_create_gz_plugins/CMakeLists.txt similarity index 87% rename from irobot_create_ignition/irobot_create_ignition_plugins/CMakeLists.txt rename to irobot_create_gz/irobot_create_gz_plugins/CMakeLists.txt index ffe03a8e..3f86601f 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/CMakeLists.txt +++ b/irobot_create_gz/irobot_create_gz_plugins/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(irobot_create_ignition_plugins) +project(irobot_create_gz_plugins) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -11,7 +11,7 @@ find_package(ament_cmake REQUIRED) set(OpenGL_GL_PREFERENCE LEGACY) # Find the Ignition gui library -find_package(gz-gui8) +find_package(gz-gui8 REQUIRED) add_subdirectory(Create3Hmi) if(BUILD_TESTING) diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/CMakeLists.txt b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/CMakeLists.txt similarity index 96% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/CMakeLists.txt rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/CMakeLists.txt index 5f09addc..721c1ca2 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/CMakeLists.txt +++ b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(Qt5 REQUIRED ) +find_package(gz-gui8 REQUIRED) + qt5_add_resources(resources_rcc Create3Hmi.qrc) add_library( diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.cc similarity index 92% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.cc index d042b776..0ba3a0f0 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc +++ b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.cc @@ -57,7 +57,7 @@ void Create3Hmi::OnCreate3Button(const int button) button_msg.set_data(button); if (!this->create3_button_pub_.Publish(button_msg)) { - ignerr << "gz::msgs::Int32 message couldn't be published at topic: " << + gzerr << "gz::msgs::Int32 message couldn't be published at topic: " << this->create3_button_topic_ << std::endl; } } @@ -72,7 +72,7 @@ void Create3Hmi::SetNamespace(const QString &_name) this->namespace_ = _name.toStdString(); this->create3_button_topic_ = this->namespace_ + "/create3_buttons"; - ignmsg << "A new robot name has been entered, publishing on topic: '" << + gzlog << "A new robot name has been entered, publishing on topic: '" << this->create3_button_topic_ << " ' " <findChild()->notifyWithDuration( QString::fromStdString("Error when advertising topic: " + this->create3_button_topic_), 4000); - ignerr << "Error when advertising topic: " << + gzerr << "Error when advertising topic: " << this->create3_button_topic_ << std::endl; }else { App()->findChild()->notifyWithDuration( diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.config b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.config similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.config rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.config diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.hh similarity index 77% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.hh index a20456b3..6ca10104 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh +++ b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.hh @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ -#define IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ +#ifndef IROBOT_CREATE_GZ__IROBOT_CREATE_GZ_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ +#define IROBOT_CREATE_GZ__IROBOT_CREATE_GZ_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ #include @@ -60,14 +60,14 @@ protected slots: void OnCreate3Button(const int button); private: - ignition::transport::Node node_; - ignition::transport::Node::Publisher create3_button_pub_; + gz::transport::Node node_; + gz::transport::Node::Publisher create3_button_pub_; std::string namespace_ = ""; std::string create3_button_topic_ = "/create3_buttons"; }; } // namespace gui -} // namespace ignition +} // namespace gz -#endif // IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ +#endif // IROBOT_CREATE_GZ__IROBOT_CREATE_GZ_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.qml similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.qml diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qrc b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.qrc similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qrc rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/Create3Hmi.qrc diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/images/One Dot.png b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/One Dot.png similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/images/One Dot.png rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/One Dot.png diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/images/Power.png b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Power.png similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/images/Power.png rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Power.png diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/images/Two Dots.png b/irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Two Dots.png similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/images/Two Dots.png rename to irobot_create_gz/irobot_create_gz_plugins/Create3Hmi/images/Two Dots.png diff --git a/irobot_create_gz/irobot_create_gz_plugins/README.md b/irobot_create_gz/irobot_create_gz_plugins/README.md new file mode 100644 index 00000000..4a997f30 --- /dev/null +++ b/irobot_create_gz/irobot_create_gz_plugins/README.md @@ -0,0 +1 @@ +# turtlebot4_gz_gui_plugins diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/package.xml b/irobot_create_gz/irobot_create_gz_plugins/package.xml similarity index 90% rename from irobot_create_ignition/irobot_create_ignition_plugins/package.xml rename to irobot_create_gz/irobot_create_gz_plugins/package.xml index d96a6ebb..23ed82f1 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/package.xml +++ b/irobot_create_gz/irobot_create_gz_plugins/package.xml @@ -1,13 +1,14 @@ - irobot_create_ignition_plugins + irobot_create_gz_plugins 2.1.0 Ignition plugins for simulated iRobot(R) Create(R) 3 Educational Robot. rkreinin BSD ament_cmake + gz_gui_vendor ros_gz diff --git a/irobot_create_ignition/irobot_create_ignition_sim/CHANGELOG.rst b/irobot_create_gz/irobot_create_gz_sim/CHANGELOG.rst similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_sim/CHANGELOG.rst rename to irobot_create_gz/irobot_create_gz_sim/CHANGELOG.rst diff --git a/irobot_create_ignition/irobot_create_ignition_sim/CMakeLists.txt b/irobot_create_gz/irobot_create_gz_sim/CMakeLists.txt similarity index 70% rename from irobot_create_ignition/irobot_create_ignition_sim/CMakeLists.txt rename to irobot_create_gz/irobot_create_gz_sim/CMakeLists.txt index c335d84d..8cb53b49 100644 --- a/irobot_create_ignition/irobot_create_ignition_sim/CMakeLists.txt +++ b/irobot_create_gz/irobot_create_gz_sim/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 3.5) -project(irobot_create_ignition_sim) +project(irobot_create_gz_sim) find_package(ament_cmake REQUIRED) ament_package() diff --git a/irobot_create_ignition/irobot_create_ignition_sim/package.xml b/irobot_create_gz/irobot_create_gz_sim/package.xml similarity index 71% rename from irobot_create_ignition/irobot_create_ignition_sim/package.xml rename to irobot_create_gz/irobot_create_gz_sim/package.xml index 33d71b9d..838868dc 100644 --- a/irobot_create_ignition/irobot_create_ignition_sim/package.xml +++ b/irobot_create_gz/irobot_create_gz_sim/package.xml @@ -1,7 +1,7 @@ - irobot_create_ignition_sim + irobot_create_gz_sim 2.1.0 Metapackage for the iRobot(R) Create(R) 3 robot Ignition simulator @@ -13,9 +13,9 @@ ament_cmake - irobot_create_ignition_bringup - irobot_create_ignition_plugins - irobot_create_ignition_toolbox + irobot_create_gz_bringup + irobot_create_gz_plugins + irobot_create_gz_toolbox ament_cmake diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/CHANGELOG.rst b/irobot_create_gz/irobot_create_gz_toolbox/CHANGELOG.rst similarity index 100% rename from irobot_create_ignition/irobot_create_ignition_toolbox/CHANGELOG.rst rename to irobot_create_gz/irobot_create_gz_toolbox/CHANGELOG.rst diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/CMakeLists.txt b/irobot_create_gz/irobot_create_gz_toolbox/CMakeLists.txt similarity index 70% rename from irobot_create_ignition/irobot_create_ignition_toolbox/CMakeLists.txt rename to irobot_create_gz/irobot_create_gz_toolbox/CMakeLists.txt index 4765a1b3..a279bf7a 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/CMakeLists.txt +++ b/irobot_create_gz/irobot_create_gz_toolbox/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(irobot_create_ignition_toolbox) +project(irobot_create_gz_toolbox) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -55,21 +55,21 @@ set(dependencies ) # Pose republisher -add_library(irobot_create_ignition_pose_republisher_lib SHARED) +add_library(irobot_create_gz_pose_republisher_lib SHARED) target_sources( - irobot_create_ignition_pose_republisher_lib + irobot_create_gz_pose_republisher_lib PRIVATE src/pose_republisher/pose_republisher_node.cpp ) -target_include_directories(irobot_create_ignition_pose_republisher_lib PUBLIC include) -ament_target_dependencies(irobot_create_ignition_pose_republisher_lib +target_include_directories(irobot_create_gz_pose_republisher_lib PUBLIC include) +ament_target_dependencies(irobot_create_gz_pose_republisher_lib ${dependencies} ) # Sensors -add_library(irobot_create_ignition_sensors_lib SHARED) +add_library(irobot_create_gz_sensors_lib SHARED) target_sources( - irobot_create_ignition_sensors_lib + irobot_create_gz_sensors_lib PRIVATE src/sensors/sensors_node.cpp src/sensors/bumper.cpp @@ -79,27 +79,27 @@ target_sources( src/sensors/mouse.cpp src/sensors/wheel_drop.cpp ) -target_include_directories(irobot_create_ignition_sensors_lib PUBLIC include) -ament_target_dependencies(irobot_create_ignition_sensors_lib +target_include_directories(irobot_create_gz_sensors_lib PUBLIC include) +ament_target_dependencies(irobot_create_gz_sensors_lib ${dependencies} ) # Interface buttons -add_library(irobot_create_ignition_interface_buttons_lib SHARED) +add_library(irobot_create_gz_interface_buttons_lib SHARED) target_sources( - irobot_create_ignition_interface_buttons_lib + irobot_create_gz_interface_buttons_lib PRIVATE src/interface_buttons/interface_buttons_node.cpp ) -target_include_directories(irobot_create_ignition_interface_buttons_lib PUBLIC include) -ament_target_dependencies(irobot_create_ignition_interface_buttons_lib +target_include_directories(irobot_create_gz_interface_buttons_lib PUBLIC include) +ament_target_dependencies(irobot_create_gz_interface_buttons_lib ${dependencies} ) set(libraries_names - irobot_create_ignition_pose_republisher_lib - irobot_create_ignition_sensors_lib - irobot_create_ignition_interface_buttons_lib + irobot_create_gz_pose_republisher_lib + irobot_create_gz_sensors_lib + irobot_create_gz_interface_buttons_lib ) # Executables @@ -111,7 +111,7 @@ target_sources( PRIVATE src/pose_republisher/pose_republisher_main.cpp ) -target_link_libraries(pose_republisher_node irobot_create_ignition_pose_republisher_lib) +target_link_libraries(pose_republisher_node irobot_create_gz_pose_republisher_lib) # Sensors node add_executable(sensors_node) @@ -120,7 +120,7 @@ target_sources( PRIVATE src/sensors/sensors_main.cpp ) -target_link_libraries(sensors_node irobot_create_ignition_sensors_lib) +target_link_libraries(sensors_node irobot_create_gz_sensors_lib) # Interface buttons node add_executable(interface_buttons_node) @@ -129,7 +129,7 @@ target_sources( PRIVATE src/interface_buttons/interface_buttons_main.cpp ) -target_link_libraries(interface_buttons_node irobot_create_ignition_interface_buttons_lib) +target_link_libraries(interface_buttons_node irobot_create_gz_interface_buttons_lib) set(executables_names pose_republisher_node diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/interface_buttons/interface_buttons_node.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp similarity index 70% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/interface_buttons/interface_buttons_node.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp index a5c8eeb3..411cd565 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/interface_buttons/interface_buttons_node.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ #include #include @@ -15,7 +15,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/int32.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { enum Create3Buttons @@ -41,6 +41,6 @@ class InterfaceButtons : public rclcpp::Node std::unique_ptr interface_buttons_; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__INTERFACE_BUTTONS__INTERFACE_BUTTONS_NODE_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/pose_republisher/pose_republisher.hpp similarity index 81% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/pose_republisher/pose_republisher.hpp index e92594e6..cf1ed8c9 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/pose_republisher/pose_republisher.hpp @@ -3,20 +3,20 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ #include #include "control_msgs/msg/dynamic_joint_state.hpp" -#include "irobot_create_ignition_toolbox/utils.hpp" +#include "irobot_create_gz_toolbox/utils.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { class PoseRepublisher : public rclcpp::Node @@ -50,6 +50,6 @@ class PoseRepublisher : public rclcpp::Node std::string wheel_joints_[2]; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__POSE_REPUBLISHER__POSE_REPUBLISHER_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/bumper.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/bumper.hpp similarity index 79% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/bumper.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/bumper.hpp index 08165cee..08fa57e8 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/bumper.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/bumper.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__BUMPER_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__BUMPER_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__BUMPER_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__BUMPER_HPP_ #include #include @@ -17,7 +17,7 @@ #include "ros_gz_interfaces/msg/contacts.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { class Bumper @@ -40,6 +40,6 @@ class Bumper std::mutex robot_pose_mutex_; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__BUMPER_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__BUMPER_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/cliff.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/cliff.hpp similarity index 76% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/cliff.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/cliff.hpp index f9efde11..eb138af8 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/cliff.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/cliff.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__CLIFF_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__CLIFF_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__CLIFF_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__CLIFF_HPP_ #include #include @@ -15,7 +15,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/laser_scan.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { class Cliff @@ -44,6 +44,6 @@ class Cliff std::string cliff_sensors_[4]; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__CLIFF_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__CLIFF_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/ir_intensity.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_intensity.hpp similarity index 73% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/ir_intensity.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_intensity.hpp index 370cca67..056a9642 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/ir_intensity.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_intensity.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ #include #include @@ -15,7 +15,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/laser_scan.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { class IrIntensity @@ -36,6 +36,6 @@ class IrIntensity std::string ir_intensity_sensors_[7]; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_INTENSITY_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/ir_opcode.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_opcode.hpp similarity index 89% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/ir_opcode.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_opcode.hpp index d12293e6..f9e8c667 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/ir_opcode.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/ir_opcode.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__IR_OPCODE_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__IR_OPCODE_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_OPCODE_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_OPCODE_HPP_ #include #include @@ -15,10 +15,10 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "irobot_create_ignition_toolbox/utils.hpp" +#include "irobot_create_gz_toolbox/utils.hpp" #include "irobot_create_toolbox/polar_coordinates.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { class IrOpcode @@ -93,6 +93,6 @@ class IrOpcode bool is_dock_visible_ = false; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__IR_OPCODE_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__IR_OPCODE_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/mouse.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/mouse.hpp similarity index 76% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/mouse.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/mouse.hpp index 8e66794f..b33c868e 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/mouse.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/mouse.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__MOUSE_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__MOUSE_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__MOUSE_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__MOUSE_HPP_ #include #include @@ -15,7 +15,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { class Mouse @@ -40,6 +40,6 @@ class Mouse tf2::Vector3 last_mouse_position_; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__MOUSE_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__MOUSE_HPP_ diff --git a/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/sensors_node.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/sensors_node.hpp new file mode 100644 index 00000000..da35f045 --- /dev/null +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/sensors_node.hpp @@ -0,0 +1,46 @@ +/* + * Copyright 2021 Clearpath Robotics, Inc. + * @author Roni Kreinin (rkreinin@clearpathrobotics.com) + */ + +#ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ + +#include +#include +#include +#include + +#include "irobot_create_gz_toolbox/sensors/bumper.hpp" +#include "irobot_create_gz_toolbox/sensors/cliff.hpp" +#include "irobot_create_gz_toolbox/sensors/ir_intensity.hpp" +#include "irobot_create_gz_toolbox/sensors/mouse.hpp" +#include "irobot_create_gz_toolbox/sensors/wheel_drop.hpp" +#include "irobot_create_gz_toolbox/sensors/ir_opcode.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace irobot_create_gz_toolbox +{ + +class SensorsNode : public rclcpp::Node +{ +public: + // Constructor and Destructor + SensorsNode(); + +private: + // Node + rclcpp::Node::SharedPtr nh_; + + // Sensors + std::unique_ptr bumper_; + std::unique_ptr cliff_; + std::unique_ptr ir_intensity_; + std::unique_ptr mouse_; + std::unique_ptr wheel_drop_; + std::unique_ptr ir_opcode_; +}; + +} // namespace irobot_create_gz_toolbox + +#endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/wheel_drop.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/wheel_drop.hpp similarity index 76% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/wheel_drop.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/wheel_drop.hpp index 30fed9b5..b889d826 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/wheel_drop.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/sensors/wheel_drop.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ #include #include @@ -14,7 +14,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/joint_state.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { class WheelDrop @@ -40,6 +40,6 @@ class WheelDrop std::map displacement_; }; -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__SENSORS__WHEEL_DROP_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/utils.hpp b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/utils.hpp similarity index 92% rename from irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/utils.hpp rename to irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/utils.hpp index a41ae7e2..8ea31ad5 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/utils.hpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/include/irobot_create_gz_toolbox/utils.hpp @@ -3,8 +3,8 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__UTILS_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__UTILS_HPP_ +#ifndef IROBOT_CREATE_GZ_TOOLBOX__UTILS_HPP_ +#define IROBOT_CREATE_GZ_TOOLBOX__UTILS_HPP_ #include #include @@ -14,7 +14,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_msgs/msg/tf_message.hpp" -namespace irobot_create_ignition_toolbox +namespace irobot_create_gz_toolbox { namespace utils @@ -97,6 +97,6 @@ inline void tf2_transform_to_pose(const tf2::Transform tf, geometry_msgs::msg::P } // namespace utils -} // namespace irobot_create_ignition_toolbox +} // namespace irobot_create_gz_toolbox -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__UTILS_HPP_ +#endif // IROBOT_CREATE_GZ_TOOLBOX__UTILS_HPP_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/package.xml b/irobot_create_gz/irobot_create_gz_toolbox/package.xml similarity index 96% rename from irobot_create_ignition/irobot_create_ignition_toolbox/package.xml rename to irobot_create_gz/irobot_create_gz_toolbox/package.xml index 721972c5..bc885b55 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/package.xml +++ b/irobot_create_gz/irobot_create_gz_toolbox/package.xml @@ -1,7 +1,7 @@ - irobot_create_ignition_toolbox + irobot_create_gz_toolbox 2.1.0 Nodes and tools for simulating in Ignition iRobot(R) Create(R) 3 Educational Robot. rkreinin diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_main.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_main.cpp similarity index 56% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_main.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_main.cpp index 1988c262..89df0700 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_main.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_main.cpp @@ -3,13 +3,13 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#include "irobot_create_ignition_toolbox/interface_buttons/interface_buttons_node.hpp" +#include "irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp" #include int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_node.cpp similarity index 95% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_node.cpp index 2d26a219..2c7a3566 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/interface_buttons/interface_buttons_node.cpp @@ -3,10 +3,10 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#include "irobot_create_ignition_toolbox/interface_buttons/interface_buttons_node.hpp" +#include "irobot_create_gz_toolbox/interface_buttons/interface_buttons_node.hpp" #include -using irobot_create_ignition_toolbox::InterfaceButtons; +using irobot_create_gz_toolbox::InterfaceButtons; InterfaceButtons::InterfaceButtons() : rclcpp::Node("sensors_node") diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_main.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/pose_republisher/pose_republisher_main.cpp similarity index 57% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_main.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/pose_republisher/pose_republisher_main.cpp index 8950bc7f..51386435 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_main.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/pose_republisher/pose_republisher_main.cpp @@ -3,13 +3,13 @@ * @author Roni Kreinin (rkreinin@clearpathrobotics.com) */ -#include "irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp" +#include "irobot_create_gz_toolbox/pose_republisher/pose_republisher.hpp" #include int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/pose_republisher/pose_republisher_node.cpp similarity index 98% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/pose_republisher/pose_republisher_node.cpp index 632ba162..6e4e6655 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/pose_republisher/pose_republisher_node.cpp @@ -6,9 +6,9 @@ #include #include -#include "irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp" +#include "irobot_create_gz_toolbox/pose_republisher/pose_republisher.hpp" -using irobot_create_ignition_toolbox::PoseRepublisher; +using irobot_create_gz_toolbox::PoseRepublisher; PoseRepublisher::PoseRepublisher() : rclcpp::Node("pose_republisher_node"), diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/bumper.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/bumper.cpp similarity index 95% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/bumper.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/bumper.cpp index b03c6055..2f2cefd8 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/bumper.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/bumper.cpp @@ -6,13 +6,13 @@ #include #include -#include "irobot_create_ignition_toolbox/sensors/bumper.hpp" -#include "irobot_create_ignition_toolbox/utils.hpp" +#include "irobot_create_gz_toolbox/sensors/bumper.hpp" +#include "irobot_create_gz_toolbox/utils.hpp" #include "irobot_create_toolbox/math.hpp" #include "irobot_create_toolbox/polar_coordinates.hpp" #include "irobot_create_toolbox/sensors/bumpers.hpp" -using irobot_create_ignition_toolbox::Bumper; +using irobot_create_gz_toolbox::Bumper; Bumper::Bumper(std::shared_ptr & nh) : nh_(nh) diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/cliff.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/cliff.cpp similarity index 94% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/cliff.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/cliff.cpp index 2030361b..29a56684 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/cliff.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/cliff.cpp @@ -7,10 +7,10 @@ #include #include -#include "irobot_create_ignition_toolbox/sensors/cliff.hpp" +#include "irobot_create_gz_toolbox/sensors/cliff.hpp" #include "irobot_create_toolbox/math.hpp" -using irobot_create_ignition_toolbox::Cliff; +using irobot_create_gz_toolbox::Cliff; Cliff::Cliff(std::shared_ptr & nh) : nh_(nh), diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_intensity.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/ir_intensity.cpp similarity index 95% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_intensity.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/ir_intensity.cpp index f8798ecb..13e0de4d 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_intensity.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/ir_intensity.cpp @@ -8,10 +8,10 @@ #include #include -#include "irobot_create_ignition_toolbox/sensors/ir_intensity.hpp" +#include "irobot_create_gz_toolbox/sensors/ir_intensity.hpp" #include "irobot_create_toolbox/math.hpp" -using irobot_create_ignition_toolbox::IrIntensity; +using irobot_create_gz_toolbox::IrIntensity; IrIntensity::IrIntensity(std::shared_ptr & nh) : nh_(nh), diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_opcode.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/ir_opcode.cpp similarity index 96% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_opcode.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/ir_opcode.cpp index ec8077a7..f40e003d 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_opcode.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/ir_opcode.cpp @@ -6,10 +6,10 @@ #include #include -#include "irobot_create_ignition_toolbox/sensors/ir_opcode.hpp" +#include "irobot_create_gz_toolbox/sensors/ir_opcode.hpp" #include "irobot_create_toolbox/polar_coordinates.hpp" -using irobot_create_ignition_toolbox::IrOpcode; +using irobot_create_gz_toolbox::IrOpcode; IrOpcode::IrOpcode(std::shared_ptr & nh) : nh_(nh) @@ -121,7 +121,7 @@ IrOpcode::EmitterCartesianPointToReceiverPolarPoint(const tf2::Vector3 & emitter receiver_pose = last_receiver_pose_; } - tf2::Vector3 emitter_wrt_receiver_pose = irobot_create_ignition_toolbox::utils::object_wrt_frame( + tf2::Vector3 emitter_wrt_receiver_pose = irobot_create_gz_toolbox::utils::object_wrt_frame( emitter_pose, receiver_pose); tf2::Vector3 emitter_wrt_receiver_point = emitter_wrt_receiver_pose + emitter_point; gz::math::Vector2d cartesian_coord = @@ -144,7 +144,7 @@ IrOpcode::ReceiverCartesianPointToEmitterPolarPoint(const tf2::Vector3 & receive } // Pose of receiver relative to the emitter - tf2::Vector3 receiver_wrt_emitter_pose = irobot_create_ignition_toolbox::utils::object_wrt_frame( + tf2::Vector3 receiver_wrt_emitter_pose = irobot_create_gz_toolbox::utils::object_wrt_frame( receiver_pose, emitter_pose); tf2::Vector3 receiver_wrt_emitter_point = receiver_wrt_emitter_pose + receiver_point; gz::math::Vector2d cartesian_coord = diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/mouse.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/mouse.cpp similarity index 93% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/mouse.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/mouse.cpp index e2d24f53..884dd4d6 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/mouse.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/mouse.cpp @@ -6,9 +6,9 @@ #include #include -#include "irobot_create_ignition_toolbox/sensors/mouse.hpp" +#include "irobot_create_gz_toolbox/sensors/mouse.hpp" -using irobot_create_ignition_toolbox::Mouse; +using irobot_create_gz_toolbox::Mouse; Mouse::Mouse(std::shared_ptr & nh) : nh_(nh), diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/sensors_main.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_main.cpp similarity index 60% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/sensors_main.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_main.cpp index f8d5cab0..4f14406d 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/sensors_main.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_main.cpp @@ -5,12 +5,12 @@ #include -#include "irobot_create_ignition_toolbox/sensors/sensors_node.hpp" +#include "irobot_create_gz_toolbox/sensors/sensors_node.hpp" int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/sensors_node.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_node.cpp similarity index 83% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/sensors_node.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_node.cpp index fa70d3ed..c5322049 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/sensors_node.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/sensors_node.cpp @@ -5,9 +5,9 @@ #include -#include "irobot_create_ignition_toolbox/sensors/sensors_node.hpp" +#include "irobot_create_gz_toolbox/sensors/sensors_node.hpp" -using irobot_create_ignition_toolbox::SensorsNode; +using irobot_create_gz_toolbox::SensorsNode; SensorsNode::SensorsNode() : rclcpp::Node("sensors_node") diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/wheel_drop.cpp b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/wheel_drop.cpp similarity index 94% rename from irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/wheel_drop.cpp rename to irobot_create_gz/irobot_create_gz_toolbox/src/sensors/wheel_drop.cpp index 01f52623..a1f63c45 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/wheel_drop.cpp +++ b/irobot_create_gz/irobot_create_gz_toolbox/src/sensors/wheel_drop.cpp @@ -6,9 +6,9 @@ #include #include -#include "irobot_create_ignition_toolbox/sensors/wheel_drop.hpp" +#include "irobot_create_gz_toolbox/sensors/wheel_drop.hpp" -using irobot_create_ignition_toolbox::WheelDrop; +using irobot_create_gz_toolbox::WheelDrop; WheelDrop::WheelDrop(std::shared_ptr & nh) : nh_(nh), diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/README.md b/irobot_create_ignition/irobot_create_ignition_bringup/README.md deleted file mode 100644 index defad810..00000000 --- a/irobot_create_ignition/irobot_create_ignition_bringup/README.md +++ /dev/null @@ -1 +0,0 @@ -# turtlebot4_ignition diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/COLCON_IGNORE b/irobot_create_ignition/irobot_create_ignition_plugins/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/README.md b/irobot_create_ignition/irobot_create_ignition_plugins/README.md deleted file mode 100644 index 4f860643..00000000 --- a/irobot_create_ignition/irobot_create_ignition_plugins/README.md +++ /dev/null @@ -1 +0,0 @@ -# turtlebot4_ignition_gui_plugins diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/sensors_node.hpp b/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/sensors_node.hpp deleted file mode 100644 index e6259323..00000000 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/sensors_node.hpp +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright 2021 Clearpath Robotics, Inc. - * @author Roni Kreinin (rkreinin@clearpathrobotics.com) - */ - -#ifndef IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ -#define IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__SENSORS_NODE_HPP_ - -#include -#include -#include -#include - -#include "irobot_create_ignition_toolbox/sensors/bumper.hpp" -#include "irobot_create_ignition_toolbox/sensors/cliff.hpp" -#include "irobot_create_ignition_toolbox/sensors/ir_intensity.hpp" -#include "irobot_create_ignition_toolbox/sensors/mouse.hpp" -#include "irobot_create_ignition_toolbox/sensors/wheel_drop.hpp" -#include "irobot_create_ignition_toolbox/sensors/ir_opcode.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace irobot_create_ignition_toolbox -{ - -class SensorsNode : public rclcpp::Node -{ -public: - // Constructor and Destructor - SensorsNode(); - -private: - // Node - rclcpp::Node::SharedPtr nh_; - - // Sensors - std::unique_ptr bumper_; - std::unique_ptr cliff_; - std::unique_ptr ir_intensity_; - std::unique_ptr mouse_; - std::unique_ptr wheel_drop_; - std::unique_ptr ir_opcode_; -}; - -} // namespace irobot_create_ignition_toolbox - -#endif // IROBOT_CREATE_IGNITION_TOOLBOX__SENSORS__SENSORS_NODE_HPP_