use latest dashing api (#926)

* [gazebo_ros] use qos Signed-off-by: Karsten Knese <[email protected]> * [gazebo_ros] avoid unused warning Signed-off-by: Karsten Knese <[email protected]> * [gazebo_plugins] use qos Signed-off-by: Karsten Knese <[email protected]> * allow_undeclared_parameters * fix tests * forward port pull request #901
ros-simulation · May 22, 2019 · 378d02b · 378d02b
1 parent ae092ff
commit 378d02b
Show file tree

Hide file tree

Showing 26 changed files with 151 additions and 96 deletions.
diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp
@@ -114,20 +114,17 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
   // Camera info publisher
   // TODO(louise) Migrate ImageConnect logic once SubscriberStatusCallback is ported to ROS2
   impl_->camera_info_pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::CameraInfo>(
-    impl_->camera_name_ + "/camera_info");
+    impl_->camera_name_ + "/camera_info", rclcpp::SensorDataQoS());
 
   RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing camera info to [%s]",
     impl_->camera_info_pub_->get_topic_name());
 
   // Trigger
   if (_sdf->Get<bool>("triggered", false).first) {
-    rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
-    qos_profile.depth = 1;
-
     impl_->trigger_sub_ = impl_->ros_node_->create_subscription<std_msgs::msg::Empty>(
       impl_->camera_name_ + "/image_trigger",
-      std::bind(&GazeboRosCamera::OnTrigger, this, std::placeholders::_1),
-      qos_profile);
+      rclcpp::QoS(rclcpp::KeepLast(1)),
+      std::bind(&GazeboRosCamera::OnTrigger, this, std::placeholders::_1));
 
     RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]",
       impl_->trigger_sub_->get_topic_name());

diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp
@@ -275,12 +275,9 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr
   }
   impl_->last_update_time_ = _model->GetWorld()->SimTime();
 
-  rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
-  qos_profile.depth = 1;
   impl_->cmd_vel_sub_ = impl_->ros_node_->create_subscription<geometry_msgs::msg::Twist>(
-    "cmd_vel", std::bind(&GazeboRosDiffDrivePrivate::OnCmdVel, impl_.get(),
-    std::placeholders::_1),
-    qos_profile);
+    "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)),
+    std::bind(&GazeboRosDiffDrivePrivate::OnCmdVel, impl_.get(), std::placeholders::_1));
   RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]",
     impl_->cmd_vel_sub_->get_topic_name());
 
@@ -294,7 +291,7 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr
   impl_->publish_odom_ = _sdf->Get<bool>("publish_odom", false).first;
   if (impl_->publish_odom_) {
     impl_->odometry_pub_ = impl_->ros_node_->create_publisher<nav_msgs::msg::Odometry>(
-      "odom", qos_profile);
+      "odom", rclcpp::QoS(rclcpp::KeepLast(1)));
 
     RCLCPP_INFO(impl_->ros_node_->get_logger(), "Advertise odometry on [%s]",
       impl_->odometry_pub_->get_topic_name());

diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp
@@ -76,8 +76,8 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
   impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
 
   impl_->wrench_sub_ = impl_->ros_node_->create_subscription<geometry_msgs::msg::Wrench>(
-    "gazebo_ros_force", std::bind(&GazeboRosForce::OnRosWrenchMsg, this,
-    std::placeholders::_1));
+    "gazebo_ros_force", rclcpp::SystemDefaultsQoS(),
+    std::bind(&GazeboRosForce::OnRosWrenchMsg, this, std::placeholders::_1));
 
   // Callback on every iteration
   impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(

diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp
@@ -66,7 +66,8 @@ void GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt
     return;
   }
 
-  impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::Imu>("~/out");
+  impl_->pub_ =
+    impl_->ros_node_->create_publisher<sensor_msgs::msg::Imu>("~/out", rclcpp::SensorDataQoS());
 
   // Create message to be reused
   auto msg = std::make_shared<sensor_msgs::msg::Imu>();
