add aikit_v2 img ros

mycobot_ai/aikit_280_pi/scripts/aikit_encode.py

@@ -5,7 +5,9 @@ from pymycobot.mycobot import MyCobot
 from pymycobot import PI_BAUD, PI_PORT
 import RPi.GPIO as GPIO
 import time
-
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
 
 
 # y轴偏移量
@@ -41,6 +43,34 @@ class Detect_marker():
         self.dist_coeffs = np.array(([[3.41360787e-01, -2.52114260e+00, -1.28012469e-03,  6.70503562e-03,
              2.57018000e+00]]))
     
+        # init a node and a publisher
+        rospy.init_node("encode_marker", anonymous=True)
+        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+
+        self.marker = Marker()
+        self.marker.header.frame_id = "joint1"
+        self.marker.ns = "cube"
+        self.marker.type = self.marker.CUBE
+        self.marker.action = self.marker.ADD
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        self.marker.color.a = 1
+        self.marker.color.r = 0.3
+        self.marker.color.g = 0.3
+        self.marker.color.b = 0.3
+
+        # marker position initial
+        self.marker.pose.position.x = 0
+        self.marker.pose.position.y = 0
+        self.marker.pose.position.z = 0.03
+        self.marker.pose.orientation.x = 0
+        self.marker.pose.orientation.y = 0
+        self.marker.pose.orientation.z = 0
+        self.marker.pose.orientation.w = 1.0
+        
+        
+    
     # 控制吸泵      
     def pub_pump(self, flag):
         if flag:
@@ -60,6 +90,12 @@ class Detect_marker():
             [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # A分拣区 A sorting area
             [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # B分拣区  B sorting area
         ]
+        
+        # publish marker
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = (coords[0][0]-x)/1000.0
+        self.marker.pose.position.y = (coords[0][1]-y)/1000.0
+        self.pub.publish(self.marker)
 
         # send coordinates to move mycobot
         self.mc.send_coords(coords[0], 30, 1)
@@ -77,6 +113,14 @@ class Detect_marker():
         time.sleep(4)
         self.pub_pump(False)  
         time.sleep(5)
+        
+        # publish marker
+        time.sleep(1)
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = coords[1][0]/1000.0
+        self.marker.pose.position.y = coords[1][1]/1000.0
+        self.pub.publish(self.marker)
+        
         self.mc.send_coords(coords[0], 30, 1)
         time.sleep(2)
 

mycobot_ai/aikit_280_pi/scripts/aikit_img.py

@@ -3,6 +3,9 @@ import cv2
 import numpy as np
 import time
 import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
 
 from pymycobot.mycobot import MyCobot
 
@@ -10,7 +13,7 @@ IS_CV_4 = cv2.__version__[0] == '4'
 __version__ = "1.0"  # Adaptive seeed
 
 
-class Object_detect():
+class Object_detect(Movement):
 
     def __init__(self, camera_x = 160, camera_y = 10):
         # inherit the parent class
@@ -79,6 +82,42 @@ class Object_detect():
         # Get ArUco marker params.
         self.aruco_params = cv2.aruco.DetectorParameters_create()
         
+        # init a Marker
+        rospy.init_node("marker", anonymous=True)
+        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+        # init a Marker
+        self.marker = Marker()
+        self.marker.header.frame_id = "joint1"
+        self.marker.ns = "cube"
+        self.marker.type = self.marker.CUBE
+        self.marker.action = self.marker.ADD
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        self.marker.color.a = 1.0
+        self.marker.color.g = 1.0
+        self.marker.color.r = 1.0
+
+        # marker position initial
+        self.marker.pose.position.x = 0
+        self.marker.pose.position.y = 0
+        self.marker.pose.position.z = 0.03
+        self.marker.pose.orientation.x = 0
+        self.marker.pose.orientation.y = 0
+        self.marker.pose.orientation.z = 0
+        self.marker.pose.orientation.w = 1.0
+
+        self.cache_x = self.cache_y = 0
+
+    # publish marker
+    def pub_marker(self, x, y, z=0.03):
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = x
+        self.marker.pose.position.y = y
+        self.marker.pose.position.z = z
+        self.marker.color.g = self.color
+        self.pub.publish(self.marker)
+        
     # pump_control pi
     def gpio_status(self, flag):
         if flag:
@@ -142,6 +181,8 @@ class Object_detect():
 
 
         self.mc.send_coords(self.move_coords[color], 25, 1)
+        self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0,
+                        self.move_coords[color][2]/1000.0)
         time.sleep(4)
 
         # close pump
@@ -366,13 +407,14 @@ class Object_detect():
 # The path to save the image folder
 def parse_folder(folder):
     restore = []
-    path1 = '/home/er/AiKit_280PI/' + folder
-    path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV/' + folder
+    path = ''
+    path1 = '/home/er/catkin_ws/src/mycobot_ros/mycobot_ai/aikit_280_pi/' + folder    # pi
+    path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV/' + folder    # windows
 
-    # if os.path.exists(path1):
-    #     path = path1
-    # elif os.path.exists(path2):
-    path = path1
+    if os.path.exists(path1):
+        path = path1
+    elif os.path.exists(path2):
+        path = path2
 
     for i, j, k in os.walk(path):
         for l in k:
@@ -571,7 +613,7 @@ def run():
                 # calculate real coord between cube and mycobot
                 real_x, real_y = detect.get_position(x, y)
                 detect.color = i
-                # detect.pub_marker(real_x / 1000.0, real_y / 1000.0)
+                detect.pub_marker(real_x / 1000.0, real_y / 1000.0)
                 detect.decide_move(real_x, real_y, detect.color)
                 # if num == 5:
                 #     detect.color = i