Fixed image and cinfo headers' frame + timestamp (from fuerte branch).

ros-drivers · Sep 26, 2013 · c071441 · c071441
1 parent 928a5c3
commit c071441
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp
@@ -146,6 +146,9 @@ void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t lev
 }
 
 void CameraDriver::ImageCallback(uvc_frame_t *frame) {
+  // TODO: Switch to {frame}'s timestamp once that becomes reliable.
+  ros::Time timestamp = ros::Time::now();
+
   boost::recursive_mutex::scoped_lock(mutex_);
 
   assert(state_ == kRunning);
@@ -169,6 +172,11 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) {
   sensor_msgs::CameraInfo::ConstPtr cinfo(
     new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
 
+  image->header.frame_id = config_.frame_id;
+  image->header.stamp = timestamp;
+  cinfo->header.frame_id = config_.frame_id;
+  cinfo->header.stamp = timestamp;
+
   cam_pub_.publish(image, cinfo);
 
   if (config_changed_) {