pps.cpp
Go to the documentation of this file.
00001 
00034 #include <multisense_ros/pps.h>
00035 #include <std_msgs/Time.h>
00036 
00037 #include <multisense_ros/StampedPps.h>
00038 
00039 using namespace crl::multisense;
00040 
00041 namespace multisense_ros {
00042 
00043 namespace { // anonymous
00044 
00045 //
00046 // Shim for C-style driver callbacks
00047 
00048 void ppsCB(const pps::Header& header, void* userDataP)
00049 { reinterpret_cast<Pps*>(userDataP)->ppsCallback(header); }
00050 
00051 
00052 }; // anonymous
00053 
00054 Pps::Pps(Channel* driver) :
00055     driver_(driver),
00056     device_nh_(""),
00057     pps_pub_(),
00058     stamped_pps_pub_(),
00059     subscribers_(0)
00060 {
00061     system::DeviceInfo deviceInfo;
00062     Status status = driver_->getDeviceInfo(deviceInfo);
00063     if (Status_Ok != status) {
00064         ROS_ERROR("Camera: failed to query device info: %s",
00065                   Channel::statusString(status));
00066         return;
00067     }
00068 
00069     if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {
00070         ROS_INFO("hardware does not support PPS");
00071         return;
00072     }
00073 
00074     system::VersionInfo v;
00075     if (Status_Ok == driver_->getVersionInfo(v) && v.sensorFirmwareVersion < 0x0202)
00076         ROS_ERROR("PPS support requires sensor firmware v2.2 or greater (sensor is running v%d.%d)\n",
00077                   v.sensorFirmwareVersion >> 8, v.sensorFirmwareVersion & 0xFF);
00078     else {
00079 
00080         //
00081         // Only publish PPS if we know firmware 2.2 or greater is running.
00082         //
00083         // 2.1 firmware had a bug where PPS events could (rarely) be published with
00084         // the previous event's timecode.
00085 
00086         pps_pub_ = device_nh_.advertise<std_msgs::Time>("pps", 5,
00087                                                         boost::bind(&Pps::connect, this),
00088                                                         boost::bind(&Pps::disconnect, this));
00089 
00090         stamped_pps_pub_ = device_nh_.advertise<multisense_ros::StampedPps>("stamped_pps", 5,
00091                                                         boost::bind(&Pps::connect, this),
00092                                                         boost::bind(&Pps::disconnect, this));
00093         driver_->addIsolatedCallback(ppsCB, this);
00094     }
00095 }
00096 
00097 Pps::~Pps()
00098 {
00099     driver_->removeIsolatedCallback(ppsCB);
00100 }
00101 
00102 void Pps::ppsCallback(const pps::Header& header)
00103 {
00104     if (subscribers_ <= 0)
00105         return;
00106 
00107     std_msgs::Time pps_msg;
00108 
00109     multisense_ros::StampedPps stamped_pps_msg;
00110 
00111     pps_msg.data = ros::Time(header.sensorTime / 1000000000ll,
00112                              header.sensorTime % 1000000000ll);
00113 
00114 
00115     stamped_pps_msg.data = pps_msg.data;
00116 
00117     stamped_pps_msg.host_time = ros::Time(header.timeSeconds,
00118                                           1000 * header.timeMicroSeconds);
00119 
00120     pps_pub_.publish(pps_msg);
00121 
00122     stamped_pps_pub_.publish(stamped_pps_msg);
00123 }
00124 
00125 void Pps::connect()
00126 {
00127     __sync_fetch_and_add(&subscribers_, 1);
00128 }
00129 
00130 void Pps::disconnect()
00131 {
00132     __sync_fetch_and_sub(&subscribers_, 1);
00133 }
00134 
00135 } // namespace


multisense_ros
Author(s):
autogenerated on Thu Aug 27 2015 14:01:22