diff --git a/config/config_pose.yaml b/config/config_pose.yaml
index a761506a..e98b85de 100644
--- a/config/config_pose.yaml
+++ b/config/config_pose.yaml
@@ -1,6 +1,5 @@
 topic_camera: "/dope/webcam_rgb_raw"
 topic_publishing: "dope"
-frame_id: "/dope"
 
 # Comment any of these lines to prevent detection / pose estimation of that object
 weights: {
diff --git a/nodes/dope b/nodes/dope
index 43141837..ffe7eb4b 100755
--- a/nodes/dope
+++ b/nodes/dope
@@ -173,9 +173,9 @@ class DopeNode(object):
         print("Running DOPE...  (Listening to camera topic: '{}')".format(topic_cam))
         print("Ctrl-C to stop")
 
-    def image_callback(self, msg):
+    def image_callback(self, image_msg):
         """Image callback"""
-        img = self.cv_bridge.imgmsg_to_cv2(msg, "rgb8")
+        img = self.cv_bridge.imgmsg_to_cv2(image_msg, "rgb8")
         # cv2.imwrite('img.png', cv2.cvtColor(img, cv2.COLOR_BGR2RGB))  # for debugging
 
         # Copy and draw image
@@ -198,20 +198,19 @@ class DopeNode(object):
                     continue
                 loc = result["location"]
                 ori = result["quaternion"]
-                msg = PoseStamped()
-                msg.header.frame_id = self.params["frame_id"]
-                msg.header.stamp = rospy.Time.now()
+                pose_msg = PoseStamped()
+                pose_msg.header = image_msg.header
                 CONVERT_SCALE_CM_TO_METERS = 100
-                msg.pose.position.x = loc[0] / CONVERT_SCALE_CM_TO_METERS
-                msg.pose.position.y = loc[1] / CONVERT_SCALE_CM_TO_METERS
-                msg.pose.position.z = loc[2] / CONVERT_SCALE_CM_TO_METERS
-                msg.pose.orientation.x = ori[0]
-                msg.pose.orientation.y = ori[1]
-                msg.pose.orientation.z = ori[2]
-                msg.pose.orientation.w = ori[3]
+                pose_msg.pose.position.x = loc[0] / CONVERT_SCALE_CM_TO_METERS
+                pose_msg.pose.position.y = loc[1] / CONVERT_SCALE_CM_TO_METERS
+                pose_msg.pose.position.z = loc[2] / CONVERT_SCALE_CM_TO_METERS
+                pose_msg.pose.orientation.x = ori[0]
+                pose_msg.pose.orientation.y = ori[1]
+                pose_msg.pose.orientation.z = ori[2]
+                pose_msg.pose.orientation.w = ori[3]
 
                 # Publish
-                self.pubs[m].publish(msg)
+                self.pubs[m].publish(pose_msg)
                 self.pub_dimension[m].publish(str(self.params['dimensions'][m]))
 
                 # Draw the cube
diff --git a/readme.md b/readme.md
index fb545bb6..d98fbf7f 100644
--- a/readme.md
+++ b/readme.md
@@ -94,7 +94,7 @@ This is the official DOPE ROS package for detection and 6-DoF pose estimation of
 
 * To debug in RViz, `rosrun rviz rviz`, then either
   * `Add > Image` to view the raw RGB image or the image with cuboids overlaid
-  * `Add > Pose` to view the object coordinate frame in 3D.  If you do not have a coordinate frame set up, you can run this static transformation: `rosrun tf static_transform_publisher 0 0 0 0.7071 0 0 -0.7071 world dope 10`.  Make sure that in RViz's `Global Options`, the `Fixed Frame` is set to `world`. 
+  * `Add > Pose` to view the object coordinate frame in 3D.  If you do not have a coordinate frame set up, you can run this static transformation: `rosrun tf2_ros static_transform_publisher 0 0 0 0.7071 0 0 -0.7071 world <camera_frame_id>`, where `<camera_frame_id>` is the `frame_id` of your input camera messages.  Make sure that in RViz's `Global Options`, the `Fixed Frame` is set to `world`.
 
 * If `rosrun` does not find the package (`[rospack] Error: package 'dope' not found`), be sure that you called `source devel/setup.bash` as mentioned above.  To find the package, run `rospack find dope`.