diff --git a/src/maintain/scripts/test.py b/src/maintain/scripts/test.py index 214299a..e5a3ff4 100755 --- a/src/maintain/scripts/test.py +++ b/src/maintain/scripts/test.py @@ -90,9 +90,10 @@ def box_callback(box, depth, color_info): color_intrinsics = color_info.K depth_image = bridge.imgmsg_to_cv2(depth, '16UC1') # get the center of screw - boundingBox = box.bounding_boxes[0] - screw_x = (boundingBox.xmax + boundingBox.xmin) / 2 - screw_y = (boundingBox.ymax + boundingBox.ymin) / 2 + if box.bounding_boxes[0] is not None: + 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) @@ -101,13 +102,24 @@ 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] - # rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z) + rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z) # calculate normal direction of screw area 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) + + # X,Y = np.meshgrid(np.arange(annulus_roi.shape[1]), np.arange(annulus_roi.shape[0])) + # X = X.reshape(-1) + # Y = Y.reshape(-1) + # Z = annulus_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] + # publish screw pose # screw_pose = PoseStamped() # screw_pose.header.stamp = rospy.Time.now() @@ -153,6 +165,7 @@ def box_callback(box, depth, color_info): screw_tf.transform.rotation.z = screw_quat_filtered[2] screw_tf.transform.rotation.w = screw_quat_filtered[3] + tf_broadcaster.sendTransform(screw_tf) diff --git a/src/yolov5_ros/launch/yolov5_d435.launch b/src/yolov5_ros/launch/yolov5_d435.launch deleted file mode 100644 index 0bbb85e..0000000 --- a/src/yolov5_ros/launch/yolov5_d435.launch +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -