Jazzy updates (#229)

* Remove the COLCON_IGNORE, fix some namespace issues with the Create3 plugin

* Initial bulk renaming ignition -> gz, ign -> gz

* Rename files & folders ignition -> gz

* Add python setup files

* Copypasta

* Remove setup files (they're no longer necessary after all?), add ament_cmake_python dependency

* Remove substitution deprecations

* Add IfCondition wrappers

* Missing import

* Rename gazebo launch file

* Update launch file

* Update the GUI config with fixed camera position, add 3d scene manager

* Enable the plugins, fix the path envars

* Remove commented-out parameters

* Initial migration from Twist to TwistStamped. Headers are not initalized; just the message type is changed so far

* Override the backup safety for now; it seems to be clobbering the undock behaviour

* Update the gz_ros_bridge to use stamped messages for cmd_vel

* Don't allow the dock/undock actions to be preempted

* Add the InteractiveViewControl so we can move the camera around inside Gazebo

* Reduce the width of wall 12 so the robot's default spawn location doesn't clip into the wall

* Add launch instructions to the readme

* Add note about how to change initial pose

* Fix logging for the buttons plugin

* Revert accidental change to changelog

* Revert changes to CMakeLists

* Set the time in the stamped twist message headers

* Revert get_default_velocity_cmd to have no parameters as it's used externally, set the stamp outside that function (since we may create the command, do some math, and then publish it later). Add an unstamped velocity command input

* Revert additional changelog modifications

* Add dependency on gz_gui_vendor (provides gz-gui8)

* Add dependency on gz_math_vendor (provides gz-math7)

* Update CI for Jazzy

* Make gz-gui8 required in cmake

* Update CI versions to latest

* Linter fixes

.github/workflows/ci.yml

@@ -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/[email protected]
-      - uses: ros-tooling/[email protected].1
+      - uses: ros-tooling/[email protected].8
         with:
-          required-ros-distributions: humble
-      - uses: ros-tooling/[email protected].5
+          required-ros-distributions: jazzy
+      - uses: ros-tooling/[email protected].14
         id: action_ros_ci_step
         with:
-          target-ros2-distro: humble
+          target-ros2-distro: jazzy
           import-token: ${{ secrets.REPO_TOKEN }}

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,36 +56,36 @@ 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
 
 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

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'),

irobot_create_common/irobot_create_common_bringup/package.xml

@@ -10,6 +10,7 @@
   <author>iRobot</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <exec_depend>irobot_create_control</exec_depend>
   <exec_depend>irobot_create_description</exec_depend>

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 <namespace>/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)

irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/behaviors_scheduler.hpp

@@ -9,7 +9,7 @@
 #include <mutex>
 
 #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<geometry_msgs::msg::Twist>;
+  using optional_output_t = boost::optional<geometry_msgs::msg::TwistStamped>;
   using run_behavior_func_t = std::function<optional_output_t(const RobotState &)>;
   using is_done_func_t = std::function<bool ()>;
   using cleanup_func_t = std::function<void ()>;

irobot_create_common/irobot_create_nodes/include/irobot_create_nodes/motion_control/drive_goal_behaviors.hpp

@@ -10,7 +10,7 @@
 #include <memory>
 #include <string>
 
-#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<irobot_create_msgs::action
   float remain_angle_travel_;
   int8_t start_sign_;
   std::atomic<bool> 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<bool> 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<irobot_create_msgs::act
   float remain_angle_travel_;
   int8_t start_sign_;
   std::atomic<bool> 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<bool> 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};

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;
             }
           }

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<irobot_create_msgs::msg::HazardDetectionVector>::SharedPtr
     hazard_detection_sub_;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr teleop_subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr teleop_subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr teleop_unstamped_subscription_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_pose_sub_;
   rclcpp::Subscription<irobot_create_msgs::msg::KidnapStatus>::SharedPtr kidnap_sub_;
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_out_pub_;
   rclcpp::Publisher<irobot_create_msgs::msg::HazardDetection>::SharedPtr backup_limit_hazard_pub_;
   rclcpp::Publisher<irobot_create_msgs::msg::WheelStatus>::SharedPtr wheel_status_pub_;
   rclcpp::TimerBase::SharedPtr control_timer_ {nullptr};
@@ -128,7 +131,7 @@ class MotionControlNode : public rclcpp::Node
   std::shared_ptr<WallFollowBehavior> 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<bool> allow_speed_param_change_ {false};

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.

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);