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

modified: src/maintain/scripts/maintain.py

Browse Source 
modified:   src/yolov5_ros/src/detect.py
This commit is contained in:
wxchen
2023-05-16 10:18:45 +08:00
parent 4d78759009
commit b696a7ca99
2 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/maintain/scripts/maintain.py
View File

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

2
src/yolov5_ros/src/detect.py
View File

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