From b696a7ca9985e4bedaa59c65187ce205805fe674 Mon Sep 17 00:00:00 2001 From: wxchen Date: Tue, 16 May 2023 10:18:45 +0800 Subject: [PATCH] modified: src/maintain/scripts/maintain.py modified: src/yolov5_ros/src/detect.py --- src/maintain/scripts/maintain.py | 7 +++++-- src/yolov5_ros/src/detect.py | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/maintain/scripts/maintain.py b/src/maintain/scripts/maintain.py index 14d2f0f..ae380c7 100755 --- a/src/maintain/scripts/maintain.py +++ b/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) diff --git a/src/yolov5_ros/src/detect.py b/src/yolov5_ros/src/detect.py index 0b79314..953c3cf 100755 --- a/src/yolov5_ros/src/detect.py +++ b/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