From f9431e75f4a9e2da82e055c36571e509e8f930ac Mon Sep 17 00:00:00 2001 From: wxchen Date: Mon, 8 May 2023 09:56:10 +0800 Subject: [PATCH] add tf publish modified: src/maintain/scripts/test.py --- src/maintain/scripts/test.py | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/maintain/scripts/test.py b/src/maintain/scripts/test.py index 457f63f..c4facfb 100755 --- a/src/maintain/scripts/test.py +++ b/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)