modified: src/maintain/scripts/maintain.py

modified:   src/yolov5_ros/src/detect.py

.gitignore

@@ -1,5 +1,6 @@
 .catkin_tools
 .vscode
+.vscode/
 .vscode/*
 /build
 /devel

.vscode/c_cpp_properties.json

@@ -1,20 +0,0 @@
-{
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "${default}",
-        "limitSymbolsToIncludedHeaders": false
-      },
-      "includePath": [
-        "/opt/ros/melodic/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS",
-      "intelliSenseMode": "gcc-x64",
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "gnu11",
-      "cppStandard": "c++14"
-    }
-  ],
-  "version": 4
-}

.vscode/settings.json

@@ -1,8 +0,0 @@
-{
-    "python.autoComplete.extraPaths": [
-        "/opt/ros/melodic/lib/python2.7/dist-packages"
-    ],
-    "python.analysis.extraPaths": [
-        "/opt/ros/melodic/lib/python2.7/dist-packages"
-    ]
-}

.vscode/tasks.json

@@ -5,8 +5,8 @@
             "command": "catkin",
             "args": [
                 "build",
-                // "-DPYTHON_EXECUTABLE=/home/da/miniconda3/envs/gsmini/bin/python",
-                "-DPYTHON_EXECUTABLE=${HOME}/.conda/envs/gsmini/bin/python"
+                "-DPYTHON_EXECUTABLE=/home/da/miniconda3/envs/gsmini/bin/python",
+                //"-DPYTHON_EXECUTABLE=${HOME}/.conda/envs/gsmini/bin/python"
             ],
             "problemMatcher": [
                 "$catkin-gcc"

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

@@ -0,0 +1,139 @@
+#! /home/da/miniconda3/envs/gsmini/bin/python
+
+import rospy
+import numpy as np
+import open3d as o3d
+from sensor_msgs.msg import Image , CameraInfo, PointCloud2
+from detection_msgs.msg import BoundingBox, BoundingBoxes
+import sensor_msgs.point_cloud2 as pc2
+import cv_bridge
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import tf2_ros
+import tf
+from geometry_msgs.msg import PoseStamped, TransformStamped
+
+bridge = CvBridge()
+color_intrinsics = None
+cloud = None
+box = None
+d_width = 100
+
+def camera_info_callback(msg):
+    global color_intrinsics
+    color_intrinsics = msg
+
+def depth_image_callback(msg):
+    global depth_image
+    depth_image = bridge.imgmsg_to_cv2(msg, '16UC1')
+
+def point_cloud_callback(msg):
+    global cloud
+    cloud = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
+
+def bounding_boxes_callback(msg):
+    global box
+    for bounding_box in msg.bounding_boxes:
+        # Assuming there's only one box, you can add a condition to filter the boxes if needed
+        box = [bounding_box.xmin - d_width, bounding_box.ymin - d_width, bounding_box.xmax + d_width, bounding_box.ymax + d_width]
+
+def main():
+    rospy.init_node("plane_fitting_node")
+
+    rospy.Subscriber("/camera/color/camera_info", CameraInfo, camera_info_callback)
+    rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, depth_image_callback)
+    rospy.Subscriber("/camera/depth/color/points", PointCloud2, point_cloud_callback)
+    rospy.Subscriber("/yolov5/detections", BoundingBoxes, bounding_boxes_callback)
+    tf_broadcaster = tf2_ros.TransformBroadcaster()
+
+    plane_pub = rospy.Publisher("/plane_pose", PoseStamped, queue_size=10)
+
+    rate = rospy.Rate(10) # 10 Hz
+
+    while not rospy.is_shutdown():
+        if color_intrinsics is not None and cloud is not None and box is not None:
+            # Get the 3D points corresponding to the box
+            fx, fy = color_intrinsics.K[0], color_intrinsics.K[4]
+            cx, cy = color_intrinsics.K[2], color_intrinsics.K[5]
+            points = []
+
+            center_x = (box[0] + box[2]) / 2
+            center_y = (box[1] + box[3]) / 2
+
+            depth_array = np.array(depth_image, dtype=np.float32)
+            pz = depth_array[int(center_y), int(center_x)] / 1000.0
+            px = (center_x - color_intrinsics.K[2]) * pz / color_intrinsics.K[0]
+            py = (center_y - color_intrinsics.K[5]) * pz / color_intrinsics.K[4]
+
+            rospy.loginfo("Center point: {}".format([px, py, pz]))
+
+            screw_point = None
+            for x, y, z in cloud:
+                if z != 0:
+                    u = int(np.round((x * fx) / z + cx))
+                    v = int(np.round((y * fy) / z + cy))
+                    if u == center_x and v == center_y:
+                        screw_point = [x, y, z]
+                    if u >= box[0] and u <= box[2] and v >= box[1] and v <= box[3]:
+                        points.append([x, y, z])
+            points = np.array(points)
+            
+            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()
+                pcd.points = o3d.utility.Vector3dVector(points)
+                plane_model, inliers = pcd.segment_plane(distance_threshold=0.02, ransac_n=3, num_iterations=100)
+                [a, b, c, d] = plane_model
+
+                # Calculate the rotation between the plane normal and the Z axis
+                normal = np.array([a, b, c])
+                z_axis = np.array([0, 0, 1])
+                cos_theta = np.dot(normal, z_axis) / (np.linalg.norm(normal) * np.linalg.norm(z_axis))
+                theta = np.arccos(cos_theta)
+                rotation_axis = np.cross(z_axis, normal)
+                rotation_axis = rotation_axis / np.linalg.norm(rotation_axis)
+                quaternion = np.hstack((rotation_axis * np.sin(theta / 2), [np.cos(theta / 2)]))
+                
+
+                # Publish the plane pose
+                # plane_pose = PoseStamped()
+                # plane_pose.header.stamp = rospy.Time.now()
+                # plane_pose.header.frame_id = "camera_color_optical_frame"
+                # plane_pose.pose.position.x = screw_point[0]
+                # plane_pose.pose.position.y = screw_point[1]
+                # plane_pose.pose.position.z = -d / np.linalg.norm(normal)
+                # plane_pose.pose.orientation.x = quaternion[0]
+                # plane_pose.pose.orientation.y = quaternion[1]
+                # plane_pose.pose.orientation.z = quaternion[2]
+                # plane_pose.pose.orientation.w = quaternion[3]
+                # plane_pub.publish(plane_pose)
+
+                # publish screw tf
+                screw_tf = TransformStamped()
+                screw_tf.header.stamp = rospy.Time.now()
+                screw_tf.header.frame_id = "camera_color_optical_frame"
+                screw_tf.child_frame_id = "screw"
+                screw_tf.transform.translation.x = px
+                screw_tf.transform.translation.y = py
+                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)
+
+
+        rate.sleep()
+
+if __name__ == "__main__":
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

src/maintain/scripts/test copy.py

@@ -1,93 +0,0 @@
-#! /home/wxchen/.conda/envs/gsmini/bin/python
-
-import numpy as np
-import cv2 as cv
-from matplotlib import pyplot as plt
-import rospy
-from sensor_msgs.msg import Image
-import message_filters
-from cv_bridge import CvBridge, CvBridgeError
-import rospkg
-
-MIN_MATCH_COUNT = 10
-pkg_path = rospkg.RosPack().get_path('maintain')
-rospy.loginfo(pkg_path)
-img_template = cv.imread(pkg_path + '/scripts/tt.png',0)
-
-def callback(rgb, depth):
-    rospy.loginfo("callback")
-    bridge = CvBridge()
-    # rospy.loginfo(rgb.header.stamp)
-    # rospy.loginfo(depth.header.stamp)
-    try:
-        rgb_image = bridge.imgmsg_to_cv2(rgb, 'bgr8')
-        depth_image = bridge.imgmsg_to_cv2(depth, '16UC1')
-
-        img_matcher = matcher(rgb_image)
-        cv.imshow("img_matcher", img_matcher)
-        cv.waitKey(1000)
-    
-    except CvBridgeError as e:
-        print(e)
-
-def matcher(img):
-
-    try:
-        # Initiate SIFT detector
-        sift = cv.SIFT_create()
-
-        # find the keypoints and descriptors with SIFT
-        kp1, des1 = sift.detectAndCompute(img_template,None)
-        kp2, des2 = sift.detectAndCompute(img,None)
-
-        FLANN_INDEX_KDTREE = 1
-        index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
-        search_params = dict(checks = 50)
-
-        flann = cv.FlannBasedMatcher(index_params, search_params)
-        matches = flann.knnMatch(des1,des2,k=2)
-
-        # store all the good matches as per Lowe's ratio test.
-        good = []
-        for m,n in matches:
-            if m.distance < 0.7*n.distance:
-                good.append(m)
-
-        if len(good)>MIN_MATCH_COUNT:
-            src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
-            dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
-
-            M, mask = cv.findHomography(src_pts, dst_pts, cv.RANSAC,5.0)
-            matchesMask = mask.ravel().tolist()
-
-            h,w = img_template.shape
-            pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
-            dst = cv.perspectiveTransform(pts,M)
-
-            roi = img[np.int32(dst)[0][0][1]:np.int32(dst)[2][0][1], np.int32(dst)[0][0][0]:np.int32(dst)[2][0][0]]
-            # roi = detect_black(roi)
-            
-            # img2 = cv.polylines(img2,[np.int32(dst)],True,255,3, cv.LINE_AA)
-        else:
-            print( "Not enough matches are found - {}/{}".format(len(good), MIN_MATCH_COUNT) )
-
-        return roi
-    except Exception as e:
-        print(e)
-
-
-
-
-if __name__ == "__main__":
-
-    rospy.init_node("maintain")  
-    rospy.loginfo("maintain task start ......") 
-
-    rgb_sub = message_filters.Subscriber("/camera/color/image_raw", Image)
-    depth_sub = message_filters.Subscriber("/camera/aligned_depth_to_color/image_raw", Image)
-
-    ts = message_filters.TimeSynchronizer([rgb_sub, depth_sub], 1)
-    ts.registerCallback(callback)
-
-
-    rospy.spin()

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
@@ -6,19 +6,83 @@ from matplotlib import pyplot as plt
 import rospy
 import tf2_ros
 import tf
-from sensor_msgs.msg import Image , CameraInfo
+from sensor_msgs.msg import Image , CameraInfo, PointCloud2
 from geometry_msgs.msg import PoseStamped, TransformStamped, Quaternion
 import message_filters
 from cv_bridge import CvBridge, CvBridgeError
 import rospkg
+# import open3d as o3d
+# from open3d_ros_helper import open3d_ros_helper as orh
 
-import os
-import sys
+import math
 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):
+    pz = depth_roi[int(y), int(x)] / 1000.0
+    px = (x - color_intrinsics[2]) * pz / color_intrinsics[0]
+    py = (y - color_intrinsics[5]) * pz / color_intrinsics[4]
+    return px, py, pz
+
+def compute_plane_normal(box, depth, color_intrinsics):
+    # 计算相机内参
+    fx = color_intrinsics[0]
+    fy = color_intrinsics[4]
+    cx = color_intrinsics[2]
+    cy = color_intrinsics[5]
+    # 计算矩形中心点坐标
+    x_center = (box[0] + box[2]) / 2
+    y_center = (box[1] + box[3]) / 2
+    z = depth[int(y_center), int(x_center)] / 1000
+    x = (x_center - cx) * z / fx
+    y = (y_center - cy) * z / fy
+    # 计算四个顶点坐标
+    x1 = (box[0] - cx) * z / fx
+    y1 = (box[1] - cy) * z / fy
+    x2 = (box[2] - cx) * z / fx
+    y2 = (box[1] - cy) * z / fy
+    x3 = (box[2] - cx) * z / fx
+    y3 = (box[3] - cy) * z / fy
+    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])] / 1000 - 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])] / 1000 - 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)
+    normal += np.cross(v3, v4)
+    normal += np.cross(v4, v1)
+    normal /= np.linalg.norm(normal)
+    # 计算法向量相对于参考向量的旋转角度和旋转轴
+    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  
+
+    # 计算法向量相对于参考向量的旋转角度和旋转轴
+    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
@@ -73,6 +137,24 @@ def calculate_image_edge_plane_normal(depth_roi):
 
     return normal
 
+# def compute_normal_vector(p1, p2, p3, p4):
+#     # Compute two vectors in the plane
+#     v1 = np.array(p2) - np.array(p1)
+#     v2 = np.array(p3) - np.array(p1)
+#     # Compute the cross product of the two vectors to get the normal vector
+#     n = np.cross(v1, v2)
+#     # Compute the fourth point in the plane
+#     p4 = np.array(p4)
+#     # Check if the fourth point is on the same side of the plane as the origin
+#     if np.dot(n, p4 - np.array(p1)) < 0:
+#         n = -n
+#     # Normalize the normal vector to obtain a unit vector
+#     n = n / np.linalg.norm(n)
+#     theta = math.acos(n[2])
+#     sin_theta_2 = math.sin(theta/2)
+#     quaternion = [math.cos(theta/2), sin_theta_2 * n[0], sin_theta_2 * n[1], sin_theta_2 * n[2]]
+#     return quaternion
+
 def filter_quaternion(quat, quat_prev, alpha):
     if quat_prev is None:
         quat_prev = quat
@@ -82,79 +164,77 @@ def filter_quaternion(quat, quat_prev, alpha):
         quat_filtered[i] = alpha * quat[i] + (1-alpha) * quat_prev[i]
     # Normalize the quaternion
     quat_filtered = quat_filtered / np.linalg.norm(quat_filtered)
-    return quat_filtered     
-
+    return quat_filtered  
 
 def box_callback(box, depth, color_info):
     try:
         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
-        # print(screw_x,screw_y)
+        # pc = orh.rospc_to_o3dpc(pc_msg)
 
-        depth_array = np.array(depth_image, dtype=np.float32)
-        depth_roi = depth_array[boundingBox.ymin:boundingBox.ymax, boundingBox.xmin:boundingBox.xmax]
+        if box is not None and len(box.bounding_boxes) > 0:
+            # get the center of screw 
+            boundingBox = box.bounding_boxes[0]
+            screw_x = (boundingBox.xmax + boundingBox.xmin) / 2
+            screw_y = (boundingBox.ymax + boundingBox.ymin) / 2
+            # print(screw_x,screw_y)
 
-        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)
-        # 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)
+            depth_array = np.array(depth_image, dtype=np.float32)
+            # depth_roi = depth_array[boundingBox.ymin:boundingBox.ymax, boundingBox.xmin:boundingBox.xmax]
 
-        # publish screw pose
-        # screw_pose = PoseStamped()
-        # screw_pose.header.stamp = rospy.Time.now()
-        # screw_pose.header.frame_id = "camera_color_optical_frame"
-        # screw_pose.pose.position.x = x
-        # screw_pose.pose.position.y = y
-        # screw_pose.pose.position.z = z
-        # screw_pose.pose.orientation.x = 0
-        # screw_pose.pose.orientation.y = 0
-        # screw_pose.pose.orientation.z = 0
-        # screw_pose.pose.orientation.w = 1
+            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.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)
+            # p4x, p4y, p4z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymax+annulus_width, depth_array, color_intrinsics)
+            # p1 = [p1x, p1y, p1z]
+            # p2 = [p2x, p2y, p2z]
+            # p3 = [p3x, p3y, p3z]
+            # p4 = [p4x, p4y, p4z]
+            # normal_q = compute_normal_vector(p1, p2, p3, p4)
+            normal_q = compute_plane_normal(box, depth_array, color_intrinsics)
 
-        # pose_pub.publish(screw_pose)
+            # 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)
 
-        # normal vector to quaternion
-        screw_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
-        screw_quat[0] = normal[0]
-        screw_quat[1] = normal[1]
-        screw_quat[2] = normal[2]
-        screw_quat[3] = 0
-        # quaternion to euler
-        screw_euler = tf.transformations.euler_from_quaternion(screw_quat)
-        screw_quat = tf.transformations.quaternion_from_euler(screw_euler[0], screw_euler[1], 0)
+            # normal vector to quaternion
+            screw_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
+            screw_quat[0] = normal_q[0]
+            screw_quat[1] = normal_q[1]
+            screw_quat[2] = normal_q[2]
+            screw_quat[3] = normal_q[3]
+
+            # quaternion to euler
+            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
+            global screw_quat_prev
+            screw_quat_filtered = filter_quaternion(screw_quat, screw_quat_prev, alpha)
+            screw_quat_prev = screw_quat_filtered
 
 
-        # Apply low-pass filter to screw quaternion
-        alpha = 0.4
-        global screw_quat_prev
-        screw_quat_filtered = filter_quaternion(screw_quat, screw_quat_prev, alpha)
-        screw_quat_prev = screw_quat_filtered
-
-
-        # publish screw tf
-        screw_tf = TransformStamped()
-        screw_tf.header.stamp = rospy.Time.now()
-        screw_tf.header.frame_id = "camera_color_optical_frame"
-        screw_tf.child_frame_id = "screw"
-        screw_tf.transform.translation.x = x
-        screw_tf.transform.translation.y = y
-        screw_tf.transform.translation.z = z
-        screw_tf.transform.rotation.x = screw_quat_filtered[0]
-        screw_tf.transform.rotation.y = screw_quat_filtered[1]
-        screw_tf.transform.rotation.z = screw_quat_filtered[2]
-        screw_tf.transform.rotation.w = screw_quat_filtered[3]
-
-        tf_broadcaster.sendTransform(screw_tf)
+            # publish screw tf
+            screw_tf = TransformStamped()
+            screw_tf.header.stamp = rospy.Time.now()
+            screw_tf.header.frame_id = "camera_color_optical_frame"
+            screw_tf.child_frame_id = "screw"
+            screw_tf.transform.translation.x = x
+            screw_tf.transform.translation.y = y
+            screw_tf.transform.translation.z = z
+            screw_tf.transform.rotation.x = screw_quat_filtered[0]
+            screw_tf.transform.rotation.y = screw_quat_filtered[1]
+            screw_tf.transform.rotation.z = screw_quat_filtered[2]
+            screw_tf.transform.rotation.w = screw_quat_filtered[3]
 
+            tf_broadcaster.sendTransform(screw_tf)
 
 
     except Exception as e:
@@ -172,6 +252,7 @@ if __name__ == "__main__":
     box_sub = message_filters.Subscriber("/yolov5/detections", BoundingBoxes)
     depth_sub = message_filters.Subscriber("/camera/aligned_depth_to_color/image_raw", Image)
     color_info = message_filters.Subscriber("/camera/color/camera_info", CameraInfo)
+    # pc_sub = message_filters.Subscriber("/camera/depth/color/points", PointCloud2)
 
     tf_broadcaster = tf2_ros.TransformBroadcaster()
 

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"/>
 
 
@@ -50,7 +50,8 @@
         <param name="publish_image" value="$(arg publish_image)"/>
         <param name="output_image_topic" value="$(arg output_image_topic)"/>
     </node>
-    <!-- <include file="$(find camera_launch)/launch/d435.launch"/> -->
+    <include file="$(find realsense2_camera)/launch/my_camera.launch" >
+    </include>
 
 
 </launch>

src/yolov5_ros/launch/yolov5_d435.launch

@@ -1,56 +0,0 @@
-<launch>
-    <!-- Detection configuration -->
-    <arg name="weights" default="$(find yolov5_ros)/best.pt"/>
-    <arg name="data" default="$(find yolov5_ros)/src/yolov5/data/coco128.yaml"/>
-    <arg name="confidence_threshold" default="0.5"/>
-    <arg name="iou_threshold" default="0.45"/>
-    <arg name="maximum_detections" default="1000"/>
-    <arg name="device" default="0"/>
-    <arg name="agnostic_nms" default="true"/>
-    <arg name="line_thickness" default="3"/>
-    <arg name="dnn" default="true"/>
-    <arg name="half" default="false"/>
-
-    <!-- replace imgsz -->
-    <arg name="inference_size_h" default="640"/>
-    <arg name="inference_size_w" default="640"/>
-
-    <!-- Visualize using OpenCV window -->
-    <arg name="view_image" default="true"/>
-
-    <!-- ROS topics -->
-    <arg name="input_image_topic" default="/clover0/main_camera/image_raw"/>
-    <arg name="output_topic" default="/yolov5/detections"/>
-
-    <!-- Optional topic (publishing annotated image) -->
-    <arg name="publish_image" default="false"/>
-    <arg name="output_image_topic" default="/yolov5/image_out"/>
-
-
-    <node pkg="yolov5_ros" name="detect" type="detect.py" output="screen">
-        <param name="weights" value="$(arg weights)"/>
-        <param name="data" value="$(arg data)"/>
-        <param name="confidence_threshold" value="$(arg confidence_threshold)"/>
-        <param name="iou_threshold" value="$(arg iou_threshold)" />
-        <param name="maximum_detections" value="$(arg maximum_detections)"/>
-        <param name="device" value="$(arg device)" />
-        <param name="agnostic_nms" value="$(arg agnostic_nms)" />
-        <param name="line_thickness" value="$(arg line_thickness)"/>
-        <param name="dnn" value="$(arg dnn)"/>
-        <param name="half" value="$(arg half)"/>
-
-        <param name="inference_size_h" value="$(arg inference_size_h)"/>
-        <param name="inference_size_w" value="$(arg inference_size_w)"/>
-
-        <param name="input_image_topic" value="$(arg input_image_topic)"/>
-        <param name="output_topic" value="$(arg output_topic)"/>
-
-        <param name="view_image" value="$(arg view_image)"/>
-
-        <param name="publish_image" value="$(arg publish_image)"/>
-        <param name="output_image_topic" value="$(arg output_image_topic)"/>
-    </node>
-    <include file="$(find camera_launch)/launch/d435.launch"/>
-
-
-</launch>

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