Foxy backports

gazebo_dev/colcon.pkg

@@ -0,0 +1,8 @@
+# Configuration file for colcon (https://colcon.readthedocs.io).
+#
+# Please see the doc for the details of the spec:
+#   - https://colcon.readthedocs.io/en/released/user/configuration.html#colcon-pkg-files
+
+{
+  "dependencies": ["Gazebo"],
+}

gazebo_plugins/colcon.pkg

@@ -0,0 +1,8 @@
+# Configuration file for colcon (https://colcon.readthedocs.io).
+#
+# Please see the doc for the details of the spec:
+#   - https://colcon.readthedocs.io/en/released/user/configuration.html#colcon-pkg-files
+
+{
+  "dependencies": ["Gazebo"],
+}

gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.hpp

@@ -52,10 +52,10 @@ class GazeboRosP3DPrivate;
       <update_rate>1</update_rate>
 
       <!-- Translation offset to be added to the pose. -->
-      <xyz_offsets>10 10 10</xyz_offsets>
+      <xyz_offset>10 10 10</xyz_offset>
 
       <!-- Rotation offset to be added to the pose, in Euler angles. -->
-      <rpy_offsets>0.1 0.1 0.1</rpy_offsets>
+      <rpy_offset>0.1 0.1 0.1</rpy_offset>
 
       <!-- Standard deviation of the noise to be added to the reported velocities. -->
       <gaussian_noise>0.01</gaussian_noise>

gazebo_plugins/src/gazebo_ros_p3d.cpp

@@ -129,18 +129,33 @@ void GazeboRosP3D::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
   RCLCPP_DEBUG(
     impl_->ros_node_->get_logger(), "Publishing on topic [%s]", impl_->topic_name_.c_str());
 
-  if (!sdf->HasElement("xyz_offsets")) {
-    RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <xyz_offsets>, defaults to 0s");
-  } else {
+  if (sdf->HasElement("xyz_offsets")) {
+    RCLCPP_WARN(
+      impl_->ros_node_->get_logger(), "<xyz_offsets> is deprecated, use <xyz_offset> instead.");
     impl_->offset_.Pos() = sdf->GetElement("xyz_offsets")->Get<ignition::math::Vector3d>();
   }
-
-  if (!sdf->HasElement("rpy_offsets")) {
-    RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <rpy_offsets>, defaults to 0s");
+  if (!sdf->HasElement("xyz_offset")) {
+    if (!sdf->HasElement("xyz_offsets")) {
+      RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <xyz_offset>, defaults to 0s");
+    }
   } else {
+    impl_->offset_.Pos() = sdf->GetElement("xyz_offset")->Get<ignition::math::Vector3d>();
+  }
+
+  if (sdf->HasElement("rpy_offsets")) {
+    RCLCPP_WARN(
+      impl_->ros_node_->get_logger(), "<rpy_offsets> is deprecated, use <rpy_offset> instead.");
     impl_->offset_.Rot() = ignition::math::Quaterniond(
       sdf->GetElement("rpy_offsets")->Get<ignition::math::Vector3d>());
   }
+  if (!sdf->HasElement("rpy_offset")) {
+    if (!sdf->HasElement("rpy_offsets")) {
+      RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <rpy_offset>, defaults to 0s");
+    }
+  } else {
+    impl_->offset_.Rot() = ignition::math::Quaterniond(
+      sdf->GetElement("rpy_offset")->Get<ignition::math::Vector3d>());
+  }
 
   if (!sdf->HasElement("gaussian_noise")) {
     RCLCPP_DEBUG(impl_->ros_node_->get_logger(), "Missing <gassian_noise>, defaults to 0.0");

gazebo_plugins/test/test_gazebo_ros_p3d.cpp

@@ -100,6 +100,79 @@ TEST_F(GazeboRosP3dTest, Publishing)
   EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.z, tol);
 }
 
+TEST_F(GazeboRosP3dTest, DeprecatedOffsetElements)
+{
+  // Load test world and start paused
+  this->Load("worlds/gazebo_ros_p3d.world", true);
+
+  // World
+  auto world = gazebo::physics::get_world();
+  ASSERT_NE(nullptr, world);
+
+  // Model
+  auto model = world->ModelByName("the_model");
+  ASSERT_NE(nullptr, model);
+
+  // Link
+  auto link = model->GetLink("box_link");
+  ASSERT_NE(nullptr, link);
+
+  // Reference link
+  auto ref_link = model->GetLink("sphere_link");
+  ASSERT_NE(nullptr, ref_link);
+
+  // Step a bit for model to settle
+  world->Step(100);
+
+  // Create node and executor
+  auto node = std::make_shared<rclcpp::Node>("gazebo_ros_p3d_test");
+  ASSERT_NE(nullptr, node);
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+
+  // Create subscriber
+  nav_msgs::msg::Odometry::SharedPtr latest_msg{nullptr};
+  auto sub = node->create_subscription<nav_msgs::msg::Odometry>(
+    "test_deprecated/p3d_test_deprecated", rclcpp::SensorDataQoS(),
+    [&latest_msg](const nav_msgs::msg::Odometry::SharedPtr _msg) {
+      latest_msg = _msg;
+    });
+
+  // Wait for a message
+  world->Step(1000);
+  unsigned int max_sleep{30u};
+  for (auto sleep = 0u; !latest_msg && sleep < max_sleep; ++sleep) {
+    executor.spin_once(100ms);
+    gazebo::common::Time::MSleep(100);
+  }
+
+  // Check message
+  ASSERT_NE(nullptr, latest_msg);
+  EXPECT_EQ("sphere_link", latest_msg->header.frame_id);
+  EXPECT_EQ("box_link", latest_msg->child_frame_id);
+
+  EXPECT_NEAR(
+    latest_msg->pose.pose.position.x,
+    -ref_link->WorldPose().Pos().X() + link->WorldPose().Pos().X() + 5, tol);
+  EXPECT_NEAR(
+    latest_msg->pose.pose.position.y,
+    -ref_link->WorldPose().Pos().Y() + link->WorldPose().Pos().Y() + 5, tol);
+  EXPECT_NEAR(
+    latest_msg->pose.pose.position.z,
+    -ref_link->WorldPose().Pos().Z() + link->WorldPose().Pos().Z() + 5, tol);
+  EXPECT_NEAR(latest_msg->pose.pose.orientation.x, link->WorldPose().Rot().X(), tol);
+  EXPECT_NEAR(latest_msg->pose.pose.orientation.y, link->WorldPose().Rot().Y(), tol);
+  EXPECT_NEAR(latest_msg->pose.pose.orientation.z, link->WorldPose().Rot().Z(), tol);
+
+  EXPECT_NEAR(0.0, latest_msg->twist.twist.linear.x, tol);
+  EXPECT_NEAR(0.0, latest_msg->twist.twist.linear.y, tol);
+  EXPECT_NEAR(0.0, latest_msg->twist.twist.linear.z, tol);
+  EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.x, tol);
+  EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.y, tol);
+  EXPECT_NEAR(0.0, latest_msg->twist.twist.angular.z, tol);
+}
+
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

gazebo_plugins/test/worlds/gazebo_ros_p3d.world

@@ -50,7 +50,19 @@
         <body_name>box_link</body_name>
         <frame_name>sphere_link</frame_name>
         <update_rate>0</update_rate>
-        <xyz_offsets>10 10 10</xyz_offsets>
+        <xyz_offset>10 10 10</xyz_offset>
+        <rpy_offset>0 0 0</rpy_offset>
+        <gaussian_noise>0</gaussian_noise>
+      </plugin>
+      <plugin name="gazebo_ros_p3d_deprecated" filename="libgazebo_ros_p3d.so">
+        <ros>
+          <namespace>/test_deprecated</namespace>
+          <remapping>odom:=p3d_test_deprecated</remapping>
+        </ros>
+        <body_name>box_link</body_name>
+        <frame_name>sphere_link</frame_name>
+        <update_rate>0</update_rate>
+        <xyz_offsets>5 5 5</xyz_offsets>
         <rpy_offsets>0 0 0</rpy_offsets>
         <gaussian_noise>0</gaussian_noise>
       </plugin>

gazebo_plugins/worlds/gazebo_ros_p3d_demo.world

@@ -57,8 +57,8 @@
         <body_name>box_link</body_name>
         <frame_name>sphere_link</frame_name>
         <update_rate>1</update_rate>
-        <xyz_offsets>10 10 10</xyz_offsets>
-        <rpy_offsets>0.1 0.1 0.1</rpy_offsets>
+        <xyz_offset>10 10 10</xyz_offset>
+        <rpy_offset>0.1 0.1 0.1</rpy_offset>
         <gaussian_noise>0.01</gaussian_noise>
       </plugin>
     </model>

gazebo_ros/colcon.pkg

@@ -0,0 +1,8 @@
+# Configuration file for colcon (https://colcon.readthedocs.io).
+#
+# Please see the doc for the details of the spec:
+#   - https://colcon.readthedocs.io/en/released/user/configuration.html#colcon-pkg-files
+
+{
+  "dependencies": ["Gazebo"],
+}

gazebo_ros/src/executor.cpp

