Jazzy updates

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/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
 
 install(
   DIRECTORY

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

@@ -82,7 +82,7 @@ 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);
 
   /// \brief Callback for robot odometry
   void robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg);
@@ -91,7 +91,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 +106,10 @@ 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<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 +128,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/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);