diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp
index af3b79bff74a046f8846a254058831eaf9201e99..10cdd51986d919502607c106d949e7a8c3070994 100644
--- a/libuvc_camera/src/camera_driver.cpp
+++ b/libuvc_camera/src/camera_driver.cpp
@@ -35,6 +35,7 @@
 
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
+#include <std_msgs/Header.h>
 #include <image_transport/camera_publisher.h>
 #include <dynamic_reconfigure/server.h>
 #include <dynamic_reconfigure/SensorLevels.h>
@@ -166,9 +167,16 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) {
   image->data.resize(image->step * image->height);
   memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
 
-  sensor_msgs::CameraInfo::ConstPtr cinfo(
+  sensor_msgs::CameraInfo::Ptr cinfo(
     new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
 
+  std_msgs::Header::Ptr imageheader(new std_msgs::Header());
+  std_msgs::Header::Ptr cinfoheader(new std_msgs::Header());
+  imageheader->frame_id = config_.frame_id;
+  cinfoheader->frame_id = config_.frame_id;
+  image->header = *imageheader;
+  cinfo->header = *cinfoheader;
+
   cam_pub_.publish(image, cinfo);
 
   if (config_changed_) {