Commit c0714417 authored by Ken Tossell's avatar Ken Tossell
Browse files

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

parent 928a5c3e
......@@ -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_) {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment