From 330cc46fc0f62d30e9b2fc63cccd38ed4c3ad1a6 Mon Sep 17 00:00:00 2001
From: Reed Hedges <reed@interreality.org>
Date: Fri, 2 Oct 2015 04:46:11 -0400
Subject: [PATCH] will measure laser data rate (temporary test)
---
LaserPublisher.cpp | 5 +++++
LaserPublisher.h | 5 +++++
2 files changed, 10 insertions(+)
diff --git a/LaserPublisher.cpp b/LaserPublisher.cpp
index 6a9606a..ab6f277 100644
--- a/LaserPublisher.cpp
+++ b/LaserPublisher.cpp
@@ -52,6 +52,8 @@ LaserPublisher::LaserPublisher(ArLaser *_l, ros::NodeHandle& _n, bool _broadcast
laserscan.range_min = 0; //laser->getMinRange() / 1000.0;
laserscan.range_max = laser->getMaxRange() / 1000.0;
pointcloud.header.frame_id = globaltfname;
+
+ readingsCallbackTime = new ArTime;
}
LaserPublisher::~LaserPublisher()
@@ -59,10 +61,12 @@ LaserPublisher::~LaserPublisher()
laser->lockDevice();
laser->remReadingCB(&laserReadingsCB);
laser->unlockDevice();
+ delete readingsCallbackTime;
}
void LaserPublisher::readingsCB()
{
+ printf("readingsCB(): %lu ms since last readingsCB() call.\n", readingsCallbackTime->mSecSince());
assert(laser);
laser->lockDevice();
publishLaserScan();
@@ -70,6 +74,7 @@ void LaserPublisher::readingsCB()
laser->unlockDevice();
if(broadcast_tf)
transform_broadcaster.sendTransform(tf::StampedTransform(lasertf, convertArTimeToROS(laser->getLastReadingTime()), parenttfname, tfname));
+ readingsCallbackTime->setToNow();
}
void LaserPublisher::publishLaserScan()
diff --git a/LaserPublisher.h b/LaserPublisher.h
index fdec9d7..bd9f0fd 100644
--- a/LaserPublisher.h
+++ b/LaserPublisher.h
@@ -6,6 +6,9 @@
#include <sensor_msgs/PointCloud.h>
#include <tf/transform_broadcaster.h>
+class ArLaser;
+class ArTime;
+
class LaserPublisher
{
public:
@@ -28,6 +31,8 @@ protected:
tf::Transform lasertf;
tf::TransformBroadcaster transform_broadcaster;
bool broadcast_tf;
+
+ ArTime *readingsCallbackTime;
};
#endif
--
GitLab