Keep input message header for output

The output message should have the same frame_id and time stamp as the input message. This makes the "frame_id" parameter superfluous, so this commit removes it as well.
kini5gowda · Jun 13, 2019 · ae387b5 · ae387b5
1 parent a89aa71
commit ae387b5
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 15 deletions.
diff --git 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
@@ -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
@@ -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`.