Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Backport #1202 to melodic-devel: fix performance metrics #1203

Merged
merged 5 commits into from
Dec 22, 2020
Merged

Backport #1202 to melodic-devel: fix performance metrics #1203

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
1c206aa
Add GAZEBO_ROS_HAS_PERFORMANCE_METRICS macro
scpeters Dec 11, 2020
60bc303
Add doc-string and fix typo in existing code
scpeters Dec 12, 2020
52f2318
Subscribe to gazebo topic on ROS topic connection
scpeters Dec 12, 2020
043e5dd
Use static variable to avoid ABI break
scpeters Dec 16, 2020
15e4771
Revert "Use static variable to avoid ABI break"
scpeters Dec 22, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 21 additions & 5 deletions gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,13 @@

#include <boost/algorithm/string.hpp>

#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
{

Expand Down Expand Up @@ -131,18 +138,26 @@ class GazeboRosApiPlugin : public SystemPlugin
/// \brief advertise services
void advertiseServices();

/// \brief
/// \brief Callback for a subscriber connecting to LinkStates ros topic.
void onLinkStatesConnect();

/// \brief
/// \brief Callback for a subscriber connecting to ModelStates ros topic.
void onModelStatesConnect();

/// \brief
/// \brief Callback for a subscriber disconnecting from LinkStates ros topic.
void onLinkStatesDisconnect();

/// \brief
/// \brief Callback for a subscriber disconnecting from ModelStates ros topic.
void onModelStatesDisconnect();

#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
/// \brief Callback for a subscriber connecting to PerformanceMetrics ros topic.
void onPerformanceMetricsConnect();

/// \brief Callback for a subscriber disconnecting from PerformanceMetrics ros topic.
void onPerformanceMetricsDisconnect();
#endif

/// \brief Function for inserting a URDF into Gazebo from ROS Service Call
bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
gazebo_msgs::SpawnModel::Response &res);
Expand Down Expand Up @@ -293,7 +308,7 @@ 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)
#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
/// \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
Expand Down Expand Up @@ -376,6 +391,7 @@ class GazeboRosApiPlugin : public SystemPlugin
ros::Publisher pub_performance_metrics_;
int pub_link_states_connection_count_;
int pub_model_states_connection_count_;
int pub_performance_metrics_connection_count_;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Adding a new member is not ABI compatible.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do we need to revert that change from noetic-devel too?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I didn't think about it before. Noetic is an interesting case since it is the last ROS 1 release; maybe an exception can be made.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In terms of the impact of an ABI break: because this is a plugin, I think breaking its ABI will only cause a problems for binaries of a plugin that links against GazeboRosApiPlugin. I don't know of any plugins that do this.

In terms of working around the ABI break: because this is a SystemPlugin, I think there will only ever be one instance open at a time. So I think we could safely make this a static variable. I'll check with @iche033 to see if he concurs.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I made it a static variable in 043e5dd, which I think will work. I can port this fix to noetic-devel if merge it here

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We haven't been strict about ABI compatibility in ROS 1, right @j-rivero ?

Especially with this plugin, I don't think many users may be extending it, probably we shouldn't be installing the header... I wouldn't block on the ABI here...

I think there will only ever be one instance open at a time.

Yeah I think that's the usual use case for this plugin, even though I think technically it can be added more than once, but that could cause other issues with duplicate topics and services...

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

so it sounds like we could merge this either way I think

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we're okay with ABI breaks, it might be nice to keep the change the same as the one on noetic-devel for consistency.

I don't mind either way.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok, I'll revert 043e5dd to maintain consistency with noetic


// ROS comm
boost::shared_ptr<ros::AsyncSpinner> async_ros_spin_;
Expand Down
45 changes: 36 additions & 9 deletions gazebo_ros/src/gazebo_ros_api_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() :
stop_(false),
pub_link_states_connection_count_(0),
pub_model_states_connection_count_(0),
pub_performance_metrics_connection_count_(0),
physics_reconfigure_initialized_(false),
pub_clock_frequency_(0),
world_created_(false),
Expand Down Expand Up @@ -198,13 +199,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;
pub_performance_metrics_connection_count_ = 0;

// Manage clock for simulated ros time
pub_clock_ = nh_->advertise<rosgraph_msgs::Clock>("/clock", 10);
Expand Down Expand Up @@ -234,7 +232,7 @@ void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response)
{

}
#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
void GazeboRosApiPlugin::onPerformanceMetrics(const boost::shared_ptr<gazebo::msgs::PerformanceMetrics const> &msg)
{
gazebo_msgs::PerformanceMetrics msg_ros;
Expand Down Expand Up @@ -414,9 +412,15 @@ 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)
#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
// publish performance metrics
pub_performance_metrics_ = nh_->advertise<gazebo_msgs::PerformanceMetrics>("performance_metrics", 10);
ros::AdvertiseOptions pub_performance_metrics_ao =
ros::AdvertiseOptions::create<gazebo_msgs::PerformanceMetrics>(
"performance_metrics",10,
boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsConnect,this),
boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsDisconnect,this),
ros::VoidPtr(), &gazebo_queue_);
pub_performance_metrics_ = nh_->advertise(pub_performance_metrics_ao);
#endif

// Advertise more services on the custom queue
Expand Down Expand Up @@ -577,14 +581,37 @@ void GazeboRosApiPlugin::onModelStatesConnect()
pub_model_states_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishModelStates,this));
}

#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
void GazeboRosApiPlugin::onPerformanceMetricsConnect()
{
pub_performance_metrics_connection_count_++;
if (pub_performance_metrics_connection_count_ == 1) // connect on first subscriber
{
performance_metric_sub_ = gazebonode_->Subscribe("/gazebo/performance_metrics",
&GazeboRosApiPlugin::onPerformanceMetrics, this);
}
}

void GazeboRosApiPlugin::onPerformanceMetricsDisconnect()
{
pub_performance_metrics_connection_count_--;
if (pub_performance_metrics_connection_count_ <= 0) // disconnect with no subscribers
{
performance_metric_sub_.reset();
if (pub_performance_metrics_connection_count_ < 0) // should not be possible
ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_performance_metrics_ in gazebo_ros.cpp? something weird");
}
}
#endif

void GazeboRosApiPlugin::onLinkStatesDisconnect()
{
pub_link_states_connection_count_--;
if (pub_link_states_connection_count_ <= 0) // disconnect with no subscribers
{
pub_link_states_event_.reset();
if (pub_link_states_connection_count_ < 0) // should not be possible
ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
}
}

Expand All @@ -595,7 +622,7 @@ void GazeboRosApiPlugin::onModelStatesDisconnect()
{
pub_model_states_event_.reset();
if (pub_model_states_connection_count_ < 0) // should not be possible
ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
}
}

Expand Down