From 9c6560d17dac211085cc0c95c65a38301862b166 Mon Sep 17 00:00:00 2001
From: wxchen <alex.wxchen@gmail.com>
Date: Mon, 8 May 2023 11:05:20 +0800
Subject: [PATCH] add geometry2 	modified:   .gitignore 	modified:  
 src/detection_msgs/CMakeLists.txt 	new file:   src/geometry2 
 modified:   src/maintain/CMakeLists.txt 	modified:  
 src/maintain/scripts/test.py 	modified:   src/yolov5_ros/CMakeLists.txt

---
 .gitignore                        |  1 +
 src/detection_msgs/CMakeLists.txt |  2 +-
 src/geometry2                     |  1 +
 src/maintain/CMakeLists.txt       |  2 +-
 src/maintain/scripts/test.py      | 26 ++++++++++++--------------
 src/yolov5_ros/CMakeLists.txt     |  2 +-
 6 files changed, 17 insertions(+), 17 deletions(-)
 create mode 160000 src/geometry2

diff --git a/.gitignore b/.gitignore
index 8ccf7d5..a81ad19 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,6 @@
 .catkin_tools
 .vscode
+.vscode/*
 /build
 /devel
 /logs
diff --git a/src/detection_msgs/CMakeLists.txt b/src/detection_msgs/CMakeLists.txt
index 6771ab5..df2a260 100644
--- a/src/detection_msgs/CMakeLists.txt
+++ b/src/detection_msgs/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.0.2...3.26.3)
 project(detection_msgs)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
diff --git a/src/geometry2 b/src/geometry2
new file mode 160000
index 0000000..fe0457f
--- /dev/null
+++ b/src/geometry2
@@ -0,0 +1 @@
+Subproject commit fe0457fe0d761a1dcc9182f9cc09912b3a4a8e2d
diff --git a/src/maintain/CMakeLists.txt b/src/maintain/CMakeLists.txt
index a73c1d2..ffa483b 100644
--- a/src/maintain/CMakeLists.txt
+++ b/src/maintain/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.0.2...3.26.3)
 project(maintain)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
diff --git a/src/maintain/scripts/test.py b/src/maintain/scripts/test.py
index c4facfb..d0ed968 100755
--- a/src/maintain/scripts/test.py
+++ b/src/maintain/scripts/test.py
@@ -33,7 +33,7 @@ def box_callback(box, depth, color_info):
         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]
-        # print(x,y,z)
+        rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z)
         # calculate normal direction of screw area
 
         X,Y = np.meshgrid(np.arange(depth_roi.shape[1]), np.arange(depth_roi.shape[0]))
@@ -48,18 +48,18 @@ def box_callback(box, depth, color_info):
         # print(normal)
 
         # 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
+        # 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
 
-        pose_pub.publish(screw_pose)
+        # pose_pub.publish(screw_pose)
 
         # publish screw tf
         screw_tf = TransformStamped()
@@ -74,7 +74,6 @@ def box_callback(box, depth, color_info):
         screw_tf.transform.rotation.z = normal[2]
         screw_tf.transform.rotation.w = 0
 
-        transform_pub.publish(screw_tf)
         tf_broadcaster.sendTransform(screw_tf)
 
 
@@ -94,7 +93,6 @@ if __name__ == "__main__":
     color_info = message_filters.Subscriber("/camera/color/camera_info", CameraInfo)
 
     pose_pub = rospy.Publisher('/screw_pose', PoseStamped, queue_size=10)
-    transform_pub = rospy.Publisher('/tf', TransformStamped, queue_size=10)
     tf_broadcaster = tf2_ros.TransformBroadcaster()
 
     ts = message_filters.TimeSynchronizer([box_sub, depth_sub, color_info], 1)
diff --git a/src/yolov5_ros/CMakeLists.txt b/src/yolov5_ros/CMakeLists.txt
index df5816b..3802220 100644
--- a/src/yolov5_ros/CMakeLists.txt
+++ b/src/yolov5_ros/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.0.2...3.26.3)
 project(yolov5_ros)
 
 ## Compile as C++11, supported in ROS Kinetic and newer