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

modified: src/maintain/scripts/test.py

Browse Source 
deleted:    src/yolov5_ros/launch/yolov5_d435.launch
This commit is contained in:
wxchen
2023-05-09 16:49:15 +08:00
parent 80f1ece25a
commit d58cd68b8f
2 changed files with 17 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/maintain/scripts/test.py
View File

@@ -90,9 +90,10 @@ def box_callback(box, depth, color_info):
color_intrinsics = color_info.K color_intrinsics = color_info.K
depth_image = bridge.imgmsg_to_cv2(depth, '16UC1') depth_image = bridge.imgmsg_to_cv2(depth, '16UC1')
# get the center of screw # get the center of screw
boundingBox = box.bounding_boxes[0] if box.bounding_boxes[0] is not None:
screw_x = (boundingBox.xmax + boundingBox.xmin) / 2 boundingBox = box.bounding_boxes[0]
screw_y = (boundingBox.ymax + boundingBox.ymin) / 2 screw_x = (boundingBox.xmax + boundingBox.xmin) / 2
screw_y = (boundingBox.ymax + boundingBox.ymin) / 2
# print(screw_x,screw_y) # print(screw_x,screw_y)
depth_array = np.array(depth_image, dtype=np.float32) depth_array = np.array(depth_image, dtype=np.float32)
@@ -101,13 +102,24 @@ def box_callback(box, depth, color_info):
z = np.mean(depth_roi) * 0.001 z = np.mean(depth_roi) * 0.001
x = (screw_x - color_intrinsics[2]) * z / color_intrinsics[0] x = (screw_x - color_intrinsics[2]) * z / color_intrinsics[0]
y = (screw_y - color_intrinsics[5]) * z / color_intrinsics[4] y = (screw_y - color_intrinsics[5]) * z / color_intrinsics[4]
# rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z) rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z)
# calculate normal direction of screw area # 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] 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) normal = calculate_image_edge_plane_normal(annulus_roi)
# print(normal) # print(normal)
# X,Y = np.meshgrid(np.arange(annulus_roi.shape[1]), np.arange(annulus_roi.shape[0]))
# X = X.reshape(-1)
# Y = Y.reshape(-1)
# Z = annulus_roi.reshape(-1)
# points = np.vstack([X, Y, Z]).T
# center = np.mean(points, axis=0)
# points = points - center
# U, S, V = np.linalg.svd(points)
# normal = V[2]
# publish screw pose # publish screw pose
# screw_pose = PoseStamped() # screw_pose = PoseStamped()
# screw_pose.header.stamp = rospy.Time.now() # screw_pose.header.stamp = rospy.Time.now()
@@ -153,6 +165,7 @@ def box_callback(box, depth, color_info):
screw_tf.transform.rotation.z = screw_quat_filtered[2] screw_tf.transform.rotation.z = screw_quat_filtered[2]
screw_tf.transform.rotation.w = screw_quat_filtered[3] screw_tf.transform.rotation.w = screw_quat_filtered[3]
tf_broadcaster.sendTransform(screw_tf) tf_broadcaster.sendTransform(screw_tf)

56
src/yolov5_ros/launch/yolov5_d435.launch
View File

@@ -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>