@@ -109,7 +110,7 @@ void GazeboRosImuSensorPrivate::OnUpdate()
     sensor_->LinearAcceleration());
 
   // Publish message
-  pub_->publish(msg_);
+  pub_->publish(*msg_);
 }
 
 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosImuSensor)

diff --git a/gazebo_plugins/src/gazebo_ros_p3d.cpp b/gazebo_plugins/src/gazebo_ros_p3d.cpp
@@ -115,7 +115,8 @@ void GazeboRosP3D::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
     return;
   }
 
-  impl_->pub_ = impl_->ros_node_->create_publisher<nav_msgs::msg::Odometry>(impl_->topic_name_);
+  impl_->pub_ = impl_->ros_node_->create_publisher<nav_msgs::msg::Odometry>(
+    impl_->topic_name_, rclcpp::SensorDataQoS());
   impl_->topic_name_ = impl_->pub_->get_topic_name();
   RCLCPP_DEBUG(
     impl_->ros_node_->get_logger(), "Publishing on topic [%s]", impl_->topic_name_.c_str());

diff --git a/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp
@@ -109,17 +109,22 @@ void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt
   if (!_sdf->HasElement("output_type")) {
     RCLCPP_WARN(
       impl_->ros_node_->get_logger(), "missing <output_type>, defaults to sensor_msgs/PointCloud2");
-    impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::PointCloud2>("~/out");
+    impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::PointCloud2>(
+      "~/out", rclcpp::SensorDataQoS());
   } else {
     std::string output_type_string = _sdf->Get<std::string>("output_type");
     if (output_type_string == "sensor_msgs/LaserScan") {
-      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::LaserScan>("~/out");
+      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::LaserScan>(
+        "~/out", rclcpp::SensorDataQoS());
     } else if (output_type_string == "sensor_msgs/PointCloud") {
-      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::PointCloud>("~/out");
+      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::PointCloud>(
+        "~/out", rclcpp::SensorDataQoS());
     } else if (output_type_string == "sensor_msgs/PointCloud2") {
-      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::PointCloud2>("~/out");
+      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::PointCloud2>(
+        "~/out", rclcpp::SensorDataQoS());
     } else if (output_type_string == "sensor_msgs/Range") {
-      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::Range>("~/out");
+      impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::Range>(
+        "~/out", rclcpp::SensorDataQoS());
     } else {
       RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Invalid <output_type> [%s]",
         output_type_string.c_str());

diff --git a/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp b/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp
@@ -41,16 +41,6 @@ TEST_F(GazeboRosDiffDriveTest, Publishing)
   auto vehicle = world->ModelByName("vehicle");
   ASSERT_NE(nullptr, vehicle);
 
-  // Step a bit for model to settle
-  world->Step(100);
-
-  // Check model state
-  EXPECT_NEAR(0.0, vehicle->WorldPose().Pos().X(), tol);
-  EXPECT_NEAR(0.0, vehicle->WorldPose().Pos().Y(), tol);
-  EXPECT_NEAR(0.0, vehicle->WorldPose().Rot().Yaw(), tol);
-  EXPECT_NEAR(0.0, vehicle->WorldLinearVel().X(), tol);
-  EXPECT_NEAR(0.0, vehicle->WorldAngularVel().Z(), tol);
-
   // Create node and executor
   auto node = std::make_shared<rclcpp::Node>("gazebo_ros_diff_drive_test");
   ASSERT_NE(nullptr, node);
@@ -61,24 +51,42 @@ TEST_F(GazeboRosDiffDriveTest, Publishing)
   // Create subscriber
   nav_msgs::msg::Odometry::SharedPtr latestMsg;
   auto sub = node->create_subscription<nav_msgs::msg::Odometry>(
-    "test/odom_test",
+    "test/odom_test", rclcpp::QoS(rclcpp::KeepLast(1)),
     [&latestMsg](const nav_msgs::msg::Odometry::SharedPtr _msg) {
       latestMsg = _msg;
     });
 
+  // Step a bit for model to settle
+  world->Step(100);
+  executor.spin_once(100ms);
+
+  // Check model state
+  EXPECT_NEAR(0.0, vehicle->WorldPose().Pos().X(), tol);
+  EXPECT_NEAR(0.0, vehicle->WorldPose().Pos().Y(), tol);
+  EXPECT_NEAR(0.0, vehicle->WorldPose().Rot().Yaw(), tol);
+  EXPECT_NEAR(0.0, vehicle->WorldLinearVel().X(), tol);
+  EXPECT_NEAR(0.0, vehicle->WorldAngularVel().Z(), tol);
+
   // Send command
-  auto pub = node->create_publisher<geometry_msgs::msg::Twist>("test/cmd_test");
+  auto pub = node->create_publisher<geometry_msgs::msg::Twist>(
+    "test/cmd_test", rclcpp::QoS(rclcpp::KeepLast(1)));
 
   auto msg = geometry_msgs::msg::Twist();
   msg.linear.x = 1.0;
   msg.angular.z = 0.1;
   pub->publish(msg);
-  executor.spin_once(100ms);
 
   // Wait for it to be processed
-  world->Step(1000);
-  executor.spin_once(100ms);
-  gazebo::common::Time::MSleep(1000);
+  int sleep{0};
+  int maxSleep{300};
+  for (; sleep < maxSleep && (vehicle->WorldLinearVel().X() < 0.9 ||
+    vehicle->WorldAngularVel().Z() < 0.09); ++sleep)
+  {
+    world->Step(100);
+    executor.spin_once(100ms);
+    gazebo::common::Time::MSleep(100);
+  }
+  EXPECT_NE(sleep, maxSleep);
 
   // Check message
   ASSERT_NE(nullptr, latestMsg);

diff --git a/gazebo_plugins/test/test_gazebo_ros_force.cpp b/gazebo_plugins/test/test_gazebo_ros_force.cpp
@@ -49,7 +49,18 @@ TEST_F(GazeboRosForceTest, ApplyForceTorque)
   auto node = std::make_shared<rclcpp::Node>("gazebo_ros_force_test");
   ASSERT_NE(nullptr, node);
 
-  auto pub = node->create_publisher<geometry_msgs::msg::Wrench>("test/force_test");
+  auto pub = node->create_publisher<geometry_msgs::msg::Wrench>(
+    "test/force_test", rclcpp::QoS(rclcpp::KeepLast(1)));
+
+  // Wait for subscriber to come up
+  unsigned int sleep = 0;
+  unsigned int max_sleep = 30;
+  while (sleep < max_sleep && pub->get_subscription_count() == 0u) {
+    gazebo::common::Time::MSleep(100);
+    sleep++;
+  }
+  EXPECT_LT(0u, pub->get_subscription_count());
+  EXPECT_LT(sleep, max_sleep);
 
   // Apply force on X
   auto msg = geometry_msgs::msg::Wrench();
@@ -58,9 +69,8 @@ TEST_F(GazeboRosForceTest, ApplyForceTorque)
   pub->publish(msg);
 
   // Wait until box moves
+  sleep = 0;
   double target_dist{0.1};
-  unsigned int sleep = 0;
-  unsigned int max_sleep = 30;
   while (sleep < max_sleep && box->WorldPose().Pos().X() < target_dist) {
     world->Step(100);
     gazebo::common::Time::MSleep(100);

diff --git a/gazebo_plugins/test/test_gazebo_ros_imu_sensor.cpp b/gazebo_plugins/test/test_gazebo_ros_imu_sensor.cpp
@@ -49,14 +49,23 @@ TEST_F(GazeboRosImuSensorTest, ImuMessageCorrect)
 
   sensor_msgs::msg::Imu::SharedPtr msg = nullptr;
   auto sub =
-    node->create_subscription<sensor_msgs::msg::Imu>("/imu/data",
+    node->create_subscription<sensor_msgs::msg::Imu>("/imu/data", rclcpp::SensorDataQoS(),
       [&msg](sensor_msgs::msg::Imu::SharedPtr _msg) {
         msg = _msg;
       });
 
   // Step until an imu message will have been published
-  world->Step(100);
-  rclcpp::spin_some(node);
+  int sleep{0};
+  int max_sleep{30};
+  while (sleep < max_sleep && nullptr == msg) {
+    world->Step(100);
+    rclcpp::spin_some(node);
+    gazebo::common::Time::MSleep(100);
+    sleep++;
+  }
+  EXPECT_LT(0u, sub->get_publisher_count());
+  EXPECT_LT(sleep, max_sleep);
+  ASSERT_NE(nullptr, msg);
 
   // Get the initial imu output when the box is at rest
   auto pre_movement_msg = std::make_shared<sensor_msgs::msg::Imu>(*msg);

diff --git a/gazebo_plugins/test/test_gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/test/test_gazebo_ros_joint_state_publisher.cpp
@@ -58,7 +58,7 @@ TEST_F(GazeboRosJointStatePublisherTest, Publishing)
   // Create subscriber
   sensor_msgs::msg::JointState::SharedPtr latestMsg;
   auto sub = node->create_subscription<sensor_msgs::msg::JointState>(
-    "test/joint_states_test",
+    "test/joint_states_test", rclcpp::SensorDataQoS(),
     [&latestMsg](const sensor_msgs::msg::JointState::SharedPtr msg) {
       latestMsg = msg;
     });

diff --git a/gazebo_plugins/test/test_gazebo_ros_p3d.cpp b/gazebo_plugins/test/test_gazebo_ros_p3d.cpp
@@ -61,7 +61,7 @@ TEST_F(GazeboRosP3dTest, Publishing)
   // Create subscriber
   nav_msgs::msg::Odometry::SharedPtr latest_msg{nullptr};
   auto sub = node->create_subscription<nav_msgs::msg::Odometry>(
-    "test/p3d_test",
+    "test/p3d_test", rclcpp::SensorDataQoS(),
     [&latest_msg](const nav_msgs::msg::Odometry::SharedPtr _msg) {
       latest_msg = _msg;
     });

diff --git a/gazebo_plugins/test/test_gazebo_ros_ray_sensor.cpp b/gazebo_plugins/test/test_gazebo_ros_ray_sensor.cpp
@@ -94,7 +94,8 @@ TEST_F(GazeboRosRaySensorTest, CorrectOutput)
 
   // Convienence function to subscribe to a topic and store it to a variable
   #define SUBSCRIBE_SETTER(msg, topic) \
-  node->create_subscription<decltype(msg)::element_type>(topic, [&msg](decltype(msg) _msg) { \
+  node->create_subscription<decltype(msg)::element_type>(topic, rclcpp::SensorDataQoS(), \
+    [&msg](decltype(msg) _msg) { \
       msg = _msg; \
     })
 

diff --git a/gazebo_ros/include/gazebo_ros/conversions/generic.hpp b/gazebo_ros/include/gazebo_ros/conversions/generic.hpp
@@ -34,7 +34,7 @@ static rclcpp::Logger conversions_logger = rclcpp::get_logger("gazebo_ros_conver
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const ignition::math::Vector3d & in)
+T Convert(const ignition::math::Vector3d &)
 {
   T::ConversionNotImplemented;
 }
@@ -44,7 +44,7 @@ T Convert(const ignition::math::Vector3d & in)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const ignition::math::Quaterniond & in)
+T Convert(const ignition::math::Quaterniond &)
 {
   T::ConversionNotImplemented;
 }
@@ -54,7 +54,7 @@ T Convert(const ignition::math::Quaterniond & in)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const gazebo::common::Time & in)
+T Convert(const gazebo::common::Time &)
 {
   T::ConversionNotImplemented;
 }
@@ -73,7 +73,7 @@ rclcpp::Time Convert(const gazebo::common::Time & in)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const gazebo::msgs::Time & in)
+T Convert(const gazebo::msgs::Time &)
 {
   T::ConversionNotImplemented;
 }

diff --git a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp
@@ -32,7 +32,7 @@ namespace gazebo_ros
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const geometry_msgs::msg::Vector3 & in)
+T Convert(const geometry_msgs::msg::Vector3 &)
 {
   T::ConversionNotImplemented;
 }
@@ -55,7 +55,7 @@ ignition::math::Vector3d Convert(const geometry_msgs::msg::Vector3 & msg)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const geometry_msgs::msg::Point32 & in)
+T Convert(const geometry_msgs::msg::Point32 &)
 {
   T::ConversionNotImplemented;
 }
