status.cpp
Go to the documentation of this file.
00001 
00034 #include <multisense_ros/status.h>
00035 
00036 #include <multisense_ros/DeviceStatus.h>
00037 
00038 namespace multisense_ros {
00039 
00040 Status::Status(crl::multisense::Channel* driver):
00041     driver_(driver),
00042     device_nh_(""),
00043     status_pub_(),
00044     status_timer_(),
00045     subscribers_(0)
00046 {
00047     status_pub_ = device_nh_.advertise<multisense_ros::DeviceStatus>("status", 5,
00048                                                         boost::bind(&Status::connect, this),
00049                                                         boost::bind(&Status::disconnect, this));
00050 
00051     status_timer_ = device_nh_.createTimer(ros::Duration(1), &Status::queryStatus, this);
00052 }
00053 
00054 Status::~Status()
00055 {
00056 }
00057 
00058 void Status::queryStatus(const ros::TimerEvent& event)
00059 {
00060     if (subscribers_ <= 0)
00061         return;
00062 
00063     if (NULL != driver_)
00064     {
00065         crl::multisense::system::StatusMessage statusMessage;
00066 
00067         if (crl::multisense::Status_Ok == driver_->getDeviceStatus(statusMessage))
00068         {
00069             multisense_ros::DeviceStatus deviceStatus;
00070 
00071             deviceStatus.time = ros::Time::now();
00072             deviceStatus.uptime = ros::Time(statusMessage.uptime);
00073             deviceStatus.systemOk = statusMessage.systemOk;
00074             deviceStatus.laserOk = statusMessage.laserOk;
00075             deviceStatus.laserMotorOk = statusMessage.laserMotorOk;
00076             deviceStatus.camerasOk = statusMessage.camerasOk;
00077             deviceStatus.imuOk = statusMessage.imuOk;
00078             deviceStatus.externalLedsOk = statusMessage.externalLedsOk;
00079             deviceStatus.processingPipelineOk = statusMessage.processingPipelineOk;
00080             deviceStatus.powerSupplyTemp = statusMessage.powerSupplyTemperature;
00081             deviceStatus.fpgaTemp = statusMessage.fpgaTemperature;
00082             deviceStatus.leftImagerTemp = statusMessage.leftImagerTemperature;
00083             deviceStatus.rightImagerTemp = statusMessage.rightImagerTemperature;
00084             deviceStatus.inputVoltage = statusMessage.inputVoltage;
00085             deviceStatus.inputCurrent = statusMessage.inputCurrent;
00086             deviceStatus.fpgaPower = statusMessage.fpgaPower;
00087             deviceStatus.logicPower = statusMessage.logicPower;
00088             deviceStatus.imagerPower = statusMessage.imagerPower;
00089 
00090             status_pub_.publish(deviceStatus);
00091         }
00092 
00093     }
00094 }
00095 
00096 void Status::connect()
00097 {
00098     __sync_fetch_and_add(&subscribers_, 1);
00099 }
00100 
00101 void Status::disconnect()
00102 {
00103     __sync_fetch_and_sub(&subscribers_, 1);
00104 }
00105 
00106 
00107 }


multisense_ros
Author(s):
autogenerated on Fri Apr 5 2019 02:28:29