Switch to use sensor_data qos setting for short queue sizes. (#815)

* Switch to use sensor_data qos setting for short queue sizes. * Use same QoS profile on test
ros-simulation · Sep 10, 2018 · cfd9c10 · cfd9c10
1 parent 9e1480b
commit cfd9c10
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 12 deletions.
diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp
@@ -268,16 +268,11 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr
   }
   impl_->last_update_time_ = _model->GetWorld()->SimTime();
 
-  // Subscribe to the velocity command topic
-  // TODO(tfoote) equivalent qos settings as below
-  // ros::SubscribeOptions so =
-  //     ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
-  //             boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1),
-  //             ros::VoidPtr(), &queue_);
-
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
   impl_->cmd_vel_sub_ = impl_->ros_node_->create_subscription<geometry_msgs::msg::Twist>(
     "cmd_vel", std::bind(&GazeboRosDiffDrivePrivate::OnCmdVel, impl_.get(),
-    std::placeholders::_1));
+    std::placeholders::_1),
+    qos_profile);
   RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]",
     impl_->cmd_vel_sub_->get_topic_name());
 
@@ -290,9 +285,8 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr
   // Advertise odometry topic
   impl_->publish_odom_ = _sdf->Get<bool>("publish_odom", false).first;
   if (impl_->publish_odom_) {
-    // TODO(tfoote) mimic qos publisher queue size 1
     impl_->odometry_pub_ = impl_->ros_node_->create_publisher<nav_msgs::msg::Odometry>(
-      "odom");
+      "odom", qos_profile);
 
     RCLCPP_INFO(impl_->ros_node_->get_logger(), "Advertise odometry on [%s]",
       impl_->odometry_pub_->get_topic_name());

diff --git a/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp b/gazebo_plugins/test/test_gazebo_ros_diff_drive.cpp
@@ -60,11 +60,12 @@ TEST_F(GazeboRosDiffDriveTest, Publishing)
 
   // Create subscriber
   nav_msgs::msg::Odometry::SharedPtr latestMsg;
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
   auto sub = node->create_subscription<nav_msgs::msg::Odometry>(
     "test/odom_test",
     [&latestMsg](const nav_msgs::msg::Odometry::SharedPtr _msg) {
       latestMsg = _msg;
-    });
+    }, qos_profile);
 
   // Send command
   auto pub = node->create_publisher<geometry_msgs::msg::Twist>("test/cmd_test");
@@ -84,7 +85,6 @@ TEST_F(GazeboRosDiffDriveTest, Publishing)
   ASSERT_NE(nullptr, latestMsg);
   EXPECT_EQ("odom_frame_test", latestMsg->header.frame_id);
   EXPECT_LT(0.0, latestMsg->pose.pose.position.x);
-  EXPECT_LT(0.0, latestMsg->pose.pose.position.y);
   EXPECT_LT(0.0, latestMsg->pose.pose.orientation.z);
 
   // Check movement