start visual_servo

.gitattributes

@@ -1 +0,0 @@
-   *.c linguist-language=python

control/draw_cude.py

@@ -0,0 +1,47 @@
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+import numpy as np
+from itertools import product, combinations
+
+
+fig = plt.figure()
+ax = fig.gca(projection='3d')
+ax.set_aspect("equal")
+
+# draw cube
+r = [-1, 1]
+for s, e in combinations(np.array(list(product(r, r, r))), 2):
+    if np.sum(np.abs(s-e)) == r[1]-r[0]:
+        ax.plot(*zip(s, e), color="b")
+
+# draw sphere
+u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
+x = np.cos(u)*np.sin(v)
+y = np.sin(u)*np.sin(v)
+z = np.cos(v)
+ax.plot_wireframe(x, y, z, color="r")
+
+# draw a point
+ax.scatter([0], [0], [0], color="g", s=100)
+
+# draw a vector
+from matplotlib.patches import FancyArrowPatch
+from mpl_toolkits.mplot3d import proj3d
+
+
+class Arrow3D(FancyArrowPatch):
+
+    def __init__(self, xs, ys, zs, *args, **kwargs):
+        FancyArrowPatch.__init__(self, (0, 0), (0, 0), *args, **kwargs)
+        self._verts3d = xs, ys, zs
+
+    def draw(self, renderer):
+        xs3d, ys3d, zs3d = self._verts3d
+        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
+        self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
+        FancyArrowPatch.draw(self, renderer)
+
+a = Arrow3D([0, 1], [0, 1], [0, 1], mutation_scale=20,
+            lw=1, arrowstyle="-|>", color="k")
+ax.add_artist(a)
+plt.show()

control/visual_servo.py

@@ -0,0 +1,84 @@
+import rospy
+from geometry_msgs.msg import Twist, PoseStamped, TransformStamped
+from sensor_msgs.msg import Image, CameraInfo
+from tf2_ros import TransformListener, Buffer
+import sys
+import cv2
+import numpy as np
+from cv_bridge import CvBridge
+bridge = CvBridge()
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+
+class Tracker():
+    def __init__(self, vehicle_type, vehicle_id):
+        self.K = np.eye(3)
+        self.lines_3d = np.array([[[0.05,0,0],[0.05,0.05,0]],[[0.05,0.05,0],[0.05,0.05,0.05]],[[0.05,0.05,0.05],[0.05,0,0.05]],[[0.05,0,0.05],[0.05,0,0]],[[0,0,0],[0,0.05,0]],[[0,0.05,0],[0,0.05,0.05]],[[0,0.05,0.05],[0,0,0.05]],[[0,0,0.05],[0,0,0]],[[0,0,0],[0.05,0,0]],[[0,0.05,0],[0.05,0.05,0]],[[0,0,0.05],[0.05,0,0.05]],[[0,0.05,0.05],[0.05,0.05,0.05]]])
+        self.plot_3d()
+        rospy.init_node(vehicle_type+'_'+vehicle_id+'_visual_servo')
+        rospy.Subscriber(vehicle_type+'_'+vehicle_id+'/le_arm/camera/image_raw', Image, image_callback)
+        rospy.Subscriber(vehicle_type+'_'+vehicle_id+'/le_arm/camera/camera_info', CameraInfo, camera_info_callback)
+        self.image_pub = rospy.Publisher(vehicle_type+'_'+vehicle_id+'/le_arm/visual_servo/image', Image, queue_size=2)
+
+        rate = rospy.Rate(20) 
+        
+        while not rospy.is_shutdown():
+            rate.sleep()
+
+    def plot_3d(self):
+        for line_3d in self.lines_3d:
+            print(zip(line_3d[0],line_3d[1]))
+            ax.plot(*zip(line_3d[0],line_3d[1]),color="b")
+        plt.show()
+
+    def draw_lines(img, lines, color=[255, 0, 0], thickness=3):
+        line_img = np.zeros(
+            (
+                img.shape[0],
+                img.shape[1],
+                3
+            ),
+            dtype=np.uint8
+        )
+        img = np.copy(img)
+        if lines is None:
+            return
+        for line in lines:
+            for x1, y1, x2, y2 in line:
+                cv2.line(line_img, (x1, y1), (x2, y2), color, thickness)
+        img = cv2.addWeighted(img, 0.8, line_img, 1.0, 0.0)
+        return img
+
+    def camera_info_callback(self, msg):
+        self.K = np.array(msg.K).reshape([3,3])
+
+    def image_callback(self, msg):
+        cv_image = bridge.imgmsg_to_cv2(msg, "rgb8")
+        gray_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2GRAY)
+        cannyed_image = cv2.Canny(gray_image, 100, 200)
+        # cv2.imshow("cannyed_image", cannyed_image)
+        # cv2.waitKey(0)
+        # lines = cv2.HoughLinesP(
+        #     cannyed_image,
+        #     rho=6,
+        #     theta=np.pi / 180,
+        #     threshold=50,
+        #     lines=np.array([]),
+        #     minLineLength=5,
+        #     maxLineGap=50
+        # )
+        # if lines is None:
+        #     return
+        # line_image = draw_lines(cv_image,lines,thickness=5)
+
+        img_ros = bridge.cv2_to_imgmsg(cannyed_image)
+        image_pub.publish(img_ros)
+    
+
+if __name__ == '__main__':
+    vehicle_type = sys.argv[1]
+    vehicle_id = sys.argv[2]
+    tracker = Tracker(vehicle_type, vehicle_id)
+

motion_planning/3d/ego_transfer.py

@@ -28,7 +28,7 @@ rospy.Subscriber(vehicle_type+"_"+vehicle_id+"/mavros/vision_pose/pose", PoseSta
 position_pub = rospy.Publisher(vehicle_type+"_"+vehicle_id+"/camera_pose", PoseStamped, queue_size=2)
 rate = rospy.Rate(60) 
 
-while True:
+while not rospy.is_shutdown():
     local_pose.header.stamp = rospy.Time.now()
     position_pub.publish(local_pose) 
     rate.sleep()

sensing/slam/vio/vins_transfer.py

@@ -29,7 +29,7 @@ rospy.Subscriber("/vins_estimator/camera_pose", Odometry, vins_callback)
 position_pub = rospy.Publisher(vehicle_type+"_"+vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=2)
 rate = rospy.Rate(60) 
 
-while True:
+while not rospy.is_shutdown():
     local_pose.header.stamp = rospy.Time.now()
     position_pub.publish(local_pose) 
     rate.sleep()