mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
Merge pull request #91 from elephantrobotics/new_mycobot_ai
New mycobot ai
This commit is contained in:
wangWking
GitHub
commit
22991d519a
35 changed files with 3484 additions and 235 deletions
|
|
@ -71,6 +71,8 @@ class MycobotTopics(object):
|
|||
rospy.init_node("mycobot_topics")
|
||||
rospy.loginfo("start ...")
|
||||
port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
|
||||
if not port:
|
||||
port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
self.mc = MyCobot(port, baud)
|
||||
|
|
|
|||
|
|
@ -2,8 +2,8 @@
|
|||
<arg name="port" default="/dev/ttyACM0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mycobot.rviz" />
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mecharm/mecharm_aikit.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mecharm)/config/mecharm.rviz" />
|
||||
<!-- <arg name="gui" default="false" /> -->
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
|
@ -14,13 +14,13 @@
|
|||
</node>
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<include file="$(find mecharm_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
|
||||
<!-- listen and pub the real angles -->
|
||||
<node name="real_listener" pkg="mypalletizer_260" type="listen_real_of_topic.py" />
|
||||
<node name="real_listener" pkg="mecharm" type="listen_real_of_topic.py" />
|
||||
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
|
|
|||
BIN
mycobot_ai/ai_mecharm_270/res/blue/goal9.jpeg
Normal file
BIN
mycobot_ai/ai_mecharm_270/res/blue/goal9.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.9 KiB |
BIN
mycobot_ai/ai_mecharm_270/res/gray/goal9.jpeg
Normal file
BIN
mycobot_ai/ai_mecharm_270/res/gray/goal9.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.7 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 5.6 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 5.5 KiB After Width: | Height: | Size: 4.4 KiB |
BIN
mycobot_ai/ai_mecharm_270/res/red/goal8.jpeg
Normal file
BIN
mycobot_ai/ai_mecharm_270/res/red/goal8.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.9 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 46 KiB After Width: | Height: | Size: 48 KiB |
493
mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py
Normal file
493
mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py
Normal file
|
|
@ -0,0 +1,493 @@
|
|||
#!/usr/bin/env python2
|
||||
# -*- coding:utf-8 -*-
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import os,sys
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from moving_utils import Movement
|
||||
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0"
|
||||
# Adaptive seeed
|
||||
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 148, camera_y = 5): # m5
|
||||
# def __init__(self, camera_x = 148, camera_y = -5): # pi
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
|
||||
# declare 270
|
||||
self.mc = None
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[0, 0, 0, 0, 90, 0], # point to grab
|
||||
[-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[109.1, -118.8, 164.9, -179.02, 11.07, 132.93], # above the red bucket
|
||||
[178.4, -98.5, 172.7, -175.8, 41.25, 159.41], # above the green bucket
|
||||
[97.9, 139.9, 170.7, 163.54, 2.03, 156.04], # above the blue bucket
|
||||
[-1.8, 143.8, 172.4, 170.69, -4.62, 161.79], # above the gray bucket
|
||||
]
|
||||
|
||||
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
|
||||
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
|
||||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
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:
|
||||
self.Pin = [2, 5]
|
||||
# self.Pin = [5]
|
||||
elif "dev" in self.robot_wio:
|
||||
# self.Pin = [20, 21]
|
||||
self.Pin = [2, 5]
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 = 0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
# set color HSV
|
||||
self.HSV = {
|
||||
"yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
|
||||
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
|
||||
"green": [np.array([35, 43, 35]), np.array([90, 255, 255])],
|
||||
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
|
||||
"cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
|
||||
}
|
||||
# use to calculate coord between cube and 270
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the 270
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the 270
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
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:
|
||||
# self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
else:
|
||||
# self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
# self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
# self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move 270
|
||||
print(color)
|
||||
self.mc.send_angles(self.move_angles[0], 30)
|
||||
time.sleep(4)
|
||||
|
||||
# send coordinates to move 270
|
||||
self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
|
||||
time.sleep(3)
|
||||
self.pub_marker(x/1000.0, y/1000.0, 140/1000.0)
|
||||
|
||||
|
||||
self.mc.send_coords([x, y, 96, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 m5
|
||||
# self.mc.send_coords([x, y, 103, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 pi
|
||||
time.sleep(3)
|
||||
self.pub_marker(x/1000.0, y/1000.0, 96/1000.0)
|
||||
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
tmp = []
|
||||
while True:
|
||||
if not tmp:
|
||||
tmp = self.mc.get_angles()
|
||||
else:
|
||||
break
|
||||
time.sleep(0.5)
|
||||
|
||||
# print(tmp)
|
||||
self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
|
||||
time.sleep(3)
|
||||
|
||||
|
||||
|
||||
self.mc.send_coords(self.move_coords[color], 30, 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(3)
|
||||
|
||||
# close pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(6)
|
||||
|
||||
if color == 1:
|
||||
self.pub_marker(
|
||||
self.move_coords[color][0]/1000.0+0.04, 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[1], 30)
|
||||
time.sleep(1.5)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
print(x, y, self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init 270
|
||||
def run(self):
|
||||
if "dev" in self.robot_m5:
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_wio:
|
||||
self.mc = MyCobot(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
def draw_marker(self, img, x, y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(img, "({},{})".format(x, y), (x, y),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self, img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params
|
||||
)
|
||||
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0] == 1:
|
||||
return None
|
||||
x1 = x2 = y1 = y2 = 0
|
||||
point_11, point_21, point_31, point_41 = corners[0][0]
|
||||
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
|
||||
(point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
|
||||
point_1, point_2, point_3, point_4 = corners[1][0]
|
||||
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
|
||||
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
|
||||
return x1, x2, y1, y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and 270
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0/ratio
|
||||
|
||||
# calculate the coords between cube and 270
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2*0.78):int(self.y1*1.1),
|
||||
int(self.x1*0.86):int(self.x2*1.08)]
|
||||
return frame
|
||||
|
||||
# detect cube color
|
||||
def color_detect(self, img):
|
||||
# set the arrangement of color'HSV
|
||||
x = y = 0
|
||||
gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊
|
||||
# transfrom the img to model of gray
|
||||
hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
for mycolor, item in self.HSV.items():
|
||||
# print("mycolor:",mycolor)
|
||||
redLower = np.array(item[0])
|
||||
redUpper = np.array(item[1])
|
||||
|
||||
# wipe off all color expect color in range
|
||||
mask = cv2.inRange(hsv, item[0], item[1])
|
||||
|
||||
# a etching operation on a picture to remove edge roughness
|
||||
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
|
||||
|
||||
# the image for expansion operation, its role is to deepen the color depth in the picture
|
||||
dilation = cv2.dilate(erosion, np.ones(
|
||||
(1, 1), np.uint8), iterations=2)
|
||||
|
||||
# adds pixels to the image
|
||||
target = cv2.bitwise_and(img, img, mask=dilation)
|
||||
|
||||
# the filtered image is transformed into a binary image and placed in binary
|
||||
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
|
||||
|
||||
# get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
|
||||
contours, hierarchy = cv2.findContours(
|
||||
dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
if len(contours) > 0:
|
||||
# do something about misidentification
|
||||
boxes = [
|
||||
box
|
||||
for box in [cv2.boundingRect(c) for c in contours]
|
||||
if min(img.shape[0], img.shape[1]) / 10
|
||||
< min(box[2], box[3])
|
||||
< min(img.shape[0], img.shape[1]) / 1
|
||||
]
|
||||
if boxes:
|
||||
for box in boxes:
|
||||
x, y, w, h = box
|
||||
# find the largest object that fits the requirements
|
||||
c = max(contours, key=cv2.contourArea)
|
||||
# get the lower left and upper right points of the positioning object
|
||||
x, y, w, h = cv2.boundingRect(c)
|
||||
# locate the target by drawing rectangle
|
||||
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
|
||||
# calculate the rectangle center
|
||||
x, y = (x*2+w)/2, (y*2+h)/2
|
||||
# calculate the real coordinates of 270 relative to the target
|
||||
|
||||
if mycolor == "red":
|
||||
self.color = 0
|
||||
elif mycolor == "green":
|
||||
self.color = 1
|
||||
elif mycolor == "cyan" or mycolor == "blue":
|
||||
self.color = 2
|
||||
else:
|
||||
self.color = 3
|
||||
|
||||
if abs(x) + abs(y) > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
# open the camera
|
||||
cap_num = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init 270
|
||||
detect.run()
|
||||
|
||||
_init_ = 20
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
num = 0
|
||||
real_sx = real_sy = 0
|
||||
while cv2.waitKey(1) < 0:
|
||||
# read camera
|
||||
_, frame = cap.read()
|
||||
# deal img
|
||||
frame = detect.transform_frame(frame)
|
||||
if _init_ > 0:
|
||||
_init_ -= 1
|
||||
continue
|
||||
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
init_num += 1
|
||||
continue
|
||||
elif init_num == 20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1)/20.0,
|
||||
(detect.sum_y1)/20.0,
|
||||
(detect.sum_x2)/20.0,
|
||||
(detect.sum_y2)/20.0,
|
||||
)
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and 270
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
nparams += 1
|
||||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and 270
|
||||
detect.set_params(
|
||||
(detect.sum_x1+detect.sum_x2)/20.0,
|
||||
(detect.sum_y1+detect.sum_y2)/20.0,
|
||||
abs(detect.sum_x1-detect.sum_x2)/10.0 +
|
||||
abs(detect.sum_y1-detect.sum_y2)/10.0
|
||||
)
|
||||
print("ok")
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
detect_result = detect.color_detect(frame)
|
||||
if detect_result is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and 270
|
||||
real_x, real_y = detect.get_position(x, y)
|
||||
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
|
||||
|
||||
else:
|
||||
num += 1
|
||||
real_sy += real_y
|
||||
real_sx += real_x
|
||||
|
||||
cv2.imshow("figure", frame)
|
||||
|
||||
# close the window
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
sys.exit()
|
||||
|
|
@ -0,0 +1,643 @@
|
|||
#!/usr/bin/env python2
|
||||
# encoding:utf-8
|
||||
from multiprocessing import Process, Pipe
|
||||
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
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0" # Adaptive seeed
|
||||
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 148, camera_y = 5):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
|
||||
# declare 270
|
||||
self.mc = None
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[0, 0, 0, 0, 90, 0], # point to grab
|
||||
[-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[109.1, -118.8, 164.9, -179.02, 11.07, 132.93], # above the red bucket
|
||||
[178.4, -98.5, 172.7, -175.8, 41.25, 159.41], # above the green bucket
|
||||
[97.9, 139.9, 170.7, 163.54, 2.03, 156.04], # above the blue bucket
|
||||
[-1.8, 143.8, 172.4, 170.69, -4.62, 161.79], # above the gray bucket
|
||||
]
|
||||
|
||||
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
|
||||
self.raspi = False
|
||||
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
|
||||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
if "dev" in self.robot_m5:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
# self.Pin = [20, 21]
|
||||
self.Pin = [2, 5]
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 = 0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
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
|
||||
|
||||
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:
|
||||
# self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
else:
|
||||
# self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
# self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
# self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move 270
|
||||
self.mc.send_angles(self.move_angles[0], 30)
|
||||
time.sleep(4)
|
||||
|
||||
print("x %s ,y %s" % (x,y))
|
||||
# send coordinates to move 270 根据不同底板机械臂，调整吸泵高度
|
||||
self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
|
||||
time.sleep(3)
|
||||
|
||||
self.mc.send_coords([x, y, 96, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 m5
|
||||
# self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 pi
|
||||
# self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
|
||||
time.sleep(3)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
tmp = []
|
||||
while True:
|
||||
if not tmp:
|
||||
tmp = self.mc.get_angles()
|
||||
else:
|
||||
break
|
||||
time.sleep(0.5)
|
||||
|
||||
# print(tmp)
|
||||
self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
|
||||
time.sleep(3)
|
||||
|
||||
|
||||
self.mc.send_coords(self.move_coords[color], 30, 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(6)
|
||||
|
||||
# close pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(6)
|
||||
|
||||
self.mc.send_angles(self.move_angles[1], 30)
|
||||
time.sleep(6)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
print(x, y, self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init 270
|
||||
def run(self):
|
||||
if "dev" in self.robot_wio :
|
||||
self.mc = MyCobot(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_m5:
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
def draw_marker(self, img, x, y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(
|
||||
img,
|
||||
"({},{})".format(x, y),
|
||||
(x, y),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL,
|
||||
1,
|
||||
(243, 0, 0),
|
||||
2,
|
||||
)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self, img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params)
|
||||
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0] == 1:
|
||||
return None
|
||||
x1 = x2 = y1 = y2 = 0
|
||||
point_11, point_21, point_31, point_41 = corners[0][0]
|
||||
x1, y1 = int(
|
||||
(point_11[0] + point_21[0] + point_31[0] + point_41[0]) /
|
||||
4.0), int(
|
||||
(point_11[1] + point_21[1] + point_31[1] + point_41[1])
|
||||
/ 4.0)
|
||||
point_1, point_2, point_3, point_4 = corners[1][0]
|
||||
x2, y2 = int(
|
||||
(point_1[0] + point_2[0] + point_3[0] + point_4[0]) /
|
||||
4.0), int(
|
||||
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) /
|
||||
4.0)
|
||||
return x1, x2, y1, y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0 / ratio
|
||||
|
||||
# calculate the coords between cube and mycobot
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y) * self.ratio +
|
||||
self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15),
|
||||
int(self.x1 * 0.7):int(self.x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
# according the class_id to get object name
|
||||
def id_class_name(self, class_id):
|
||||
for key, value in self.labels.items():
|
||||
if class_id == int(key):
|
||||
return value
|
||||
|
||||
# detect object
|
||||
def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection):
|
||||
i = 0
|
||||
MIN_MATCH_COUNT = 5
|
||||
# sift = cv2.xfeatures2d.SIFT_create()
|
||||
|
||||
# find the keypoints and descriptors with SIFT
|
||||
# kp = []
|
||||
# des = []
|
||||
kp = kp_list
|
||||
des = desc_list
|
||||
|
||||
# for i in goal:
|
||||
# kp0, des0 = sift.detectAndCompute(i, None)
|
||||
# kp.append(kp0)
|
||||
# des.append(des0)
|
||||
|
||||
# kp1, des1 = sift.detectAndCompute(goal, None)
|
||||
# kp2, des2 = sift.detectAndCompute(img, None)
|
||||
kp2, des2 = kp_img, desc_img
|
||||
|
||||
# FLANN parameters
|
||||
FLANN_INDEX_KDTREE = 0
|
||||
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
|
||||
search_params = dict(checks=50) # or pass empty dictionary
|
||||
flann = cv2.FlannBasedMatcher(index_params, search_params)
|
||||
|
||||
x, y = 0, 0
|
||||
try:
|
||||
for i in range(len(des)):
|
||||
matches = flann.knnMatch(des[i], des2, k=2)
|
||||
# store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。
|
||||
good = []
|
||||
for m, n in matches:
|
||||
if m.distance < 0.7 * n.distance:
|
||||
good.append(m)
|
||||
|
||||
# When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
|
||||
if len(good) > MIN_MATCH_COUNT:
|
||||
|
||||
# extract corresponding point pairs from matching 从匹配中提取出对应点对
|
||||
# query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引
|
||||
src_pts = np.float32([kp[i][m.queryIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
dst_pts = np.float32([kp2[m.trainIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
|
||||
# Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
|
||||
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,
|
||||
5.0)
|
||||
matchesMask = mask.ravel().tolist()
|
||||
# Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
|
||||
h, w, d = goal[i].shape
|
||||
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
|
||||
[w - 1, 0]]).reshape(-1, 1, 2)
|
||||
dst = cv2.perspectiveTransform(pts, M)
|
||||
coord = (dst[0][0] + dst[1][0] + dst[2][0] +
|
||||
dst[3][0]) / 4.0
|
||||
connection.send((DRAW_COORDS, coord))
|
||||
# cv2.putText(img, "{}".format(coord), (50, 60),
|
||||
# fontFace=None, fontScale=1,
|
||||
# color=(0, 255, 0), lineType=1)
|
||||
print(format(dst[0][0][0]))
|
||||
x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] +
|
||||
dst[3][0][0]) / 4.0
|
||||
y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] +
|
||||
dst[3][0][1]) / 4.0
|
||||
|
||||
# bound box 绘制边框
|
||||
# img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
connection.send((DRAW_RECT, dst))
|
||||
# cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA)
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
if x + y > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
# The path to save the image folder
|
||||
def parse_folder(folder):
|
||||
restore = []
|
||||
# path = ''
|
||||
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder
|
||||
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder
|
||||
|
||||
if os.path.exists(path1):
|
||||
path = path1
|
||||
elif os.path.exists(path2):
|
||||
path = path2
|
||||
|
||||
# print("path:",path)
|
||||
for i, j, k in os.walk(path):
|
||||
for l in k:
|
||||
restore.append(cv2.imread(folder + '/{}'.format(l)))
|
||||
return restore
|
||||
|
||||
def compute_keypoints_and_descriptors(sift, images_lists):
|
||||
kp_list = []
|
||||
desc_list = []
|
||||
for images in images_lists:
|
||||
kp_tmp = []
|
||||
desc_tmp = []
|
||||
for img in images:
|
||||
kp, desc = sift.detectAndCompute(img, None)
|
||||
kp_tmp.append(kp)
|
||||
desc_tmp.append(desc)
|
||||
kp_list.append(kp_tmp)
|
||||
desc_list.append(desc_tmp)
|
||||
|
||||
return kp_list, desc_list
|
||||
|
||||
GET_FRAME = 1
|
||||
STOP_PROCESSING = 2
|
||||
DRAW_COORDS = 3
|
||||
DRAW_RECT = 4
|
||||
CLEAR_DRAW = 5
|
||||
CROP_FRAME = 6
|
||||
|
||||
def get_frame(connection):
|
||||
connection.send(GET_FRAME)
|
||||
frame = connection.recv()
|
||||
return frame
|
||||
|
||||
def process_transform_frame(frame, x1, y1, x2, y2):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
# if x1 != x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
# frame = frame[int(y2 * 0.2):int(y1 * 1.15),
|
||||
# int(x1 * 0.7):int(x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
def process_display_frame(connection):
|
||||
cap_num = 0
|
||||
coord = None
|
||||
dst = None
|
||||
x1 = 0
|
||||
y1 = 0
|
||||
x2 = 0
|
||||
y2 = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
while cv2.waitKey(1) < 0:
|
||||
_, frame = cap.read()
|
||||
frame = process_transform_frame(frame, x1, y1, x2, y2)
|
||||
if connection.poll():
|
||||
request = connection.recv()
|
||||
if request == GET_FRAME:
|
||||
connection.send(frame)
|
||||
elif request == CLEAR_DRAW:
|
||||
coord = None
|
||||
dst = None
|
||||
elif type(request) is tuple:
|
||||
if request[0] == DRAW_COORDS:
|
||||
coord = request[1]
|
||||
elif request[0] == DRAW_RECT:
|
||||
dst = request[1]
|
||||
elif request[0] == CROP_FRAME:
|
||||
x1 = request[1]
|
||||
y1 = request[2]
|
||||
x2 = request[3]
|
||||
y2 = request[4]
|
||||
|
||||
if not coord is None:
|
||||
cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
|
||||
fontScale=1, color=(0, 255, 0), lineType=1)
|
||||
if not dst is None:
|
||||
frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
cv2.imshow("figure", frame)
|
||||
time.sleep(0.04)
|
||||
connection.send(STOP_PROCESSING)
|
||||
|
||||
def run():
|
||||
parent_conn, child_conn = Pipe()
|
||||
child = Process(target = process_display_frame, args=(child_conn,))
|
||||
child.start()
|
||||
|
||||
res_queue = [[], [], [], []]
|
||||
res_queue[0] = parse_folder('res/red')
|
||||
res_queue[1] = parse_folder('res/green')
|
||||
res_queue[2] = parse_folder('res/blue')
|
||||
res_queue[3] = parse_folder('res/gray')
|
||||
|
||||
|
||||
sift = cv2.xfeatures2d.SIFT_create()
|
||||
kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
|
||||
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
|
||||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
# _init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
# num = 0
|
||||
# real_sx = real_sy = 0
|
||||
while True:
|
||||
start_time = time.time()
|
||||
if parent_conn.poll():
|
||||
data = parent_conn.recv()
|
||||
if data == STOP_PROCESSING:
|
||||
break
|
||||
# read camera
|
||||
frame = get_frame(parent_conn)
|
||||
# deal img
|
||||
#frame = detect.transform_frame(frame)
|
||||
|
||||
# if _init_ > 0:
|
||||
# _init_ -= 1
|
||||
# continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
init_num += 1
|
||||
continue
|
||||
elif init_num == 20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0,
|
||||
)
|
||||
parent_conn.send((CROP_FRAME,
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0))
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
nparams += 1
|
||||
print ("ok")
|
||||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mycobot
|
||||
detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
|
||||
(detect.sum_y1 + detect.sum_y2) / 20.0,
|
||||
abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
|
||||
abs(detect.sum_y1 - detect.sum_y2) / 10.0)
|
||||
print("ok")
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
kp_img, desc_img = sift.detectAndCompute(frame, None)
|
||||
frame = get_frame(parent_conn)
|
||||
for i, v in enumerate(res_queue):
|
||||
# HACK: to update frame every time
|
||||
detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
|
||||
if detect_result:
|
||||
x, y = detect_result
|
||||
# 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.decide_move(real_x, real_y, detect.color)
|
||||
# if num == 5:
|
||||
# detect.color = i
|
||||
# detect.pub_marker(real_sx / 5.0 / 1000.0,
|
||||
# real_sy / 5.0 / 1000.0)
|
||||
# detect.decide_move(real_sx / 5.0, real_sy / 5.0,
|
||||
# detect.color)
|
||||
# num = real_sx = real_sy = 0
|
||||
# else:
|
||||
# num += 1
|
||||
# real_sy += real_y
|
||||
# real_sx += real_x
|
||||
parent_conn.send(CLEAR_DRAW)
|
||||
|
||||
# cv2.imshow("figure", frame)
|
||||
time.sleep(0.05)
|
||||
end_time = time.time()
|
||||
print("loop_time = ", end_time - start_time)
|
||||
|
||||
# close the window
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
# cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
sys.exit()
|
||||
|
||||
child.join()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run()
|
||||
# Object_detect().take_photo()
|
||||
# Object_detect().cut_photo()
|
||||
BIN
mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg
Normal file
BIN
mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.5 KiB |
BIN
mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg
Normal file
BIN
mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.3 KiB |
BIN
mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg
Normal file
BIN
mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.5 KiB |
BIN
mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg
Normal file
BIN
mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.9 KiB |
BIN
mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg
Normal file
BIN
mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.5 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 63 KiB After Width: | Height: | Size: 42 KiB |
480
mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py
Executable file
480
mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py
Executable file
|
|
@ -0,0 +1,480 @@
|
|||
# encoding:utf-8
|
||||
#!/usr/bin/env python2
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import os,sys
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from moving_utils import Movement
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0"
|
||||
# Adaptive seeed
|
||||
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 155, camera_y = 10):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
self.mc = None
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point
|
||||
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket
|
||||
[238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green
|
||||
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue
|
||||
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray
|
||||
]
|
||||
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
|
||||
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
|
||||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
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:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
# self.Pin = [20, 21]
|
||||
self.Pin = [2, 5]
|
||||
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 = 0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
# set color HSV
|
||||
self.HSV = {
|
||||
"yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
|
||||
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
|
||||
"green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255]
|
||||
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
|
||||
"cyan": [np.array([89, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255])
|
||||
}
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
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)
|
||||
|
||||
def gpio_status(self, flag):
|
||||
if flag:
|
||||
# self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
else:
|
||||
# self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
# self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
# self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mycobot
|
||||
print (color)
|
||||
self.mc.send_angles(self.move_angles[1], 25)
|
||||
time.sleep(3)
|
||||
|
||||
# send coordinates to move mycobot
|
||||
self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
|
||||
time.sleep(3)
|
||||
|
||||
# self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
|
||||
# time.sleep(3)
|
||||
|
||||
self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0)
|
||||
time.sleep(3)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
tmp = []
|
||||
while True:
|
||||
if not tmp:
|
||||
tmp = self.mc.get_angles()
|
||||
else:
|
||||
break
|
||||
time.sleep(0.5)
|
||||
|
||||
# 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)
|
||||
time.sleep(3)
|
||||
|
||||
# close pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(4)
|
||||
|
||||
if color == 1:
|
||||
self.pub_marker(
|
||||
self.move_coords[color][0]/1000.0+0.04, 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.pub_angles(self.move_angles[0], 20)
|
||||
self.mc.send_angles(self.move_angles[0], 25)
|
||||
time.sleep(3)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
print(x, y, self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mycobot
|
||||
def run(self):
|
||||
if "dev" in self.robot_wio :
|
||||
self.mc = MyCobot(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_m5:
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
|
||||
def draw_marker(self, img, x, y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(img, "({},{})".format(x, y), (x, y),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self, img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params
|
||||
)
|
||||
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0] == 1:
|
||||
return None
|
||||
x1 = x2 = y1 = y2 = 0
|
||||
point_11, point_21, point_31, point_41 = corners[0][0]
|
||||
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
|
||||
(point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
|
||||
point_1, point_2, point_3, point_4 = corners[1][0]
|
||||
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
|
||||
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
|
||||
return x1, x2, y1, y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0/ratio
|
||||
|
||||
# calculate the coords between cube and mycobot
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2*0.78):int(self.y1*1.1),
|
||||
int(self.x1*0.86):int(self.x2*1.08)]
|
||||
return frame
|
||||
|
||||
# detect cube color
|
||||
def color_detect(self, img):
|
||||
# set the arrangement of color'HSV
|
||||
x = y = 0
|
||||
gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊
|
||||
# transfrom the img to model of gray
|
||||
hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
for mycolor, item in self.HSV.items():
|
||||
redLower = np.array(item[0])
|
||||
redUpper = np.array(item[1])
|
||||
# wipe off all color expect color in range
|
||||
mask = cv2.inRange(hsv, item[0], item[1])
|
||||
# a etching operation on a picture to remove edge roughness
|
||||
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
|
||||
# the image for expansion operation, its role is to deepen the color depth in the picture
|
||||
dilation = cv2.dilate(erosion, np.ones(
|
||||
(1, 1), np.uint8), iterations=2)
|
||||
|
||||
|
||||
# adds pixels to the image
|
||||
target = cv2.bitwise_and(img, img, mask=dilation)
|
||||
# the filtered image is transformed into a binary image and placed in binary
|
||||
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
|
||||
# get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
|
||||
contours, hierarchy = cv2.findContours(
|
||||
dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
if len(contours) > 0:
|
||||
# do something about misidentification
|
||||
boxes = [
|
||||
box
|
||||
for box in [cv2.boundingRect(c) for c in contours]
|
||||
if min(img.shape[0], img.shape[1]) / 10
|
||||
< min(box[2], box[3])
|
||||
< min(img.shape[0], img.shape[1]) / 1
|
||||
]
|
||||
if boxes:
|
||||
for box in boxes:
|
||||
x, y, w, h = box
|
||||
# find the largest object that fits the requirements
|
||||
c = max(contours, key=cv2.contourArea)
|
||||
# get the lower left and upper right points of the positioning object
|
||||
x, y, w, h = cv2.boundingRect(c)
|
||||
# locate the target by drawing rectangle
|
||||
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
|
||||
# calculate the rectangle center
|
||||
x, y = (x*2+w)/2, (y*2+h)/2
|
||||
# calculate the real coordinates of mycobot relative to the target
|
||||
if mycolor == "red":
|
||||
self.color = 0
|
||||
|
||||
elif mycolor == "green":
|
||||
self.color = 1
|
||||
|
||||
elif mycolor == "cyan" or mycolor == "blue":
|
||||
self.color = 2
|
||||
|
||||
else:
|
||||
self.color = 3
|
||||
|
||||
|
||||
if abs(x) + abs(y) > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# open the camera
|
||||
cap_num = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
_init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
num = 0
|
||||
real_sx = real_sy = 0
|
||||
while cv2.waitKey(1) < 0:
|
||||
# read camera
|
||||
_, frame = cap.read()
|
||||
# deal img
|
||||
frame = detect.transform_frame(frame)
|
||||
if _init_ > 0:
|
||||
_init_ -= 1
|
||||
continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
init_num += 1
|
||||
continue
|
||||
elif init_num == 20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1)/20.0,
|
||||
(detect.sum_y1)/20.0,
|
||||
(detect.sum_x2)/20.0,
|
||||
(detect.sum_y2)/20.0,
|
||||
)
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
nparams += 1
|
||||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mycobot
|
||||
detect.set_params(
|
||||
(detect.sum_x1+detect.sum_x2)/20.0,
|
||||
(detect.sum_y1+detect.sum_y2)/20.0,
|
||||
abs(detect.sum_x1-detect.sum_x2)/10.0 +
|
||||
abs(detect.sum_y1-detect.sum_y2)/10.0
|
||||
)
|
||||
print ("ok")
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
detect_result = detect.color_detect(frame)
|
||||
if detect_result is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and mycobot
|
||||
real_x, real_y = detect.get_position(x, y)
|
||||
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
|
||||
|
||||
else:
|
||||
num += 1
|
||||
real_sy += real_y
|
||||
real_sx += real_x
|
||||
|
||||
cv2.imshow("figure", frame)
|
||||
660
mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py
Executable file
660
mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py
Executable file
|
|
@ -0,0 +1,660 @@
|
|||
# encoding:utf-8
|
||||
#!/usr/bin/env python2
|
||||
|
||||
from multiprocessing import Process, Pipe
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import os,sys
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from moving_utils import Movement
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0" # Adaptive seeed
|
||||
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 155, camera_y = 10):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
self.mc = None
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point
|
||||
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket
|
||||
[238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green
|
||||
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue
|
||||
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray
|
||||
]
|
||||
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
|
||||
self.raspi = False
|
||||
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
|
||||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
if "dev" in self.robot_m5:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
self.Pin = [2, 5]
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 = 0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
# load model of img recognition
|
||||
# self.model_path = os.path.join(dir_path, "frozen_inference_graph.pb")
|
||||
# self.pbtxt_path = os.path.join(dir_path, "graph.pbtxt")
|
||||
# self.label_path = os.path.join(dir_path, "labels.json")
|
||||
# # load class labels
|
||||
# self.labels = json.load(open(self.label_path))
|
||||
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
self.aruco_params = cv2.aruco.DetectorParameters_create()
|
||||
|
||||
# if IS_CV_4:
|
||||
# self.net = cv2.dnn.readNetFromTensorflow(self.model_path, self.pbtxt_path)
|
||||
# else:
|
||||
# print('Load tensorflow model need the version of opencv is 4.')
|
||||
# exit(0)
|
||||
# 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
|
||||
|
||||
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)
|
||||
|
||||
def gpio_status(self, flag):
|
||||
if flag:
|
||||
# self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
else:
|
||||
self.GPIO.output(20, 1)
|
||||
# self.GPIO.output(21, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
# self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
# self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mycobot
|
||||
print (color)
|
||||
self.mc.send_angles(self.move_angles[1], 25)
|
||||
time.sleep(3)
|
||||
|
||||
# send coordinates to move mycobot
|
||||
self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
|
||||
time.sleep(3)
|
||||
|
||||
# self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
|
||||
# time.sleep(3)
|
||||
|
||||
# self.mc.send_coords([x, y, 105, 179.87, -3.78, -62.75], 25, 0)
|
||||
self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0)
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
tmp = []
|
||||
while True:
|
||||
if not tmp:
|
||||
tmp = self.mc.get_angles()
|
||||
else:
|
||||
break
|
||||
time.sleep(0.5)
|
||||
|
||||
# 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(5)
|
||||
|
||||
|
||||
|
||||
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
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(4)
|
||||
|
||||
self.mc.send_angles(self.move_angles[0], 25)
|
||||
time.sleep(3)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
print(x, y, self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mycobot
|
||||
def run(self):
|
||||
if "dev" in self.robot_wio :
|
||||
self.mc = MyCobot(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_m5:
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
def draw_marker(self, img, x, y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(
|
||||
img,
|
||||
"({},{})".format(x, y),
|
||||
(x, y),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL,
|
||||
1,
|
||||
(243, 0, 0),
|
||||
2,
|
||||
)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self, img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params)
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0] == 1:
|
||||
return None
|
||||
x1 = x2 = y1 = y2 = 0
|
||||
point_11, point_21, point_31, point_41 = corners[0][0]
|
||||
x1, y1 = int(
|
||||
(point_11[0] + point_21[0] + point_31[0] + point_41[0]) /
|
||||
4.0), int(
|
||||
(point_11[1] + point_21[1] + point_31[1] + point_41[1])
|
||||
/ 4.0)
|
||||
point_1, point_2, point_3, point_4 = corners[1][0]
|
||||
x2, y2 = int(
|
||||
(point_1[0] + point_2[0] + point_3[0] + point_4[0]) /
|
||||
4.0), int(
|
||||
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) /
|
||||
4.0)
|
||||
return x1, x2, y1, y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0 / ratio
|
||||
|
||||
# calculate the coords between cube and mycobot
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y) * self.ratio +
|
||||
self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15),
|
||||
int(self.x1 * 0.7):int(self.x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
# according the class_id to get object name
|
||||
def id_class_name(self, class_id):
|
||||
for key, value in self.labels.items():
|
||||
if class_id == int(key):
|
||||
return value
|
||||
|
||||
# detect object
|
||||
def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection):
|
||||
i = 0
|
||||
MIN_MATCH_COUNT = 5
|
||||
# sift = cv2.xfeatures2d.SIFT_create()
|
||||
|
||||
# find the keypoints and descriptors with SIFT
|
||||
# kp = []
|
||||
# des = []
|
||||
kp = kp_list
|
||||
des = desc_list
|
||||
|
||||
# for i in goal:
|
||||
# kp0, des0 = sift.detectAndCompute(i, None)
|
||||
# kp.append(kp0)
|
||||
# des.append(des0)
|
||||
|
||||
# kp1, des1 = sift.detectAndCompute(goal, None)
|
||||
# kp2, des2 = sift.detectAndCompute(img, None)
|
||||
kp2, des2 = kp_img, desc_img
|
||||
|
||||
# FLANN parameters
|
||||
FLANN_INDEX_KDTREE = 0
|
||||
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
|
||||
search_params = dict(checks=50) # or pass empty dictionary
|
||||
flann = cv2.FlannBasedMatcher(index_params, search_params)
|
||||
|
||||
x, y = 0, 0
|
||||
try:
|
||||
for i in range(len(des)):
|
||||
matches = flann.knnMatch(des[i], des2, k=2)
|
||||
# store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。
|
||||
good = []
|
||||
for m, n in matches:
|
||||
if m.distance < 0.7 * n.distance:
|
||||
good.append(m)
|
||||
|
||||
# When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
|
||||
if len(good) > MIN_MATCH_COUNT:
|
||||
|
||||
# extract corresponding point pairs from matching 从匹配中提取出对应点对
|
||||
# query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引
|
||||
src_pts = np.float32([kp[i][m.queryIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
dst_pts = np.float32([kp2[m.trainIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
|
||||
# Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
|
||||
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,
|
||||
5.0)
|
||||
matchesMask = mask.ravel().tolist()
|
||||
# Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
|
||||
h, w, d = goal[i].shape
|
||||
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
|
||||
[w - 1, 0]]).reshape(-1, 1, 2)
|
||||
dst = cv2.perspectiveTransform(pts, M)
|
||||
coord = (dst[0][0] + dst[1][0] + dst[2][0] +
|
||||
dst[3][0]) / 4.0
|
||||
connection.send((DRAW_COORDS, coord))
|
||||
# cv2.putText(img, "{}".format(coord), (50, 60),
|
||||
# fontFace=None, fontScale=1,
|
||||
# color=(0, 255, 0), lineType=1)
|
||||
print(format(dst[0][0][0]))
|
||||
x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] +
|
||||
dst[3][0][0]) / 4.0
|
||||
y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] +
|
||||
dst[3][0][1]) / 4.0
|
||||
|
||||
# bound box 绘制边框
|
||||
# img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
connection.send((DRAW_RECT, dst))
|
||||
# cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA)
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
if x + y > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
# The path to save the image folder
|
||||
def parse_folder(folder):
|
||||
restore = []
|
||||
path = ''
|
||||
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' + folder
|
||||
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' + folder
|
||||
if os.path.exists(path1):
|
||||
path = path1
|
||||
elif os.path.exists(path2):
|
||||
path = path2
|
||||
|
||||
# print("path:",path)
|
||||
|
||||
for i, j, k in os.walk(path):
|
||||
for l in k:
|
||||
restore.append(cv2.imread(folder + '/{}'.format(l)))
|
||||
return restore
|
||||
|
||||
def compute_keypoints_and_descriptors(sift, images_lists):
|
||||
kp_list = []
|
||||
desc_list = []
|
||||
for images in images_lists:
|
||||
kp_tmp = []
|
||||
desc_tmp = []
|
||||
for img in images:
|
||||
kp, desc = sift.detectAndCompute(img, None)
|
||||
kp_tmp.append(kp)
|
||||
desc_tmp.append(desc)
|
||||
kp_list.append(kp_tmp)
|
||||
desc_list.append(desc_tmp)
|
||||
|
||||
return kp_list, desc_list
|
||||
|
||||
GET_FRAME = 1
|
||||
STOP_PROCESSING = 2
|
||||
DRAW_COORDS = 3
|
||||
DRAW_RECT = 4
|
||||
CLEAR_DRAW = 5
|
||||
CROP_FRAME = 6
|
||||
|
||||
def get_frame(connection):
|
||||
connection.send(GET_FRAME)
|
||||
frame = connection.recv()
|
||||
return frame
|
||||
|
||||
def process_transform_frame(frame, x1, y1, x2, y2):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
# if x1 != x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
# frame = frame[int(y2 * 0.2):int(y1 * 1.15),
|
||||
# int(x1 * 0.7):int(x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
def process_display_frame(connection):
|
||||
cap_num = 0
|
||||
coord = None
|
||||
dst = None
|
||||
x1 = 0
|
||||
y1 = 0
|
||||
x2 = 0
|
||||
y2 = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
while cv2.waitKey(1) < 0:
|
||||
_, frame = cap.read()
|
||||
frame = process_transform_frame(frame, x1, y1, x2, y2)
|
||||
if connection.poll():
|
||||
request = connection.recv()
|
||||
if request == GET_FRAME:
|
||||
connection.send(frame)
|
||||
elif request == CLEAR_DRAW:
|
||||
coord = None
|
||||
dst = None
|
||||
elif type(request) is tuple:
|
||||
if request[0] == DRAW_COORDS:
|
||||
coord = request[1]
|
||||
elif request[0] == DRAW_RECT:
|
||||
dst = request[1]
|
||||
elif request[0] == CROP_FRAME:
|
||||
x1 = request[1]
|
||||
y1 = request[2]
|
||||
x2 = request[3]
|
||||
y2 = request[4]
|
||||
|
||||
if not coord is None:
|
||||
cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
|
||||
fontScale=1, color=(0, 255, 0), lineType=1)
|
||||
if not dst is None:
|
||||
frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
cv2.imshow("figure", frame)
|
||||
time.sleep(0.04)
|
||||
connection.send(STOP_PROCESSING)
|
||||
|
||||
def run():
|
||||
parent_conn, child_conn = Pipe()
|
||||
child = Process(target = process_display_frame, args=(child_conn,))
|
||||
child.start()
|
||||
|
||||
# Object_detect().take_photo()
|
||||
# Object_detect().cut_photo()
|
||||
# goal = Object_detect().distinguist()
|
||||
|
||||
res_queue = [[], [], [], []]
|
||||
res_queue[0] = parse_folder('res/red')
|
||||
res_queue[1] = parse_folder('res/green')
|
||||
res_queue[2] = parse_folder('res/blue')
|
||||
res_queue[3] = parse_folder('res/gray')
|
||||
|
||||
# res_queue = []
|
||||
# res_queue.extend(parse_folder('res/red'))
|
||||
# res_queue.extend(parse_folder('res/green'))
|
||||
# res_queue.extend(parse_folder('res/gray'))
|
||||
# res_queue.extend(parse_folder('res/blue'))
|
||||
|
||||
sift = cv2.xfeatures2d.SIFT_create()
|
||||
kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
|
||||
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
# _init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
# num = 0
|
||||
# real_sx = real_sy = 0
|
||||
while True:
|
||||
start_time = time.time()
|
||||
if parent_conn.poll():
|
||||
data = parent_conn.recv()
|
||||
if data == STOP_PROCESSING:
|
||||
break
|
||||
# read camera
|
||||
frame = get_frame(parent_conn)
|
||||
# deal img
|
||||
#frame = detect.transform_frame(frame)
|
||||
|
||||
# if _init_ > 0:
|
||||
# _init_ -= 1
|
||||
# continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
init_num += 1
|
||||
continue
|
||||
elif init_num == 20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0,
|
||||
)
|
||||
parent_conn.send((CROP_FRAME,
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0))
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
nparams += 1
|
||||
print ("ok")
|
||||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mycobot
|
||||
detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
|
||||
(detect.sum_y1 + detect.sum_y2) / 20.0,
|
||||
abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
|
||||
abs(detect.sum_y1 - detect.sum_y2) / 10.0)
|
||||
print("ok")
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
kp_img, desc_img = sift.detectAndCompute(frame, None)
|
||||
frame = get_frame(parent_conn)
|
||||
for i, v in enumerate(res_queue):
|
||||
# HACK: to update frame every time
|
||||
detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
|
||||
if detect_result:
|
||||
x, y = detect_result
|
||||
# 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.decide_move(real_x, real_y, detect.color)
|
||||
# if num == 5:
|
||||
# detect.color = i
|
||||
# detect.pub_marker(real_sx / 5.0 / 1000.0,
|
||||
# real_sy / 5.0 / 1000.0)
|
||||
# detect.decide_move(real_sx / 5.0, real_sy / 5.0,
|
||||
# detect.color)
|
||||
# num = real_sx = real_sy = 0
|
||||
# else:
|
||||
# num += 1
|
||||
# real_sy += real_y
|
||||
# real_sx += real_x
|
||||
parent_conn.send(CLEAR_DRAW)
|
||||
|
||||
# cv2.imshow("figure", frame)
|
||||
time.sleep(0.05)
|
||||
end_time = time.time()
|
||||
# print("loop_time = ", end_time - start_time)
|
||||
|
||||
# close the window
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
# cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
sys.exit()
|
||||
child.join()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run()
|
||||
# Object_detect().take_photo()
|
||||
# Object_detect().cut_photo()
|
||||
0
mycobot_ai/ai_mycobot_280/scripts/ai_windows.py
Normal file → Executable file
0
mycobot_ai/ai_mycobot_280/scripts/ai_windows.py
Normal file → Executable file
0
mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py
Normal file → Executable file
0
mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py
Normal file → Executable file
0
mycobot_ai/ai_mycobot_280/scripts/detect_obj_img_folder_opt.py
Normal file → Executable file
0
mycobot_ai/ai_mycobot_280/scripts/detect_obj_img_folder_opt.py
Normal file → Executable file
|
|
@ -1,14 +1,14 @@
|
|||
# -*- coding: utf-8 -*-
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from pymycobot.genre import Angle
|
||||
from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
|
||||
# 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
|
||||
import time
|
||||
|
||||
mc = MyCobot("/dev/ttyACM0", 115200)
|
||||
# mc = MyCobot("/dev/ttyUSB0", 115200)
|
||||
# mc = MyCobot("/dev/ttyAMA0", 1000000)
|
||||
|
||||
mc.send_angles([0,0,0,0,0,0], 25)
|
||||
# mc.send_angles([0,0,0,0,0,0], 25)
|
||||
# print(mc.get_angles())
|
||||
# mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0.0, -15], 30)
|
||||
# time.sleep(4)
|
||||
|
|
@ -42,3 +42,16 @@ mc.send_angles([0,0,0,0,0,0], 25)
|
|||
# print("angles:%s"% mc.get_angles())
|
||||
# print("coords:%s"% mc.get_coords())
|
||||
# print("\n")
|
||||
move_coords = [
|
||||
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket
|
||||
[232.5, -124.6, 212.8, -169.94, -5.88, -97.63], # green
|
||||
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue
|
||||
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray
|
||||
]
|
||||
|
||||
# mc.send_coords(move_coords[1],20, 1)
|
||||
# mc.send_angles([-13.44, -57.3, 0.7, -22.76, -4.74, -6.76], 20)
|
||||
mc.send_coords([238.8, -124.1, 204.3, -169.69, -5.52, -96.52], 20, 1)
|
||||
time.sleep(3)
|
||||
print(mc.get_angles())
|
||||
print(mc.get_coords())
|
||||
|
|
@ -2,8 +2,8 @@
|
|||
<arg name="port" default="/dev/ttyACM0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mycobot.rviz" />
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260_aikit.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
||||
<!-- <arg name="gui" default="false" /> -->
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
|
@ -14,7 +14,7 @@
|
|||
</node>
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<include file="$(find mypalletizer_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
|
|
|
|||
BIN
mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg
Normal file
BIN
mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.7 KiB |
BIN
mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg
Normal file
BIN
mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.1 KiB |
BIN
mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg
Normal file
BIN
mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.3 KiB |
BIN
mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg
Normal file
BIN
mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.4 KiB |
BIN
mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg
Normal file
BIN
mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.9 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 46 KiB After Width: | Height: | Size: 48 KiB |
|
|
@ -0,0 +1,486 @@
|
|||
# -*- coding:utf-8 -*-
|
||||
#!/usr/bin/env python2
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import os,sys
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
from pymycobot.mypalletizer import MyPalletizer
|
||||
from moving_utils import Movement
|
||||
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0"
|
||||
# Adaptive seeed
|
||||
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 160, camera_y = 10):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
|
||||
# declare mypal260
|
||||
self.mc = None
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[0, 0, 0, 0], # init the point
|
||||
[-29.0, 5.88, -4.92, -76.28], # point to grab
|
||||
[17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[132.6, -155.6, 211.8, -20.9], # above the red bucket
|
||||
[232.5, -134.1, 197.7, -45.26], # above the green bucket
|
||||
[111.6, 159, 221.5, -120], # above the blue bucket
|
||||
[-15.9, 164.6, 217.5, -119.35], # above the gray bucket
|
||||
]
|
||||
|
||||
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
|
||||
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
|
||||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
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:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
# self.Pin = [20, 21]
|
||||
self.Pin = [2, 5]
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 = 0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
|
||||
# set color HSV
|
||||
self.HSV = {
|
||||
"yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
|
||||
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
|
||||
"green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255]
|
||||
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
|
||||
"cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255])
|
||||
}
|
||||
|
||||
# use to calculate coord between cube and mypal260
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mypal260
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mypal260
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
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:
|
||||
# self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
else:
|
||||
# self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
# self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
# self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mypal260
|
||||
print(color)
|
||||
self.mc.send_angles(self.move_angles[0], 20)
|
||||
time.sleep(3)
|
||||
|
||||
# send coordinates to move mypal260
|
||||
self.mc.send_coords([x, y, 160, 0], 20, 0)
|
||||
time.sleep(1.5)
|
||||
self.mc.send_coords([x, y, 96, 0], 20, 0)
|
||||
time.sleep(1.5)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
self.mc.send_angle(2, 0, 20)
|
||||
time.sleep(0.3)
|
||||
self.mc.send_angle(3, -20, 20)
|
||||
time.sleep(2)
|
||||
|
||||
self.mc.send_coords(self.move_coords[color], 20, 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(3)
|
||||
|
||||
# close pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(6)
|
||||
|
||||
if color == 1:
|
||||
self.pub_marker(
|
||||
self.move_coords[color][0]/1000.0+0.04, 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[1], 20)
|
||||
time.sleep(1.5)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
print(x, y, self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mypal260
|
||||
def run(self):
|
||||
if "dev" in self.robot_m5:
|
||||
self.mc = MyPalletizer(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_wio:
|
||||
self.mc = MyPalletizer(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyPalletizer(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
print('start pump')
|
||||
self.pub_pump(False, self.Pin)
|
||||
print('end')
|
||||
self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
def draw_marker(self, img, x, y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(img, "({},{})".format(x, y), (x, y),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self, img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params
|
||||
)
|
||||
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0] == 1:
|
||||
return None
|
||||
x1 = x2 = y1 = y2 = 0
|
||||
point_11, point_21, point_31, point_41 = corners[0][0]
|
||||
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
|
||||
(point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
|
||||
point_1, point_2, point_3, point_4 = corners[1][0]
|
||||
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
|
||||
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
|
||||
return x1, x2, y1, y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mypal260
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0/ratio
|
||||
|
||||
# calculate the coords between cube and mypal260
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
# frame = frame[int(self.y2*0.2):int(self.y1*1.15),
|
||||
# int(self.x1*0.7):int(self.x2*1.15)]
|
||||
frame = frame[int(self.y2*0.78):int(self.y1*1.1),
|
||||
int(self.x1*0.86):int(self.x2*1.08)]
|
||||
return frame
|
||||
|
||||
# detect cube color
|
||||
def color_detect(self, img):
|
||||
# set the arrangement of color'HSV
|
||||
x = y = 0
|
||||
gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊
|
||||
# transfrom the img to model of gray
|
||||
hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
for mycolor, item in self.HSV.items():
|
||||
# print("mycolor:",mycolor)
|
||||
redLower = np.array(item[0])
|
||||
redUpper = np.array(item[1])
|
||||
|
||||
# wipe off all color expect color in range
|
||||
mask = cv2.inRange(hsv, item[0], item[1])
|
||||
|
||||
# a etching operation on a picture to remove edge roughness
|
||||
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
|
||||
|
||||
# the image for expansion operation, its role is to deepen the color depth in the picture
|
||||
dilation = cv2.dilate(erosion, np.ones(
|
||||
(1, 1), np.uint8), iterations=2)
|
||||
|
||||
# adds pixels to the image
|
||||
target = cv2.bitwise_and(img, img, mask=dilation)
|
||||
|
||||
# the filtered image is transformed into a binary image and placed in binary
|
||||
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
|
||||
|
||||
# get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
|
||||
contours, hierarchy = cv2.findContours(
|
||||
dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
if len(contours) > 0:
|
||||
# do something about misidentification
|
||||
boxes = [
|
||||
box
|
||||
for box in [cv2.boundingRect(c) for c in contours]
|
||||
if min(img.shape[0], img.shape[1]) / 10
|
||||
< min(box[2], box[3])
|
||||
< min(img.shape[0], img.shape[1]) / 1
|
||||
]
|
||||
if boxes:
|
||||
for box in boxes:
|
||||
x, y, w, h = box
|
||||
# find the largest object that fits the requirements
|
||||
c = max(contours, key=cv2.contourArea)
|
||||
# get the lower left and upper right points of the positioning object
|
||||
x, y, w, h = cv2.boundingRect(c)
|
||||
# locate the target by drawing rectangle
|
||||
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
|
||||
# calculate the rectangle center
|
||||
x, y = (x*2+w)/2, (y*2+h)/2
|
||||
# calculate the real coordinates of mypal260 relative to the target
|
||||
if mycolor == "red":
|
||||
self.color = 0
|
||||
|
||||
elif mycolor == "green":
|
||||
self.color = 1
|
||||
|
||||
elif mycolor == "cyan" or mycolor == "blue":
|
||||
self.color = 2
|
||||
|
||||
else:
|
||||
self.color = 3
|
||||
|
||||
|
||||
|
||||
if abs(x) + abs(y) > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
# open the camera
|
||||
cap_num = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mypal260
|
||||
detect.run()
|
||||
|
||||
_init_ = 20
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
num = 0
|
||||
real_sx = real_sy = 0
|
||||
while cv2.waitKey(1) < 0:
|
||||
# read camera
|
||||
_, frame = cap.read()
|
||||
# deal img
|
||||
frame = detect.transform_frame(frame)
|
||||
if _init_ > 0:
|
||||
_init_ -= 1
|
||||
continue
|
||||
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
init_num += 1
|
||||
continue
|
||||
elif init_num == 20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1)/20.0,
|
||||
(detect.sum_y1)/20.0,
|
||||
(detect.sum_x2)/20.0,
|
||||
(detect.sum_y2)/20.0,
|
||||
)
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mypal260
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
nparams += 1
|
||||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mypal260
|
||||
detect.set_params(
|
||||
(detect.sum_x1+detect.sum_x2)/20.0,
|
||||
(detect.sum_y1+detect.sum_y2)/20.0,
|
||||
abs(detect.sum_x1-detect.sum_x2)/10.0 +
|
||||
abs(detect.sum_y1-detect.sum_y2)/10.0
|
||||
)
|
||||
print("ok")
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
detect_result = detect.color_detect(frame)
|
||||
if detect_result is None:
|
||||
cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and mypal260
|
||||
real_x, real_y = detect.get_position(x, y)
|
||||
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
|
||||
|
||||
else:
|
||||
num += 1
|
||||
real_sy += real_y
|
||||
real_sx += real_x
|
||||
|
||||
cv2.imshow("figure", frame)
|
||||
|
||||
# close the window
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
sys.exit()
|
||||
|
|
@ -0,0 +1,628 @@
|
|||
#!/usr/bin/env python2
|
||||
# encoding:utf-8
|
||||
from multiprocessing import Process, Pipe
|
||||
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.mypalletizer import MyPalletizer
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0" # Adaptive seeed
|
||||
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 160, camera_y = 10):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
|
||||
# declare mypal260
|
||||
self.mc = None
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[0, 0, 0, 0], # init the point
|
||||
[-29.0, 5.88, -4.92, -76.28], # point to grab
|
||||
[17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[132.6, -155.6, 211.8, -20.9], # above the red bucket
|
||||
[232.5, -134.1, 197.7, -45.26], # above the green bucket
|
||||
[111.6, 159, 221.5, -120], # above the blue bucket
|
||||
[-15.9, 164.6, 217.5, -119.35], # above the gray bucket
|
||||
]
|
||||
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
|
||||
self.raspi = False
|
||||
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
|
||||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
if "dev" in self.robot_m5:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
self.Pin = [2, 5]
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 = 0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
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
|
||||
|
||||
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:
|
||||
self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
else:
|
||||
self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
# self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
# self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
print(color)
|
||||
# send Angle to move mypal260
|
||||
self.mc.send_angles(self.move_angles[0], 20)
|
||||
time.sleep(3)
|
||||
|
||||
# send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度
|
||||
self.mc.send_coords([x, y, 160, 0], 20, 0)
|
||||
time.sleep(1.5)
|
||||
self.mc.send_coords([x, y, 96, 0], 20, 0)
|
||||
time.sleep(1.5)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
self.mc.send_angle(2, 0, 20)
|
||||
time.sleep(0.3)
|
||||
self.mc.send_angle(3, -18, 20)
|
||||
time.sleep(2)
|
||||
|
||||
self.mc.send_coords(self.move_coords[color], 20, 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(3)
|
||||
|
||||
# close pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(6)
|
||||
|
||||
self.mc.send_angles(self.move_angles[1], 20)
|
||||
time.sleep(1.5)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
print(x, y, self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mypal260
|
||||
def run(self):
|
||||
if "dev" in self.robot_m5:
|
||||
self.mc = MyPalletizer(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_wio:
|
||||
self.mc = MyPalletizer(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyPalletizer(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
def draw_marker(self, img, x, y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(
|
||||
img,
|
||||
"({},{})".format(x, y),
|
||||
(x, y),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL,
|
||||
1,
|
||||
(243, 0, 0),
|
||||
2,
|
||||
)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self, img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params)
|
||||
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0] == 1:
|
||||
return None
|
||||
x1 = x2 = y1 = y2 = 0
|
||||
point_11, point_21, point_31, point_41 = corners[0][0]
|
||||
x1, y1 = int(
|
||||
(point_11[0] + point_21[0] + point_31[0] + point_41[0]) /
|
||||
4.0), int(
|
||||
(point_11[1] + point_21[1] + point_31[1] + point_41[1])
|
||||
/ 4.0)
|
||||
point_1, point_2, point_3, point_4 = corners[1][0]
|
||||
x2, y2 = int(
|
||||
(point_1[0] + point_2[0] + point_3[0] + point_4[0]) /
|
||||
4.0), int(
|
||||
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) /
|
||||
4.0)
|
||||
return x1, x2, y1, y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0 / ratio
|
||||
|
||||
# calculate the coords between cube and mycobot
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y) * self.ratio +
|
||||
self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15),
|
||||
int(self.x1 * 0.7):int(self.x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
# according the class_id to get object name
|
||||
def id_class_name(self, class_id):
|
||||
for key, value in self.labels.items():
|
||||
if class_id == int(key):
|
||||
return value
|
||||
|
||||
# detect object
|
||||
def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection):
|
||||
i = 0
|
||||
MIN_MATCH_COUNT = 5
|
||||
# sift = cv2.xfeatures2d.SIFT_create()
|
||||
|
||||
# find the keypoints and descriptors with SIFT
|
||||
# kp = []
|
||||
# des = []
|
||||
kp = kp_list
|
||||
des = desc_list
|
||||
|
||||
# for i in goal:
|
||||
# kp0, des0 = sift.detectAndCompute(i, None)
|
||||
# kp.append(kp0)
|
||||
# des.append(des0)
|
||||
|
||||
# kp1, des1 = sift.detectAndCompute(goal, None)
|
||||
# kp2, des2 = sift.detectAndCompute(img, None)
|
||||
kp2, des2 = kp_img, desc_img
|
||||
|
||||
# FLANN parameters
|
||||
FLANN_INDEX_KDTREE = 0
|
||||
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
|
||||
search_params = dict(checks=50) # or pass empty dictionary
|
||||
flann = cv2.FlannBasedMatcher(index_params, search_params)
|
||||
|
||||
x, y = 0, 0
|
||||
try:
|
||||
for i in range(len(des)):
|
||||
matches = flann.knnMatch(des[i], des2, k=2)
|
||||
# store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。
|
||||
good = []
|
||||
for m, n in matches:
|
||||
if m.distance < 0.7 * n.distance:
|
||||
good.append(m)
|
||||
|
||||
# When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
|
||||
if len(good) > MIN_MATCH_COUNT:
|
||||
|
||||
# extract corresponding point pairs from matching 从匹配中提取出对应点对
|
||||
# query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引
|
||||
src_pts = np.float32([kp[i][m.queryIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
dst_pts = np.float32([kp2[m.trainIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
|
||||
# Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
|
||||
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,
|
||||
5.0)
|
||||
matchesMask = mask.ravel().tolist()
|
||||
# Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
|
||||
h, w, d = goal[i].shape
|
||||
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
|
||||
[w - 1, 0]]).reshape(-1, 1, 2)
|
||||
dst = cv2.perspectiveTransform(pts, M)
|
||||
coord = (dst[0][0] + dst[1][0] + dst[2][0] +
|
||||
dst[3][0]) / 4.0
|
||||
connection.send((DRAW_COORDS, coord))
|
||||
# cv2.putText(img, "{}".format(coord), (50, 60),
|
||||
# fontFace=None, fontScale=1,
|
||||
# color=(0, 255, 0), lineType=1)
|
||||
print(format(dst[0][0][0]))
|
||||
x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] +
|
||||
dst[3][0][0]) / 4.0
|
||||
y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] +
|
||||
dst[3][0][1]) / 4.0
|
||||
|
||||
# bound box 绘制边框
|
||||
# img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
connection.send((DRAW_RECT, dst))
|
||||
# cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA)
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
if x + y > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
# The path to save the image folder
|
||||
def parse_folder(folder):
|
||||
restore = []
|
||||
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder
|
||||
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder
|
||||
|
||||
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:
|
||||
restore.append(cv2.imread(folder + '/{}'.format(l)))
|
||||
return restore
|
||||
|
||||
def compute_keypoints_and_descriptors(sift, images_lists):
|
||||
kp_list = []
|
||||
desc_list = []
|
||||
for images in images_lists:
|
||||
kp_tmp = []
|
||||
desc_tmp = []
|
||||
for img in images:
|
||||
kp, desc = sift.detectAndCompute(img, None)
|
||||
kp_tmp.append(kp)
|
||||
desc_tmp.append(desc)
|
||||
kp_list.append(kp_tmp)
|
||||
desc_list.append(desc_tmp)
|
||||
|
||||
return kp_list, desc_list
|
||||
|
||||
GET_FRAME = 1
|
||||
STOP_PROCESSING = 2
|
||||
DRAW_COORDS = 3
|
||||
DRAW_RECT = 4
|
||||
CLEAR_DRAW = 5
|
||||
CROP_FRAME = 6
|
||||
|
||||
def get_frame(connection):
|
||||
connection.send(GET_FRAME)
|
||||
frame = connection.recv()
|
||||
return frame
|
||||
|
||||
def process_transform_frame(frame, x1, y1, x2, y2):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
# if x1 != x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
# frame = frame[int(y2 * 0.2):int(y1 * 1.15),
|
||||
# int(x1 * 0.7):int(x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
def process_display_frame(connection):
|
||||
cap_num = 0
|
||||
coord = None
|
||||
dst = None
|
||||
x1 = 0
|
||||
y1 = 0
|
||||
x2 = 0
|
||||
y2 = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
while cv2.waitKey(1) < 0:
|
||||
_, frame = cap.read()
|
||||
frame = process_transform_frame(frame, x1, y1, x2, y2)
|
||||
if connection.poll():
|
||||
request = connection.recv()
|
||||
if request == GET_FRAME:
|
||||
connection.send(frame)
|
||||
elif request == CLEAR_DRAW:
|
||||
coord = None
|
||||
dst = None
|
||||
elif type(request) is tuple:
|
||||
if request[0] == DRAW_COORDS:
|
||||
coord = request[1]
|
||||
elif request[0] == DRAW_RECT:
|
||||
dst = request[1]
|
||||
elif request[0] == CROP_FRAME:
|
||||
x1 = request[1]
|
||||
y1 = request[2]
|
||||
x2 = request[3]
|
||||
y2 = request[4]
|
||||
|
||||
if not coord is None:
|
||||
cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
|
||||
fontScale=1, color=(0, 255, 0), lineType=1)
|
||||
if not dst is None:
|
||||
frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
cv2.imshow("figure", frame)
|
||||
time.sleep(0.04)
|
||||
connection.send(STOP_PROCESSING)
|
||||
|
||||
def run():
|
||||
parent_conn, child_conn = Pipe()
|
||||
child = Process(target = process_display_frame, args=(child_conn,))
|
||||
child.start()
|
||||
|
||||
res_queue = [[], [], [], []]
|
||||
res_queue[0] = parse_folder('res/red')
|
||||
res_queue[1] = parse_folder('res/green')
|
||||
res_queue[2] = parse_folder('res/blue')
|
||||
res_queue[3] = parse_folder('res/gray')
|
||||
|
||||
sift = cv2.xfeatures2d.SIFT_create()
|
||||
kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
|
||||
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
|
||||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
# _init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
# num = 0
|
||||
# real_sx = real_sy = 0
|
||||
while True:
|
||||
start_time = time.time()
|
||||
if parent_conn.poll():
|
||||
data = parent_conn.recv()
|
||||
if data == STOP_PROCESSING:
|
||||
break
|
||||
# read camera
|
||||
frame = get_frame(parent_conn)
|
||||
# deal img
|
||||
#frame = detect.transform_frame(frame)
|
||||
|
||||
# if _init_ > 0:
|
||||
# _init_ -= 1
|
||||
# continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
init_num += 1
|
||||
continue
|
||||
elif init_num == 20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0,
|
||||
)
|
||||
parent_conn.send((CROP_FRAME,
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0))
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
nparams += 1
|
||||
print ("ok")
|
||||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mycobot
|
||||
detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
|
||||
(detect.sum_y1 + detect.sum_y2) / 20.0,
|
||||
abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
|
||||
abs(detect.sum_y1 - detect.sum_y2) / 10.0)
|
||||
print("ok")
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
kp_img, desc_img = sift.detectAndCompute(frame, None)
|
||||
frame = get_frame(parent_conn)
|
||||
for i, v in enumerate(res_queue):
|
||||
# HACK: to update frame every time
|
||||
detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
|
||||
if detect_result:
|
||||
x, y = detect_result
|
||||
# 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.decide_move(real_x, real_y, detect.color)
|
||||
# if num == 5:
|
||||
# detect.color = i
|
||||
# detect.pub_marker(real_sx / 5.0 / 1000.0,
|
||||
# real_sy / 5.0 / 1000.0)
|
||||
# detect.decide_move(real_sx / 5.0, real_sy / 5.0,
|
||||
# detect.color)
|
||||
# num = real_sx = real_sy = 0
|
||||
# else:
|
||||
# num += 1
|
||||
# real_sy += real_y
|
||||
# real_sx += real_x
|
||||
parent_conn.send(CLEAR_DRAW)
|
||||
|
||||
# cv2.imshow("figure", frame)
|
||||
time.sleep(0.05)
|
||||
end_time = time.time()
|
||||
# print("loop_time = ", end_time - start_time)
|
||||
|
||||
# close the window
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
# cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
sys.exit()
|
||||
|
||||
child.join()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run()
|
||||
# Object_detect().take_photo()
|
||||
# Object_detect().cut_photo()
|
||||
|
|
@ -4,7 +4,7 @@ from pymycobot.genre import Angle
|
|||
from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
|
||||
import time,os
|
||||
|
||||
mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200)
|
||||
# mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200)
|
||||
|
||||
# mc = MyPalletizer("/dev/ttyAMA0", 1000000)
|
||||
# mc.send_angles([-29.0, 5.88, -4.92, -76.28],25) # init the point coords:[155.3, -86.1, 218.4, -47.28]
|
||||
|
|
@ -13,8 +13,8 @@ mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200)
|
|||
# mc.send_angles([-47.1, 10.19, -10.1, -76.37],25) # above the red bucket; coords:[127.3, -137.1, 219.2, -29.26]
|
||||
# time.sleep(1.5)
|
||||
|
||||
mc.send_angles([0,0,-15,0],25)
|
||||
time.sleep(2)
|
||||
# mc.send_angles([0,0,-15,0],25)
|
||||
# time.sleep(2)
|
||||
|
||||
# mc.send_coords([141.2, -142.0, 206.2, -26.8],25,1) # above the red bucket
|
||||
# time.sleep(2)
|
||||
|
|
@ -38,4 +38,27 @@ time.sleep(2)
|
|||
# mc.set_servo_calibration(1)
|
||||
# mc.set_servo_calibration(2)
|
||||
# mc.set_servo_calibration(3)
|
||||
# mc.set_servo_calibration(4)
|
||||
# mc.set_servo_calibration(4)
|
||||
|
||||
import sys
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
print("Please enter blue:")
|
||||
blue = input()
|
||||
print("Please enter green:")
|
||||
green = input()
|
||||
print("Please enter red:")
|
||||
red = input()
|
||||
|
||||
color = np.uint8([[[blue, green, red]]])
|
||||
hsv_color = cv2.cvtColor(color, cv2.COLOR_BGR2HSV)
|
||||
|
||||
hue = hsv_color[0][0][0]
|
||||
|
||||
print("Lower bound is :")
|
||||
print("[" + str(hue - 10) + ", 100, 100]\n")
|
||||
|
||||
print("Upper bound is :"),
|
||||
print("[" + str(hue + 10) + ", 255, 255]")
|
||||
|
||||
|
|
|
|||
|
|
@ -8,8 +8,9 @@ Panels:
|
|||
- /Status1
|
||||
- /RobotModel1
|
||||
- /TF1
|
||||
- /Marker1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 601
|
||||
Tree Height: 607
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
|
|
@ -18,7 +19,7 @@ Panels:
|
|||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
Splitter Ratio: 0.588679016
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
|
|
@ -29,8 +30,6 @@ Panels:
|
|||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
|
|
@ -42,7 +41,7 @@ Visualization Manager:
|
|||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Line Width: 0.0299999993
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
|
|
@ -69,6 +68,11 @@ Visualization Manager:
|
|||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
env:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
|
|
@ -107,6 +111,8 @@ Visualization Manager:
|
|||
All Enabled: true
|
||||
base:
|
||||
Value: true
|
||||
env:
|
||||
Value: true
|
||||
link1:
|
||||
Value: true
|
||||
link2:
|
||||
|
|
@ -117,13 +123,15 @@ Visualization Manager:
|
|||
Value: true
|
||||
link5:
|
||||
Value: true
|
||||
Marker Scale: 0.30000001192092896
|
||||
Marker Scale: 0.300000012
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
base:
|
||||
env:
|
||||
{}
|
||||
link1:
|
||||
link2:
|
||||
link3:
|
||||
|
|
@ -132,6 +140,14 @@ Visualization Manager:
|
|||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /cube
|
||||
Name: Marker
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
|
|
@ -147,10 +163,7 @@ Visualization Manager:
|
|||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
|
|
@ -160,33 +173,33 @@ Visualization Manager:
|
|||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 1.2028908729553223
|
||||
Distance: 1.20289087
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0.10758385062217712
|
||||
Z: 0.107583851
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Focal Shape Size: 0.0500000007
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.42039862275123596
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 0.420398623
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 1.1754094362258911
|
||||
Yaw: 1.17540944
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 892
|
||||
Height: 888
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002eefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002ee000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002eefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002ee000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002ee00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
|
|
@ -198,201 +211,3 @@ Window Geometry:
|
|||
Width: 1568
|
||||
X: 144
|
||||
Y: 41
|
||||
|
||||
# Panels:
|
||||
# - Class: rviz/Displays
|
||||
# Help Height: 78
|
||||
# Name: Displays
|
||||
# Property Tree Widget:
|
||||
# Expanded:
|
||||
# - /Global Options1
|
||||
# - /Status1
|
||||
# - /RobotModel1
|
||||
# - /TF1
|
||||
# Splitter Ratio: 0.5
|
||||
# Tree Height: 775
|
||||
# - Class: rviz/Selection
|
||||
# Name: Selection
|
||||
# - Class: rviz/Tool Properties
|
||||
# Expanded:
|
||||
# - /2D Pose Estimate1
|
||||
# - /2D Nav Goal1
|
||||
# - /Publish Point1
|
||||
# Name: Tool Properties
|
||||
# Splitter Ratio: 0.588679016
|
||||
# - Class: rviz/Views
|
||||
# Expanded:
|
||||
# - /Current View1
|
||||
# Name: Views
|
||||
# Splitter Ratio: 0.5
|
||||
# - Class: rviz/Time
|
||||
# Experimental: false
|
||||
# Name: Time
|
||||
# SyncMode: 0
|
||||
# SyncSource: ""
|
||||
# Toolbars:
|
||||
# toolButtonStyle: 2
|
||||
# Visualization Manager:
|
||||
# Class: ""
|
||||
# Displays:
|
||||
# - Alpha: 0.5
|
||||
# Cell Size: 1
|
||||
# Class: rviz/Grid
|
||||
# Color: 160; 160; 164
|
||||
# Enabled: true
|
||||
# Line Style:
|
||||
# Line Width: 0.0299999993
|
||||
# Value: Lines
|
||||
# Name: Grid
|
||||
# Normal Cell Count: 0
|
||||
# Offset:
|
||||
# X: 0
|
||||
# Y: 0
|
||||
# Z: 0
|
||||
# Plane: XY
|
||||
# Plane Cell Count: 10
|
||||
# Reference Frame: <Fixed Frame>
|
||||
# Value: true
|
||||
# - Alpha: 1
|
||||
# Class: rviz/RobotModel
|
||||
# Collision Enabled: false
|
||||
# Enabled: true
|
||||
# Links:
|
||||
# All Links Enabled: true
|
||||
# Expand link Details: false
|
||||
# Expand Link Details: false
|
||||
# Expand Tree: false
|
||||
# Link Tree Style: Links in Alphabetic Order
|
||||
# base:
|
||||
# Alpha: 1
|
||||
# Show Axes: false
|
||||
# Show Trail: false
|
||||
# Value: true
|
||||
# link1:
|
||||
# Alpha: 1
|
||||
# Show Axes: false
|
||||
# Show Trail: false
|
||||
# Value: true
|
||||
# link2:
|
||||
# Alpha: 1
|
||||
# Show Axes: false
|
||||
# Show Trail: false
|
||||
# Value: true
|
||||
# link3:
|
||||
# Alpha: 1
|
||||
# Show Axes: false
|
||||
# Show Trail: false
|
||||
# Value: true
|
||||
# link4:
|
||||
# Alpha: 1
|
||||
# Show Axes: false
|
||||
# Show Trail: false
|
||||
# Value: true
|
||||
# link5:
|
||||
# Alpha: 1
|
||||
# Show Axes: false
|
||||
# Show Trail: false
|
||||
# Value: true
|
||||
|
||||
# Name: RobotModel
|
||||
# Robot Description: robot_description
|
||||
# TF Prefix: ""
|
||||
# Update Interval: 0
|
||||
# Value: true
|
||||
# Visual Enabled: true
|
||||
# - Class: rviz/TF
|
||||
# Enabled: true
|
||||
# Frame Timeout: 15
|
||||
# Frames:
|
||||
# All Enabled: true
|
||||
# base:
|
||||
# Value: true
|
||||
# link1:
|
||||
# Value: true
|
||||
# link2:
|
||||
# Value: true
|
||||
# link3:
|
||||
# Value: true
|
||||
# link4:
|
||||
# Value: true
|
||||
# link5:
|
||||
# Value: true
|
||||
|
||||
# Marker Scale: 0.300000012
|
||||
# Name: TF
|
||||
# Show Arrows: true
|
||||
# Show Axes: true
|
||||
# Show Names: true
|
||||
# Tree:
|
||||
# base:
|
||||
# link1:
|
||||
# link2:
|
||||
# link3:
|
||||
# link4:
|
||||
# link5:
|
||||
# {}
|
||||
# Update Interval: 0
|
||||
# Value: true
|
||||
# Enabled: true
|
||||
# Global Options:
|
||||
# Background Color: 48; 48; 48
|
||||
# Default Light: true
|
||||
# Fixed Frame: base
|
||||
# Frame Rate: 30
|
||||
# Name: root
|
||||
# Tools:
|
||||
# - Class: rviz/Interact
|
||||
# Hide Inactive Objects: true
|
||||
# - Class: rviz/MoveCamera
|
||||
# - Class: rviz/Select
|
||||
# - Class: rviz/FocusCamera
|
||||
# - Class: rviz/Measure
|
||||
# - Class: rviz/SetInitialPose
|
||||
# Topic: /initialpose
|
||||
# - Class: rviz/SetGoal
|
||||
# Topic: /move_base_simple/goal
|
||||
# - Class: rviz/PublishPoint
|
||||
# Single click: true
|
||||
# Topic: /clicked_point
|
||||
# Value: true
|
||||
# Views:
|
||||
# Current:
|
||||
# Class: rviz/Orbit
|
||||
# Distance: 1.20289087
|
||||
# Enable Stereo Rendering:
|
||||
# Stereo Eye Separation: 0.0599999987
|
||||
# Stereo Focal Distance: 1
|
||||
# Swap Stereo Eyes: false
|
||||
# Value: false
|
||||
# Focal Point:
|
||||
# X: 0
|
||||
# Y: 0
|
||||
# Z: 0.107583851
|
||||
# Focal Shape Fixed Size: true
|
||||
# Focal Shape Size: 0.0500000007
|
||||
# Invert Z Axis: false
|
||||
# Name: Current View
|
||||
# Near Clip Distance: 0.00999999978
|
||||
# Pitch: 0.440398335
|
||||
# Target Frame: <Fixed Frame>
|
||||
# Value: Orbit (rviz)
|
||||
# Yaw: 0.430389732
|
||||
# Saved: ~
|
||||
# Window Geometry:
|
||||
# Displays:
|
||||
# collapsed: false
|
||||
# Height: 1056
|
||||
# Hide Left Dock: false
|
||||
# Hide Right Dock: false
|
||||
# QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
# Selection:
|
||||
# collapsed: false
|
||||
# Time:
|
||||
# collapsed: false
|
||||
# Tool Properties:
|
||||
# collapsed: false
|
||||
# Views:
|
||||
# collapsed: false
|
||||
# Width: 1855
|
||||
# X: 65
|
||||
# Y: 24
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
<launch>
|
||||
<arg name="port" default="/dev/ttyUSB1" />
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service -->
|
||||
|
|
|
|||
|
|
@ -67,11 +67,17 @@ class MypalTopics(object):
|
|||
|
||||
rospy.init_node("mypal_topics")
|
||||
rospy.loginfo("start ...")
|
||||
port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
|
||||
print(port)
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
self.mc = MyPalletizer(port, baud)
|
||||
port_m5 = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
|
||||
# if not port:
|
||||
port_wio = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
|
||||
if "dev" in port_m5:
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
rospy.loginfo("%s,%s" % (port_m5, baud))
|
||||
self.mc = MyPalletizer(port_m5, baud)
|
||||
elif "dev" in port_wio:
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
rospy.loginfo("%s,%s" % (port_wio, baud))
|
||||
self.mc = MyPalletizer(port_wio, baud)
|
||||
self.lock = threading.Lock()
|
||||
|
||||
def start(self):
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue