modified: src/maintain/scripts/maintain.py

modified:   src/yolov5_ros/src/detect.py

.vscode/tasks.json

@@ -0,0 +1,18 @@
+{
+    "tasks": [
+        {
+            "type": "shell",
+            "command": "catkin",
+            "args": [
+                "build",
+                "-DPYTHON_EXECUTABLE=/home/da/miniconda3/envs/gsmini/bin/python",
+                //"-DPYTHON_EXECUTABLE=${HOME}/.conda/envs/gsmini/bin/python"
+            ],
+            "problemMatcher": [
+                "$catkin-gcc"
+            ],
+            "group": "build",
+            "label": "catkin: build"
+        }
+    ]
+}

src/maintain/launch/maintain.launch

@@ -1,4 +1,4 @@
 <launch>
-    <node pkg="maintain" type="test.py" name="maintain" output="screen">
+    <node pkg="maintain" type="maintain.py" name="maintain" output="screen">
     </node>
 </launch>

src/maintain/scripts/maintain.py

@@ -1,4 +1,4 @@
-#! /home/wxchen/.conda/envs/gsmini/bin/python
+#! /home/da/miniconda3/envs/gsmini/bin/python
 
 import rospy
 import numpy as np
@@ -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/maintain/scripts/test.py

@@ -1,4 +1,4 @@
-#! /home/wxchen/.conda/envs/gsmini/bin/python
+#! /home/da/miniconda3/envs/gsmini/bin/python
 
 import numpy as np
 import cv2 as cv
@@ -19,7 +19,7 @@ from rostopic import get_topic_type
 from detection_msgs.msg import BoundingBox, BoundingBoxes
 
 bridge = CvBridge()
-annulus_width = 10
+annulus_width = 20
 
 # 2d to 3d
 def computer_2d_3d(x, y, depth_roi, color_intrinsics):
@@ -37,7 +37,7 @@ def compute_plane_normal(box, depth, color_intrinsics):
     # 计算矩形中心点坐标
     x_center = (box[0] + box[2]) / 2
     y_center = (box[1] + box[3]) / 2
-    z = depth[int(y_center), int(x_center)]
+    z = depth[int(y_center), int(x_center)] / 1000
     x = (x_center - cx) * z / fx
     y = (y_center - cy) * z / fy
     # 计算四个顶点坐标
@@ -50,10 +50,10 @@ def compute_plane_normal(box, depth, color_intrinsics):
     x4 = (box[0] - cx) * z / fx
     y4 = (box[3] - cy) * z / fy
     # 计算矩形边缘向量
-    v1 = np.array([x2 - x1, y2 - y1, depth[int(box[1]), int(box[0])] - z])
+    v1 = np.array([x2 - x1, y2 - y1, depth[int(box[1]), int(box[0])] / 1000 - z])
-    v2 = np.array([x3 - x2, y3 - y2, depth[int(box[1]), int(box[2])] - z])
+    v2 = np.array([x3 - x2, y3 - y2, depth[int(box[1]), int(box[2])] / 1000 - z])
-    v3 = np.array([x4 - x3, y4 - y3, depth[int(box[3]), int(box[2])] - z])
+    v3 = np.array([x4 - x3, y4 - y3, depth[int(box[3]), int(box[2])] / 1000 - z])
-    v4 = np.array([x1 - x4, y1 - y4, depth[int(box[3]), int(box[0])] - z])
+    v4 = np.array([x1 - x4, y1 - y4, depth[int(box[3]), int(box[0])] / 1000 - z])
     # 计算平面法向量
     normal = np.cross(v1, v2)
     normal += np.cross(v2, v3)
@@ -72,6 +72,18 @@ def compute_plane_normal(box, depth, color_intrinsics):
     quaternion = [qx, qy, qz, qw]
     return quaternion  
 
+    # 计算法向量相对于参考向量的旋转角度和旋转轴
+    ref_vector = np.array([0, 0, 1])
+    normal_vector = normal
+    angle = math.acos(np.dot(ref_vector, normal_vector) / (np.linalg.norm(ref_vector) * np.linalg.norm(normal_vector)))
+    axis = np.cross(ref_vector, normal_vector)
+    axis = axis / np.linalg.norm(axis)
+
+    # 将旋转角度和旋转轴转换为四元数
+    qx, qy, qz, qw = tf.transformations.quaternion_about_axis(angle, axis)
+    quaternion = [qx, qy, qz, qw]
+    return quaternion   
+
 def calculate_image_edge_plane_normal(depth_roi):
     # Get the shape of the depth_roi
     height, width = depth_roi.shape
@@ -173,7 +185,7 @@ def box_callback(box, depth, color_info):
             x, y, z = computer_2d_3d(screw_x, screw_y, depth_array, color_intrinsics)
             # rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z)
             # calculate normal direction of screw area
-            box = [boundingBox.ymin - annulus_width, boundingBox.xmin - annulus_width, boundingBox.ymax + annulus_width, boundingBox.xmax + annulus_width]
+            box = [boundingBox.xmin - annulus_width, boundingBox.ymin - annulus_width, boundingBox.xmax + annulus_width, boundingBox.ymax + annulus_width]
             # p1x, p1y, p1z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymin-annulus_width, depth_array, color_intrinsics)
             # p2x, p2y, p2z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymin-annulus_width, depth_array, color_intrinsics)
             # p3x, p3y, p3z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymax+annulus_width, depth_array, color_intrinsics)
@@ -189,7 +201,6 @@ def box_callback(box, depth, color_info):
             # normal = calculate_image_edge_plane_normal(annulus_roi)
             # print(normal)
 
-
             # normal vector to quaternion
             screw_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
             screw_quat[0] = normal_q[0]
@@ -201,6 +212,7 @@ def box_callback(box, depth, color_info):
             screw_euler = tf.transformations.euler_from_quaternion(screw_quat)
             screw_quat_zero_z = tf.transformations.quaternion_from_euler(screw_euler[0], screw_euler[1], 0)
 
+            print(screw_euler)
 
             # Apply low-pass filter to screw quaternion
             alpha = 0.4

src/yolov5_ros/launch/yolov5.launch

@@ -2,7 +2,7 @@
     <!-- Detection configuration -->
     <arg name="weights" default="$(find yolov5_ros)/src/yolov5/best.pt"/>
     <arg name="data" default="$(find yolov5_ros)/src/yolov5/data/mydata.yaml"/>
-    <arg name="confidence_threshold" default="0.75"/>
+    <arg name="confidence_threshold" default="0.70"/>
     <arg name="iou_threshold" default="0.45"/>
     <arg name="maximum_detections" default="1000"/>
     <arg name="device" default="0"/>
@@ -23,7 +23,7 @@
     <arg name="output_topic" default="/yolov5/detections"/>
 
     <!-- Optional topic (publishing annotated image) -->
-    <arg name="publish_image" default="false"/>
+    <arg name="publish_image" default="true"/>
     <arg name="output_image_topic" default="/yolov5/image_out"/>
 
 

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