modified: src/maintain/scripts/maintain.py

modified:   src/yolov5_ros/src/detect.py

src/maintain/scripts/maintain.py

@@ -78,9 +78,10 @@ def main():
                         points.append([x, y, z])
             points = np.array(points)
             
-            if px != 0 and py != 0 and pz != 0:
+            if points.shape[0] > 200 and px != 0 and py != 0 and pz != 0:
 
                 # rospy.loginfo("Screw point: {}".format(screw_point))
+                # rospy.loginfo(points.shape)
 
                 # Fit a plane to the points
                 pcd = o3d.geometry.PointCloud()
@@ -118,12 +119,14 @@ def main():
                 screw_tf.child_frame_id = "screw"
                 screw_tf.transform.translation.x = px
                 screw_tf.transform.translation.y = py
-                screw_tf.transform.translation.z = pz
+                screw_tf.transform.translation.z = -d / np.linalg.norm(normal)
                 screw_tf.transform.rotation.x = quaternion[0]
                 screw_tf.transform.rotation.y = quaternion[1]
                 screw_tf.transform.rotation.z = quaternion[2]
                 screw_tf.transform.rotation.w = quaternion[3]
 
+                rospy.loginfo("tf_broadcaster")
+
                 tf_broadcaster.sendTransform(screw_tf)
 
 

src/yolov5_ros/src/detect.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#! /home/da/miniconda3/envs/gsmini/bin/python
 
 import rospy
 import cv2