statistics.cpp

Go to the documentation of this file.

 
#include <multisense_ros/statistics.h>
 
#include <multisense_ros/ChannelStatistics.h>
 
namespace multisense_ros {
 
Statistics::Statistics(crl::multisense::Channel* driver):
    driver_(driver),
    device_nh_(""),
    statistics_pub_(),
    statistics_timer_(),
    subscribers_(0)
{
    statistics_pub_ = device_nh_.advertise<multisense_ros::ChannelStatistics>("channel_statistics", 5,
                                                        std::bind(&Statistics::connect, this),
                                                        std::bind(&Statistics::disconnect, this));
 
    statistics_timer_ = device_nh_.createTimer(ros::Duration(1), &Statistics::queryStatistics, this);
}
 
Statistics::~Statistics()
{
}
 
void Statistics::queryStatistics(const ros::TimerEvent& event)
{
    (void) event;
 
    if (subscribers_ <= 0)
        return;
 
    if (NULL != driver_)
    {
        crl::multisense::system::ChannelStatistics statistics = driver_->getStats();
 
        multisense_ros::ChannelStatistics statisticsMsg;
 
        statisticsMsg.header.stamp = ros::Time::now();
 
        statisticsMsg.num_missed_headers = statistics.numMissedHeaders;
        statisticsMsg.num_dropped_assemblers = statistics.numDroppedAssemblers;
        statisticsMsg.num_image_meta_data = statistics.numImageMetaData;
        statisticsMsg.num_dispatched_image = statistics.numDispatchedImage;
        statisticsMsg.num_dispatched_lidar = statistics.numDispatchedLidar;
        statisticsMsg.num_dispatched_pps = statistics.numDispatchedPps;
        statisticsMsg.num_dispatched_imu = statistics.numDispatchedImu;
        statisticsMsg.num_dispatched_compressed_image = statistics.numDispatchedCompressedImage;
        statisticsMsg.num_dispatched_ground_surface_spline = statistics.numDispatchedGroundSurfaceSpline;
        statisticsMsg.num_dispatched_april_tag_detections = statistics.numDispatchedAprilTagDetections;
 
        statistics_pub_.publish(statisticsMsg);
    }
}
 
void Statistics::connect()
{
    __sync_fetch_and_add(&subscribers_, 1);
}
 
void Statistics::disconnect()
{
    __sync_fetch_and_sub(&subscribers_, 1);
}
 
 
}