pps.cpp

Go to the documentation of this file.

 
#include <multisense_ros/pps.h>
#include <std_msgs/Time.h>
 
#include <multisense_ros/StampedPps.h>
 
using namespace crl::multisense;
 
namespace multisense_ros {
 
namespace { // anonymous
 
//
// Shim for C-style driver callbacks
 
void ppsCB(const pps::Header& header, void* userDataP)
{ reinterpret_cast<Pps*>(userDataP)->ppsCallback(header); }
 
 
} // anonymous
 
Pps::Pps(Channel* driver) :
    driver_(driver),
    device_nh_(""),
    pps_pub_(),
    stamped_pps_pub_(),
    subscribers_(0)
{
    system::DeviceInfo deviceInfo;
    Status status = driver_->getDeviceInfo(deviceInfo);
    if (Status_Ok != status) {
        ROS_ERROR("Camera: failed to query device info: %s",
                  Channel::statusString(status));
        return;
    }
 
    if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {
        ROS_INFO("hardware does not support PPS");
        return;
    }
 
    system::VersionInfo v;
    if (Status_Ok == driver_->getVersionInfo(v) && v.sensorFirmwareVersion < 0x0202)
        ROS_ERROR("PPS support requires sensor firmware v2.2 or greater (sensor is running v%d.%d)\n",
                  v.sensorFirmwareVersion >> 8, v.sensorFirmwareVersion & 0xFF);
    else {
 
        //
        // Only publish PPS if we know firmware 2.2 or greater is running.
        //
        // 2.1 firmware had a bug where PPS events could (rarely) be published with
        // the previous event's timecode.
 
        pps_pub_ = device_nh_.advertise<std_msgs::Time>("pps", 5,
                                                        std::bind(&Pps::connect, this),
                                                        std::bind(&Pps::disconnect, this));
 
        stamped_pps_pub_ = device_nh_.advertise<multisense_ros::StampedPps>("stamped_pps", 5,
                                                        std::bind(&Pps::connect, this),
                                                        std::bind(&Pps::disconnect, this));
        driver_->addIsolatedCallback(ppsCB, this);
    }
}
 
Pps::~Pps()
{
    driver_->removeIsolatedCallback(ppsCB);
}
 
void Pps::ppsCallback(const pps::Header& header)
{
    if (subscribers_ <= 0)
        return;
 
    std_msgs::Time pps_msg;
 
    multisense_ros::StampedPps stamped_pps_msg;
 
    pps_msg.data = ros::Time(header.sensorTime / 1000000000ll,
                             header.sensorTime % 1000000000ll);
 
 
    stamped_pps_msg.data = pps_msg.data;
 
    stamped_pps_msg.host_time = ros::Time(header.timeSeconds,
                                          1000 * header.timeMicroSeconds);
 
    pps_pub_.publish(pps_msg);
 
    stamped_pps_pub_.publish(stamped_pps_msg);
}
 
void Pps::connect()
{
    const int32_t old_sub_count = __sync_fetch_and_add(&subscribers_, 1);
 
    // If we had 0 subscribers before we added 1 (only 1 new subscriber)
    if (old_sub_count == 0)
    {
        Status status = driver_->startStreams(Source_Pps);
        if (Status_Ok != status)
        {
            ROS_ERROR("Pps: failed to start stream: %s", Channel::statusString(status));
        }
    }
}
 
void Pps::disconnect()
{
    const int32_t old_sub_count = __sync_fetch_and_sub(&subscribers_, 1);
 
    // If we only had 1 subscriber before we subtracted 1 (0 subscribers remaining)
    if (old_sub_count == 1)
    {
        Status status = driver_->stopStreams(Source_Pps);
        if (Status_Ok != status)
        {
            ROS_ERROR("Pps: failed to stop stream: %s", Channel::statusString(status));
        }
    }
}
 
} // namespace