From 227162d7672b0d2d1d2f0822b75d253c4a1d864c Mon Sep 17 00:00:00 2001 From: wxchen Date: Tue, 9 May 2023 16:47:18 +0800 Subject: [PATCH] Add initial conditions modified: src/maintain/scripts/test.py --- src/maintain/scripts/test.py | 106 +++++++++++++++++------------------ 1 file changed, 53 insertions(+), 53 deletions(-) diff --git a/src/maintain/scripts/test.py b/src/maintain/scripts/test.py index 11d4f95..35c94a2 100755 --- a/src/maintain/scripts/test.py +++ b/src/maintain/scripts/test.py @@ -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: