wxchen/maintain
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

modified: src/maintain/scripts/test.py

Browse Source
This commit is contained in:
wxchen
2023-05-08 11:11:43 +08:00
parent 231f5fc815
commit bda7453e51
1 changed files with 30 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/maintain/scripts/test.py
View File

@@ -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
@@ -32,18 +33,18 @@ def box_callback(box, depth, color_info):
z = np.mean(depth_roi) * 0.001
x = (screw_x - color_intrinsics[2]) * z / color_intrinsics[0]
y = (screw_y - color_intrinsics[5]) * z / color_intrinsics[4]
print(x,y,z)
# print(x,y,z)
# calculate normal direction of screw area
# X,Y = np.meshgrid(np.arange(depth_roi.shape[1]), np.arange(depth_roi.shape[0]))
# X = X.reshape(-1)
# Y = Y.reshape(-1)
# Z = depth_roi.reshape(-1)
# points = np.vstack([X, Y, Z]).T
# center = np.mean(points, axis=0)
# points = points - center
# U, S, V = np.linalg.svd(points)
# normal = V[2]
X,Y = np.meshgrid(np.arange(depth_roi.shape[1]), np.arange(depth_roi.shape[0]))
X = X.reshape(-1)
Y = Y.reshape(-1)
Z = depth_roi.reshape(-1)
points = np.vstack([X, Y, Z]).T
center = np.mean(points, axis=0)
points = points - center
U, S, V = np.linalg.svd(points)
normal = V[2]
# print(normal)
# publish screw pose
@@ -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)