From 1ab2668215c2bd582868cd988ce2e579037b188a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 30 Sep 2020 01:52:00 +0200 Subject: [PATCH] [Noetic] Bridge to republish PerformanceMetrics in ROS (#1145) Signed-off-by: ahcorde Co-authored-by: Ian Chen --- gazebo_msgs/CMakeLists.txt | 2 + gazebo_msgs/msg/PerformanceMetrics.msg | 4 ++ gazebo_msgs/msg/SensorPerformanceMetric.msg | 4 ++ .../gazebo_ros/gazebo_ros_api_plugin.h | 7 ++++ gazebo_ros/src/gazebo_ros_api_plugin.cpp | 39 ++++++++++++++++++- 5 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 gazebo_msgs/msg/PerformanceMetrics.msg create mode 100644 gazebo_msgs/msg/SensorPerformanceMetric.msg diff --git a/gazebo_msgs/CMakeLists.txt b/gazebo_msgs/CMakeLists.txt index 673f86bba..8507ddace 100644 --- 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 new file mode 100644 index 000000000..f4c8e919b --- /dev/null +++ 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 new file mode 100644 index 000000000..770fa0708 --- /dev/null +++ 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 index 8512eb589..28b599dcd 100644 --- 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 &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 index cccce6beb..c6007ce06 100644 --- 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("~/light/modify"); request_pub_ = gazebonode_->Advertise("~/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 &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("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("