From a895fde7d73722f5e3c01f898fa9407c46abb36d Mon Sep 17 00:00:00 2001 From: weijian Date: Wed, 7 Dec 2022 14:42:56 +0800 Subject: [PATCH] add aikit_280pi color detect ros --- .../scripts/listen_real_of_topic.py | 2 +- .../aikit_280_pi/scripts/aikit_color.py | 61 ++++++++++++++++--- .../launch/communication_topic_pi.launch | 4 +- .../scripts/mycobot_topics_pi.py | 2 +- 4 files changed, 55 insertions(+), 14 deletions(-) diff --git a/mycobot_280/mycobot_280pi/scripts/listen_real_of_topic.py b/mycobot_280/mycobot_280pi/scripts/listen_real_of_topic.py index 631d0a2..33aa333 100755 --- a/mycobot_280/mycobot_280pi/scripts/listen_real_of_topic.py +++ b/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 diff --git a/mycobot_ai/aikit_280_pi/scripts/aikit_color.py b/mycobot_ai/aikit_280_pi/scripts/aikit_color.py index c873705..582e35a 100644 --- a/mycobot_ai/aikit_280_pi/scripts/aikit_color.py +++ b/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 diff --git a/mycobot_communication/launch/communication_topic_pi.launch b/mycobot_communication/launch/communication_topic_pi.launch index b51bf6f..c0c3fe9 100644 --- a/mycobot_communication/launch/communication_topic_pi.launch +++ b/mycobot_communication/launch/communication_topic_pi.launch @@ -1,7 +1,7 @@ - - + + diff --git a/mycobot_communication/scripts/mycobot_topics_pi.py b/mycobot_communication/scripts/mycobot_topics_pi.py index f529c53..2c0bcb4 100755 --- a/mycobot_communication/scripts/mycobot_topics_pi.py +++ b/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