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

Add initial conditions

Browse Source 
modified:   src/maintain/scripts/test.py
This commit is contained in:
wxchen
2023-05-09 16:47:18 +08:00
parent 7045f42ceb
commit 227162d767
1 changed files with 53 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -61,67 +61,67 @@ def box_callback(box, depth, color_info):
depth_image = bridge.imgmsg_to_cv2(depth, '16UC1')
# pc = orh.rospc_to_o3dpc(pc_msg)
# get the center of screw
boundingBox = box.bounding_boxes[0]
screw_x = (boundingBox.xmax + boundingBox.xmin) / 2
screw_y = (boundingBox.ymax + boundingBox.ymin) / 2
# print(screw_x,screw_y)
if box is not None and len(box.bounding_boxes) > 0:
# get the center of screw
boundingBox = box.bounding_boxes[0]
screw_x = (boundingBox.xmax + boundingBox.xmin) / 2
screw_y = (boundingBox.ymax + boundingBox.ymin) / 2
# print(screw_x,screw_y)
depth_array = np.array(depth_image, dtype=np.float32)
depth_roi = depth_array[boundingBox.ymin:boundingBox.ymax, boundingBox.xmin:boundingBox.xmax]
x, y, z = computer_2d_3d(screw_x, screw_y, depth_roi, color_intrinsics)
# rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z)
# calculate normal direction of screw area
p1x, p1y, p1z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymin-annulus_width, depth_roi, color_intrinsics)
p2x, p2y, p2z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymin-annulus_width, depth_roi, color_intrinsics)
p3x, p3y, p3z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymax+annulus_width, depth_roi, color_intrinsics)
p4x, p4y, p4z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymax+annulus_width, depth_roi, color_intrinsics)
p1 = [p1x, p1y, p1z]
p2 = [p2x, p2y, p2z]
p3 = [p3x, p3y, p3z]
p4 = [p4x, p4y, p4z]
normal = compute_normal_vector(p1, p2, p3, p4)
# annulus_roi = depth_array[boundingBox.ymin-annulus_width:boundingBox.ymax+annulus_width, boundingBox.xmin-annulus_width:boundingBox.xmax+annulus_width]
# normal = calculate_image_edge_plane_normal(annulus_roi)
# print(normal)
depth_array = np.array(depth_image, dtype=np.float32)
depth_roi = depth_array[boundingBox.ymin:boundingBox.ymax, boundingBox.xmin:boundingBox.xmax]
# normal vector to quaternion
screw_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
screw_quat[0] = normal[0]
screw_quat[1] = normal[1]
screw_quat[2] = normal[2]
screw_quat[3] = 0
x, y, z = computer_2d_3d(screw_x, screw_y, depth_roi, color_intrinsics)
# rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z)
# calculate normal direction of screw area
p1x, p1y, p1z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymin-annulus_width, depth_roi, color_intrinsics)
p2x, p2y, p2z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymin-annulus_width, depth_roi, color_intrinsics)
p3x, p3y, p3z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymax+annulus_width, depth_roi, color_intrinsics)
p4x, p4y, p4z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymax+annulus_width, depth_roi, color_intrinsics)
p1 = [p1x, p1y, p1z]
p2 = [p2x, p2y, p2z]
p3 = [p3x, p3y, p3z]
p4 = [p4x, p4y, p4z]
normal = compute_normal_vector(p1, p2, p3, p4)
# annulus_roi = depth_array[boundingBox.ymin-annulus_width:boundingBox.ymax+annulus_width, boundingBox.xmin-annulus_width:boundingBox.xmax+annulus_width]
# normal = calculate_image_edge_plane_normal(annulus_roi)
# print(normal)
# quaternion to euler
screw_euler = tf.transformations.euler_from_quaternion(screw_quat)
screw_quat = tf.transformations.quaternion_from_euler(screw_euler[0], screw_euler[1], 0)
# normal vector to quaternion
screw_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
screw_quat[0] = normal[0]
screw_quat[1] = normal[1]
screw_quat[2] = normal[2]
screw_quat[3] = 0
# quaternion to euler
screw_euler = tf.transformations.euler_from_quaternion(screw_quat)
screw_quat = tf.transformations.quaternion_from_euler(screw_euler[0], screw_euler[1], 0)
# Apply low-pass filter to screw quaternion
alpha = 0.4
global screw_quat_prev
screw_quat_filtered = filter_quaternion(screw_quat, screw_quat_prev, alpha)
screw_quat_prev = screw_quat_filtered
# Apply low-pass filter to screw quaternion
alpha = 0.4
global screw_quat_prev
screw_quat_filtered = filter_quaternion(screw_quat, screw_quat_prev, alpha)
screw_quat_prev = screw_quat_filtered
# 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 = screw_quat_filtered[0]
screw_tf.transform.rotation.y = screw_quat_filtered[1]
screw_tf.transform.rotation.z = screw_quat_filtered[2]
screw_tf.transform.rotation.w = screw_quat_filtered[3]
# 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 = screw_quat_filtered[0]
screw_tf.transform.rotation.y = screw_quat_filtered[1]
screw_tf.transform.rotation.z = screw_quat_filtered[2]
screw_tf.transform.rotation.w = screw_quat_filtered[3]
tf_broadcaster.sendTransform(screw_tf)
tf_broadcaster.sendTransform(screw_tf)
except Exception as e: