diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index af3b79bff74a046f8846a254058831eaf9201e99..b92a2f6b03e25618946e417e53020e35e3dcd753 100644 --- 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_) {