add aikit_280pi color detect ros

mycobot_280/mycobot_280pi/scripts/listen_real_of_topic.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # encoding=utf-8
 import math
 

mycobot_ai/aikit_280_pi/scripts/aikit_color.py

@@ -14,7 +14,7 @@ __version__ = "1.0"
 # Adaptive seeed
 
 
-class Object_detect():
+class Object_detect(Movement):
 
     def __init__(self, camera_x = 155, camera_y = 10):
         # inherit the parent class
@@ -42,12 +42,10 @@ class Object_detect():
         self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
         self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
         self.raspi = False
-        if "dev" in self.robot_m5:
+        if "dev" in self.robot_m5 or "dev" in self.robot_wio:
             self.Pin = [2, 5]
         elif "dev" in self.robot_wio:
-            # self.Pin = [20, 21]
-            self.Pin = [2, 5]
-
+            self.Pin = [20, 21]
             for i in self.move_coords:
                 i[2] -= 20
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
@@ -98,7 +96,40 @@ class Object_detect():
         # Get ArUco marker params. 获取 ArUco 标记参数
         self.aruco_params = cv2.aruco.DetectorParameters_create()
 
-
+        # init a node and a publisher
+        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
+        
+    # 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:
@@ -157,10 +188,11 @@ class Object_detect():
         # print(tmp)
         self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
         time.sleep(2.5)
-
+        self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
+        
         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)
+        
+        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(3)
        
         # close pump
@@ -170,6 +202,15 @@ class Object_detect():
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             self.gpio_status(False)
         time.sleep(5)
+        
+        if color == 1:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0-0.02
+            )
+        elif color == 0:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0
+            )
 
         self.mc.send_angles(self.move_angles[0], 25)
         time.sleep(4.5)
@@ -458,7 +499,7 @@ if __name__ == "__main__":
             real_x, real_y = detect.get_position(x, y)
             # print('real_x',round(real_x, 3),round(real_y, 3))
             if num == 20:
-                
+                detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
                 detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
                 num = real_sx = real_sy = 0
 

mycobot_communication/launch/communication_topic_pi.launch

@@ -1,7 +1,7 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->
-	<arg name="port" default="/dev/ttyUSB0" />
-	<arg name="baud" default="115200" />
+	<arg name="port" default="/dev/ttyAMA0" />
+	<arg name="baud" default="1000000" />
 
 	<!-- Open communication service --><!-- 开启通讯服务 -->
 	<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_pi.py" output="screen">

mycobot_communication/scripts/mycobot_topics_pi.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # -*- coding:utf-8 -*-
 import time
 import os