add tf publish

modified:   src/maintain/scripts/test.py

src/maintain/scripts/test.py

@@ -4,8 +4,9 @@ import numpy as np
 import cv2 as cv
 from matplotlib import pyplot as plt
 import rospy
+import tf2_ros
 from sensor_msgs.msg import Image , CameraInfo
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseStamped, TransformStamped
 import message_filters
 from cv_bridge import CvBridge, CvBridgeError
 import rospkg
@@ -60,6 +61,22 @@ def box_callback(box, depth, color_info):
 
         pose_pub.publish(screw_pose)
 
+        # publish screw tf
+        screw_tf = TransformStamped()
+        screw_tf.header.stamp = rospy.Time.now()
+        screw_tf.header.frame_id = "camera_color_optical_frame"
+        screw_tf.child_frame_id = "screw"
+        screw_tf.transform.translation.x = x
+        screw_tf.transform.translation.y = y
+        screw_tf.transform.translation.z = z
+        screw_tf.transform.rotation.x = normal[0]
+        screw_tf.transform.rotation.y = normal[1]
+        screw_tf.transform.rotation.z = normal[2]
+        screw_tf.transform.rotation.w = 0
+
+        transform_pub.publish(screw_tf)
+        tf_broadcaster.sendTransform(screw_tf)
+
 
 
     except Exception as e:
@@ -77,6 +94,8 @@ if __name__ == "__main__":
     color_info = message_filters.Subscriber("/camera/color/camera_info", CameraInfo)
 
     pose_pub = rospy.Publisher('/screw_pose', PoseStamped, queue_size=10)
+    transform_pub = rospy.Publisher('/tf', TransformStamped, queue_size=10)
+    tf_broadcaster = tf2_ros.TransformBroadcaster()
 
     ts = message_filters.TimeSynchronizer([box_sub, depth_sub, color_info], 1)
     ts.registerCallback(box_callback)