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,
                                                        boost::bind(&Status::connect, this),
                                                        boost::bind(&Status::disconnect, this));

    status_timer_ = device_nh_.createTimer(ros::Duration(1), &Status::queryStatus, this);
}

Status::~Status()
{
}

void Status::queryStatus(const ros::TimerEvent& 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);
}


}