status.cpp

Go to the documentation of this file.

 
#include <multisense_ros/status.h>
 
#include <multisense_ros/DeviceStatus.h>
 
namespace multisense_ros {
 
Status::Status(crl::multisense::Channel* driver):
    driver_(driver),
    device_nh_(""),
    status_pub_(),
    status_timer_(),
    subscribers_(0)
{
    status_pub_ = device_nh_.advertise<multisense_ros::DeviceStatus>("status", 5,
                                                        std::bind(&Status::connect, this),
                                                        std::bind(&Status::disconnect, this));
 
    status_timer_ = device_nh_.createTimer(ros::Duration(1), &Status::queryStatus, this);
}
 
Status::~Status()
{
}
 
void Status::queryStatus(const ros::TimerEvent& event)
{
    (void) event;
 
    if (subscribers_ <= 0)
        return;
 
    if (NULL != driver_)
    {
        crl::multisense::system::StatusMessage statusMessage;
 
        if (crl::multisense::Status_Ok == driver_->getDeviceStatus(statusMessage))
        {
            multisense_ros::DeviceStatus deviceStatus;
 
            deviceStatus.time = ros::Time::now();
            deviceStatus.uptime = ros::Time(statusMessage.uptime);
            deviceStatus.systemOk = statusMessage.systemOk;
            deviceStatus.laserOk = statusMessage.laserOk;
            deviceStatus.laserMotorOk = statusMessage.laserMotorOk;
            deviceStatus.camerasOk = statusMessage.camerasOk;
            deviceStatus.imuOk = statusMessage.imuOk;
            deviceStatus.externalLedsOk = statusMessage.externalLedsOk;
            deviceStatus.processingPipelineOk = statusMessage.processingPipelineOk;
            deviceStatus.powerSupplyTemp = statusMessage.powerSupplyTemperature;
            deviceStatus.fpgaTemp = statusMessage.fpgaTemperature;
            deviceStatus.leftImagerTemp = statusMessage.leftImagerTemperature;
            deviceStatus.rightImagerTemp = statusMessage.rightImagerTemperature;
            deviceStatus.inputVoltage = statusMessage.inputVoltage;
            deviceStatus.inputCurrent = statusMessage.inputCurrent;
            deviceStatus.fpgaPower = statusMessage.fpgaPower;
            deviceStatus.logicPower = statusMessage.logicPower;
            deviceStatus.imagerPower = statusMessage.imagerPower;
 
            status_pub_.publish(deviceStatus);
        }
 
    }
}
 
void Status::connect()
{
    __sync_fetch_and_add(&subscribers_, 1);
}
 
void Status::disconnect()
{
    __sync_fetch_and_sub(&subscribers_, 1);
}
 
 
}