@@ -562,6 +562,9 @@ transport::PublisherPtr performanceMetricsPub;
562
562
563
563
void PublishPerformanceMetrics ()
564
564
{
565
+ if (!performanceMetricsPub || !performanceMetricsPub->HasConnections ())
566
+ return ;
567
+
565
568
physics::WorldPtr world = physics::get_world (" default" );
566
569
567
570
// / Outgoing run-time simulation performance metrics.
@@ -598,11 +601,12 @@ void PublishPerformanceMetrics()
598
601
{
599
602
double updateSimRate =
600
603
(sensor->LastMeasurementTime () - sensorsLastMeasurementTime[name]).Double ();
601
- double updateRealRate =
602
- (world->RealTime () - worldLastMeasurementTime[name]).Double ();
603
604
sensorsLastMeasurementTime[name] = sensor->LastMeasurementTime ();
604
605
if (updateSimRate > 0.0 )
605
606
{
607
+ double updateRealRate =
608
+ (world->RealTime () - worldLastMeasurementTime[name]).Double ();
609
+
606
610
struct sensorPerformanceMetricsType emptySensorPerfomanceMetrics;
607
611
auto ret2 = sensorPerformanceMetrics.insert (
608
612
std::pair<std::string, struct sensorPerformanceMetricsType >
@@ -648,8 +652,7 @@ void PublishPerformanceMetrics()
648
652
}
649
653
650
654
// Publish data
651
- if (performanceMetricsPub && performanceMetricsPub->HasConnections ())
652
- performanceMetricsPub->Publish (performanceMetricsMsg);
655
+ performanceMetricsPub->Publish (performanceMetricsMsg);
653
656
}
654
657
655
658
// ///////////////////////////////////////////////
@@ -709,7 +712,7 @@ void Server::Run()
709
712
710
713
performanceMetricsPub =
711
714
this ->dataPtr ->node ->Advertise <msgs::PerformanceMetrics>(
712
- " ~ /performance_metrics" , 100 , 5 );
715
+ " /gazebo /performance_metrics" , 100 , 5 );
713
716
714
717
GZ_PROFILE_THREAD_NAME (" gzserver" );
715
718
// Stay on this loop until Gazebo needs to be shut down
0 commit comments