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_