[Noetic] Bridge to republish PerformanceMetrics in ROS (#1145)

Signed-off-by: ahcorde <ahcorde@gmail.com> Co-authored-by: Ian Chen <ichen@osrfoundation.org>
ros-simulation · Sep 29, 2020 · 1ab2668 · 1ab2668
1 parent 6aad408
commit 1ab2668
Show file tree

Hide file tree

Showing 5 changed files with 54 additions and 2 deletions.
diff --git a/gazebo_msgs/CMakeLists.txt b/gazebo_msgs/CMakeLists.txt
@@ -21,6 +21,8 @@ add_message_files(
   ModelStates.msg
   ODEJointProperties.msg
   ODEPhysics.msg
+  PerformanceMetrics.msg
+  SensorPerformanceMetric.msg
   WorldState.msg
   )
 

diff --git a/gazebo_msgs/msg/PerformanceMetrics.msg b/gazebo_msgs/msg/PerformanceMetrics.msg
@@ -0,0 +1,4 @@
+Header header
+
+float64 real_time_factor
+gazebo_msgs/SensorPerformanceMetric[] sensors
diff --git a/gazebo_msgs/msg/SensorPerformanceMetric.msg b/gazebo_msgs/msg/SensorPerformanceMetric.msg
@@ -0,0 +1,4 @@
+string name
+float64 sim_update_rate
+float64 real_update_rate
+float64 fps
diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h b/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
@@ -81,6 +81,7 @@
 #include "gazebo_msgs/LinkState.h"
 #include "gazebo_msgs/ModelStates.h"
 #include "gazebo_msgs/LinkStates.h"
+#include "gazebo_msgs/PerformanceMetrics.h"
 
 #include "geometry_msgs/Vector3.h"
 #include "geometry_msgs/Wrench.h"
@@ -292,6 +293,10 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \brief Unused
   void onResponse(ConstResponsePtr &response);
 
+#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+  /// \brief Subscriber callback for performance metrics. This will be send in the ROS network
+  void onPerformanceMetrics(const boost::shared_ptr<gazebo::msgs::PerformanceMetrics const> &msg);
+#endif
   /// \brief utility for checking if string is in URDF format
   bool isURDF(std::string model_xml);
 
@@ -321,6 +326,7 @@ class GazeboRosApiPlugin : public SystemPlugin
   gazebo::transport::PublisherPtr factory_pub_;
   gazebo::transport::PublisherPtr factory_light_pub_;
   gazebo::transport::PublisherPtr light_modify_pub_;
+  gazebo::transport::SubscriberPtr performance_metric_sub_;
   gazebo::transport::PublisherPtr request_pub_;
   gazebo::transport::SubscriberPtr response_sub_;
 
@@ -367,6 +373,7 @@ class GazeboRosApiPlugin : public SystemPlugin
   ros::Subscriber    set_model_state_topic_;
   ros::Publisher     pub_link_states_;
   ros::Publisher     pub_model_states_;
+  ros::Publisher     pub_performance_metrics_;
   int                pub_link_states_connection_count_;
   int                pub_model_states_connection_count_;
 

diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp
@@ -192,7 +192,10 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
   light_modify_pub_ = gazebonode_->Advertise<gazebo::msgs::Light>("~/light/modify");
   request_pub_ = gazebonode_->Advertise<gazebo::msgs::Request>("~/request");
   response_sub_ = gazebonode_->Subscribe("~/response",&GazeboRosApiPlugin::onResponse, this);
-
+#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+  performance_metric_sub_ = gazebonode_->Subscribe("/gazebo/performance_metrics",
+    &GazeboRosApiPlugin::onPerformanceMetrics, this);
+#endif
   // reset topic connection counts
   pub_link_states_connection_count_ = 0;
   pub_model_states_connection_count_ = 0;
@@ -225,6 +228,33 @@ void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response)
 {
 
 }
+#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+void GazeboRosApiPlugin::onPerformanceMetrics(const boost::shared_ptr<gazebo::msgs::PerformanceMetrics const> &msg)
+{
+  gazebo_msgs::PerformanceMetrics msg_ros;
+  msg_ros.header.stamp = ros::Time::now();
+  msg_ros.real_time_factor = msg->real_time_factor();
+  for (auto sensor: msg->sensor())
+  {
+    gazebo_msgs::SensorPerformanceMetric sensor_msgs;
+    sensor_msgs.sim_update_rate = sensor.sim_update_rate();
+    sensor_msgs.real_update_rate = sensor.real_update_rate();
+    sensor_msgs.name = sensor.name();
+
+    if (sensor.has_fps())
+    {
+      sensor_msgs.fps = sensor.fps();
+    }
+    else{
+      sensor_msgs.fps = -1;
+    }
+
+    msg_ros.sensors.push_back(sensor_msgs);
+  }
+
+  pub_performance_metrics_.publish(msg_ros);
+}
+#endif
 
 void GazeboRosApiPlugin::gazeboQueueThread()
 {
@@ -378,6 +408,11 @@ void GazeboRosApiPlugin::advertiseServices()
                                                             ros::VoidPtr(), &gazebo_queue_);
   pub_model_states_ = nh_->advertise(pub_model_states_ao);
 
+#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+  // publish performance metrics
+  pub_performance_metrics_ = nh_->advertise<gazebo_msgs::PerformanceMetrics>("performance_metrics", 10);
+#endif
+
   // Advertise more services on the custom queue
   std::string set_link_properties_service_name("set_link_properties");
   ros::AdvertiseServiceOptions set_link_properties_aso =
@@ -586,7 +621,7 @@ bool GazeboRosApiPlugin::spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
     if (pos1 != std::string::npos && pos2 != std::string::npos)
       model_xml.replace(pos1,pos2-pos1+2,std::string(""));
   }
-  
+
   // Remove comments from URDF
   {
     std::string open_comment("<!--");