modified: src/maintain/scripts/test.py

modified:   .gitignore
	modified:   .vscode/tasks.json
	modified:   src/maintain/scripts/test.py
	modified:   src/vision_opencv/cv_bridge/python/cv_bridge/__pycache__/core.cpython-38.pyc

.gitignore

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

.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/scripts/test.py

@@ -1,11 +1,12 @@
-#! /home/wxchen/.conda/envs/gsmini/bin/python
+#! /home/da/miniconda3/envs/gsmini/bin/python
 
 import numpy as np
 import cv2 as cv
 from matplotlib import pyplot as plt
 import rospy
+import tf2_ros
 from sensor_msgs.msg import Image , CameraInfo
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseStamped, TransformStamped
 import message_filters
 from cv_bridge import CvBridge, CvBridgeError
 import rospkg
@@ -60,6 +61,22 @@ def box_callback(box, depth, color_info):
 
         pose_pub.publish(screw_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 = x
+        screw_tf.transform.translation.y = y
+        screw_tf.transform.translation.z = z
+        screw_tf.transform.rotation.x = normal[0]
+        screw_tf.transform.rotation.y = normal[1]
+        screw_tf.transform.rotation.z = normal[2]
+        screw_tf.transform.rotation.w = 0
+
+        transform_pub.publish(screw_tf)
+        tf_broadcaster.sendTransform(screw_tf)
+
 
 
     except Exception as e:
@@ -77,6 +94,8 @@ 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)
     ts.registerCallback(box_callback)