@@ -155,7 +155,7 @@ geometry_msgs::msg::Quaternion Convert(const ignition::math::Quaterniond & in)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const geometry_msgs::msg::Quaternion & in)
+T Convert(const geometry_msgs::msg::Quaternion &)
 {
   T::ConversionNotImplemented;
 }

diff --git a/gazebo_ros/include/gazebo_ros/conversions/sensor_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/sensor_msgs.hpp
@@ -40,8 +40,9 @@ namespace gazebo_ros
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
-T Convert(const gazebo::msgs::LaserScanStamped & in, double min_intensity = 0.0)
+T Convert(const gazebo::msgs::LaserScanStamped &, double min_intensity = 0.0)
 {
+  (void)min_intensity;
   T::ConversionNotImplemented;
 }
 

diff --git a/gazebo_ros/include/gazebo_ros/node.hpp b/gazebo_ros/include/gazebo_ros/node.hpp
@@ -136,13 +136,11 @@ Node::SharedPtr Node::CreateWithArgs(Args && ... args)
   // TODO(chapulina): use rclcpp::is_initialized() once that's available, see
   // https://github.com/ros2/rclcpp/issues/518
   Node::SharedPtr node;
-  if (rclcpp::is_initialized()) {
-    node = std::make_shared<Node>(std::forward<Args>(args) ...);
-  } else {
+  if (!rclcpp::is_initialized()) {
     rclcpp::init(0, nullptr);
     RCLCPP_INFO(internal_logger(), "ROS was initialized without arguments.");
-    node = std::make_shared<Node>(std::forward<Args>(args) ...);
   }
+  node = std::make_shared<Node>(std::forward<Args>(args) ...);
 
   // Store shared pointer to static executor in this object
   node->executor_ = static_executor_.lock();

diff --git a/gazebo_ros/include/gazebo_ros/testing_utils.hpp b/gazebo_ros/include/gazebo_ros/testing_utils.hpp
@@ -131,7 +131,7 @@ get_message_or_timeout(
   std::atomic_bool msg_received(false);
   typename T::SharedPtr msg = nullptr;
 
-  auto sub = node->create_subscription<T>(topic,
+  auto sub = node->create_subscription<T>(topic, rclcpp::SystemDefaultsQoS(),
       [&msg_received, &msg](typename T::SharedPtr _msg) {
         // If this is the first message from this topic, increment the counter
         if (!msg_received.exchange(true)) {

diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp
@@ -62,11 +62,9 @@ void GazeboRosInit::Load(int argc, char ** argv)
 
   // Offer transient local durability on the clock topic so that if publishing is infrequent (e.g.
   // the simulation is paused), late subscribers can receive the previously published message(s).
-  rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
-  qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
   impl_->clock_pub_ = impl_->ros_node_->create_publisher<rosgraph_msgs::msg::Clock>(
     "/clock",
-    qos_profile);
+    rclcpp::QoS(rclcpp::KeepLast(10)).transient_local());
 
   // Get publish rate from parameter if set
   rclcpp::Parameter rate_param;