@@ -22,15 +22,22 @@ namespace gazebo_ros
 Executor::Executor()
 : spin_thread_(std::bind(&Executor::run, this))
 {
+  using namespace std::chrono_literals;
   sigint_handle_ = gazebo::event::Events::ConnectSigInt(std::bind(&Executor::shutdown, this));
+  while (!this->spinning) {
+    // TODO(ivanpauno): WARN Terrible hack here!!!!
+    // We cannot call rclcpp::shutdown asynchronously, because it generates exceptions that
+    // cannot be caught properly (see https://github.com/ros2/rclcpp/issues/1139).
+    // Executor::cancel() doesn't cause this problem, but it has a race.
+    // Wait until the launched thread starts spinning to avoid the race ...
+    std::this_thread::sleep_for(100ms);
+  }
 }
 
 Executor::~Executor()
 {
   // If ros was not already shutdown by SIGINT handler, do it now
-  if (rclcpp::ok()) {
-    rclcpp::shutdown();
-  }
+  this->shutdown();
   spin_thread_.join();
 }
 
@@ -41,7 +48,9 @@ void Executor::run()
 
 void Executor::shutdown()
 {
-  rclcpp::shutdown();
+  if (this->spinning) {
+    this->cancel();
+  }
 }
 
 }  // namespace gazebo_ros

gazebo_ros/src/gazebo_ros_init.cpp

@@ -33,6 +33,14 @@
 #include <memory>
 #include <string>
 
+#ifndef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+#if \
+  (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || \
+  (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+#define GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+#endif
+#endif  // ifndef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+
 namespace gazebo_ros
 {
 
@@ -78,8 +86,7 @@ class GazeboRosInitPrivate
     std_srvs::srv::Empty::Request::SharedPtr req,
     std_srvs::srv::Empty::Response::SharedPtr res);
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
   /// \brief Subscriber callback for performance metrics. This will be send in the ROS network
   /// \param[in] msg Received PerformanceMetrics message
   void onPerformanceMetrics(ConstPerformanceMetricsPtr & msg);
@@ -157,8 +164,7 @@ void GazeboRosInit::Load(int argc, char ** argv)
     "/clock",
     rclcpp::QoS(rclcpp::KeepLast(10)).transient_local());
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
   impl_->performance_metrics_pub_ =
     impl_->ros_node_->create_publisher<gazebo_msgs::msg::PerformanceMetrics>(
     "performance_metrics", 10);
@@ -178,8 +184,7 @@ void GazeboRosInit::Load(int argc, char ** argv)
     std::bind(&GazeboRosInitPrivate::OnWorldCreated, impl_.get(), std::placeholders::_1));
 }
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
 void GazeboRosInitPrivate::onPerformanceMetrics(
   ConstPerformanceMetricsPtr & msg)
 {
@@ -238,14 +243,10 @@ void GazeboRosInitPrivate::OnWorldCreated(const std::string & _world_name)
       &GazeboRosInitPrivate::OnUnpause, this,
       std::placeholders::_1, std::placeholders::_2));
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
-  // Gazebo transport
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+  // Initialize gazebo transport node
   gz_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
   gz_node_->Init(world_->Name());
-  performance_metric_sub_ = gz_node_->Subscribe(
-    "/gazebo/performance_metrics",
-    &GazeboRosInitPrivate::onPerformanceMetrics, this);
 #endif
 }
 
@@ -263,6 +264,18 @@ void GazeboRosInitPrivate::PublishSimTime(const gazebo::common::UpdateInfo & _in
   rosgraph_msgs::msg::Clock clock;
   clock.clock = gazebo_ros::Convert<builtin_interfaces::msg::Time>(_info.simTime);
   clock_pub_->publish(clock);
+
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+  if (!performance_metric_sub_ && performance_metrics_pub_->get_subscription_count() > 0) {
+    // Subscribe to gazebo performance_metrics topic if there are ros subscribers
+    performance_metric_sub_ = gz_node_->Subscribe(
+      "/gazebo/performance_metrics",
+      &GazeboRosInitPrivate::onPerformanceMetrics, this);
+  } else if (performance_metric_sub_ && performance_metrics_pub_->get_subscription_count() == 0) {
+    // Unsubscribe from gazebo performance_metrics topic if there are no more ros subscribers
+    performance_metric_sub_.reset();
+  }
+#endif
 }
 
 void GazeboRosInitPrivate::OnResetSimulation(

gazebo_ros_control/colcon.pkg

@@ -0,0 +1,8 @@
+# Configuration file for colcon (https://colcon.readthedocs.io).
+#
+# Please see the doc for the details of the spec:
+#   - https://colcon.readthedocs.io/en/released/user/configuration.html#colcon-pkg-files
+
+{
+  "dependencies": ["Gazebo"],
+}