pps.cpp
Go to the documentation of this file.
1 
34 #include <multisense_ros/pps.h>
35 #include <std_msgs/Time.h>
36 
37 #include <multisense_ros/StampedPps.h>
38 
39 using namespace crl::multisense;
40 
41 namespace multisense_ros {
42 
43 namespace { // anonymous
44 
45 //
46 // Shim for C-style driver callbacks
47 
48 void ppsCB(const pps::Header& header, void* userDataP)
49 { reinterpret_cast<Pps*>(userDataP)->ppsCallback(header); }
50 
51 
52 }; // anonymous
53 
54 Pps::Pps(Channel* driver) :
55  driver_(driver),
56  device_nh_(""),
57  pps_pub_(),
58  stamped_pps_pub_(),
59  subscribers_(0)
60 {
61  system::DeviceInfo deviceInfo;
62  Status status = driver_->getDeviceInfo(deviceInfo);
63  if (Status_Ok != status) {
64  ROS_ERROR("Camera: failed to query device info: %s",
65  Channel::statusString(status));
66  return;
67  }
68 
69  if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {
70  ROS_INFO("hardware does not support PPS");
71  return;
72  }
73 
75  if (Status_Ok == driver_->getVersionInfo(v) && v.sensorFirmwareVersion < 0x0202)
76  ROS_ERROR("PPS support requires sensor firmware v2.2 or greater (sensor is running v%d.%d)\n",
78  else {
79 
80  //
81  // Only publish PPS if we know firmware 2.2 or greater is running.
82  //
83  // 2.1 firmware had a bug where PPS events could (rarely) be published with
84  // the previous event's timecode.
85 
86  pps_pub_ = device_nh_.advertise<std_msgs::Time>("pps", 5,
87  boost::bind(&Pps::connect, this),
88  boost::bind(&Pps::disconnect, this));
89 
90  stamped_pps_pub_ = device_nh_.advertise<multisense_ros::StampedPps>("stamped_pps", 5,
91  boost::bind(&Pps::connect, this),
92  boost::bind(&Pps::disconnect, this));
93  driver_->addIsolatedCallback(ppsCB, this);
94  }
95 }
96 
98 {
100 }
101 
102 void Pps::ppsCallback(const pps::Header& header)
103 {
104  if (subscribers_ <= 0)
105  return;
106 
107  std_msgs::Time pps_msg;
108 
109  multisense_ros::StampedPps stamped_pps_msg;
110 
111  pps_msg.data = ros::Time(header.sensorTime / 1000000000ll,
112  header.sensorTime % 1000000000ll);
113 
114 
115  stamped_pps_msg.data = pps_msg.data;
116 
117  stamped_pps_msg.host_time = ros::Time(header.timeSeconds,
118  1000 * header.timeMicroSeconds);
119 
120  pps_pub_.publish(pps_msg);
121 
122  stamped_pps_pub_.publish(stamped_pps_msg);
123 }
124 
126 {
127  __sync_fetch_and_add(&subscribers_, 1);
128 }
129 
131 {
132  __sync_fetch_and_sub(&subscribers_, 1);
133 }
134 
135 } // namespace
void publish(const boost::shared_ptr< M > &message) const
void disconnect()
Definition: pps.cpp:130
ros::NodeHandle device_nh_
Definition: pps.h:63
void connect()
Definition: pps.cpp:125
crl::multisense::Channel * driver_
Definition: pps.h:58
int32_t subscribers_
Definition: pps.h:75
virtual Status getVersionInfo(system::VersionInfo &v)=0
virtual Status removeIsolatedCallback(image::Callback callback)=0
virtual Status addIsolatedCallback(image::Callback callback, DataSource imageSourceMask, void *userDataP=NULL)=0
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void ppsCallback(const crl::multisense::pps::Header &header)
Definition: pps.cpp:102
virtual Status getDeviceInfo(system::DeviceInfo &info)=0
ros::Publisher stamped_pps_pub_
Definition: pps.h:70
ros::Publisher pps_pub_
Definition: pps.h:68
#define ROS_ERROR(...)


multisense_ros
Author(s):
autogenerated on Wed Jan 8 2020 03:37:59