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,5 +1,6 @@
 .catkin_tools
 .vscode
+.vscode/*
 /build
 /devel
 /logs

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

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

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 = PoseStamped()
-        screw_pose.header.stamp = rospy.Time.now()
+        # screw_pose.header.stamp = rospy.Time.now()
-        screw_pose.header.frame_id = "camera_color_optical_frame"
+        # screw_pose.header.frame_id = "camera_color_optical_frame"
-        screw_pose.pose.position.x = x
+        # screw_pose.pose.position.x = x
-        screw_pose.pose.position.y = y
+        # screw_pose.pose.position.y = y
-        screw_pose.pose.position.z = z
+        # screw_pose.pose.position.z = z
-        screw_pose.pose.orientation.x = 0
+        # screw_pose.pose.orientation.x = 0
-        screw_pose.pose.orientation.y = 0
+        # screw_pose.pose.orientation.y = 0
-        screw_pose.pose.orientation.z = 0
+        # screw_pose.pose.orientation.z = 0
-        screw_pose.pose.orientation.w = 1
+        # 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)

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