mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
248 lines
8.4 KiB
Python
Executable file
248 lines
8.4 KiB
Python
Executable file
# encoding: UTF-8
|
|
#!/usr/bin/env python2
|
|
import cv2 as cv
|
|
import os
|
|
import numpy as np
|
|
import time
|
|
import rospy
|
|
from visualization_msgs.msg import Marker
|
|
from moving_utils import Movement
|
|
|
|
# y轴偏移量
|
|
pump_y = -55
|
|
# x轴偏移量
|
|
pump_x = 15
|
|
|
|
|
|
class Detect_marker(Movement):
|
|
def __init__(self):
|
|
super(Detect_marker, self).__init__()
|
|
# set cache of real coord
|
|
self.cache_x = self.cache_y = 0
|
|
|
|
# which robot
|
|
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]
|
|
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
|
|
self.GPIO = GPIO
|
|
GPIO.setwarnings(False)
|
|
GPIO.setmode(GPIO.BCM)
|
|
GPIO.setup(20, GPIO.OUT)
|
|
GPIO.setup(21, GPIO.OUT)
|
|
self.raspi = True
|
|
if self.raspi:
|
|
self.gpio_status(False)
|
|
else:
|
|
self.pub_pump(False, self.Pin)
|
|
# Creating a Camera Object
|
|
cap_num = 0
|
|
self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L)
|
|
# Get ArUco marker dict that can be detected.
|
|
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
|
# Get ArUco marker params.
|
|
self.aruco_params = cv.aruco.DetectorParameters_create()
|
|
self.calibrationParams = cv.FileStorage(
|
|
"calibrationFileName.xml", cv.FILE_STORAGE_READ)
|
|
# Get distance coefficient.
|
|
self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat()
|
|
|
|
height = self.cap.get(4)
|
|
focal_length = width = self.cap.get(3)
|
|
center = [width / 2, height / 2]
|
|
# Calculate the camera matrix.
|
|
self.camera_matrix = np.array(
|
|
[
|
|
[focal_length, 0, center[0]],
|
|
[0, focal_length, center[1]],
|
|
[0, 0, 1],
|
|
],
|
|
dtype=np.float32,
|
|
)
|
|
# init a node and a publisher
|
|
rospy.init_node("encode_marker", anonymous=True)
|
|
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
|
|
|
self.marker = Marker()
|
|
self.marker.header.frame_id = "/joint1"
|
|
self.marker.ns = "cube"
|
|
self.marker.type = self.marker.CUBE
|
|
self.marker.action = self.marker.ADD
|
|
self.marker.scale.x = 0.04
|
|
self.marker.scale.y = 0.04
|
|
self.marker.scale.z = 0.04
|
|
self.marker.color.a = 1
|
|
self.marker.color.r = 0.3
|
|
self.marker.color.g = 0.3
|
|
self.marker.color.b = 0.3
|
|
|
|
# marker position initial
|
|
self.marker.pose.position.x = 0
|
|
self.marker.pose.position.y = 0
|
|
self.marker.pose.position.z = 0.03
|
|
self.marker.pose.orientation.x = 0
|
|
self.marker.pose.orientation.y = 0
|
|
self.marker.pose.orientation.z = 0
|
|
self.marker.pose.orientation.w = 1.0
|
|
|
|
# Grasping motion
|
|
def move(self, x, y):
|
|
if self.raspi:
|
|
coords = [
|
|
[145.6, -64.9, 285.2, 179.88, 7.67, 179],
|
|
[130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
|
|
]
|
|
else:
|
|
coords = [
|
|
[135.0, -65.5, 280.1, 178.99, 5.38, -179.9],
|
|
[136.1, -141.6, 243.9, 178.99, 5.38, -179.9]
|
|
]
|
|
|
|
# publish marker
|
|
self.marker.header.stamp = rospy.Time.now()
|
|
self.marker.pose.position.x = (coords[0][0]-x)/1000.0
|
|
self.marker.pose.position.y = (coords[0][1]-y)/1000.0
|
|
self.pub.publish(self.marker)
|
|
|
|
# send coordinates to move mycobot
|
|
self.pub_coords(coords[0], 30, 1)
|
|
time.sleep(2)
|
|
self.pub_coords([coords[0][0]-x, coords[0][1]-y,
|
|
240, 178.99, 5.38, -179.9], 25, 1)
|
|
time.sleep(2)
|
|
self.pub_coords([coords[0][0]-x, coords[0][1]-y,
|
|
200, 178.99, 5.38, -179.9], 25, 1)
|
|
time.sleep(2)
|
|
if "dev" in self.robot_m5 or self.raspi:
|
|
self.pub_coords([coords[0][0]-x, coords[0][1]-y,
|
|
90, 178.99, 5.38, -179.9], 25, 1)
|
|
elif "dev" in self.robot_wio:
|
|
self.pub_coords([coords[0][0]-x+20, coords[0][1] -
|
|
y-10, 70, 178.99, 5.38, -179.9], 25, 1)
|
|
time.sleep(2)
|
|
if self.raspi:
|
|
self.gpio_status(True)
|
|
else:
|
|
self.pub_pump(True, self.Pin)
|
|
time.sleep(1)
|
|
self.pub_coords(coords[0], 30, 1)
|
|
time.sleep(3)
|
|
self.pub_coords(coords[1], 30, 1)
|
|
time.sleep(2)
|
|
if self.raspi:
|
|
self.gpio_status(False)
|
|
else:
|
|
self.pub_pump(False, self.Pin)
|
|
# publish marker
|
|
time.sleep(1)
|
|
self.marker.header.stamp = rospy.Time.now()
|
|
self.marker.pose.position.x = coords[1][0]/1000.0
|
|
self.marker.pose.position.y = coords[1][1]/1000.0
|
|
self.pub.publish(self.marker)
|
|
|
|
self.pub_coords(coords[0], 30, 1)
|
|
time.sleep(2)
|
|
|
|
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)
|
|
|
|
# decide whether grab cube
|
|
def decide_move(self, x, y):
|
|
|
|
print(x, 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
|
|
if "dev" in self.robot_jes:
|
|
if x > -20:
|
|
y += 10
|
|
if y > -25:
|
|
x -= 5
|
|
x += 10
|
|
|
|
self.move(x, y)
|
|
|
|
# init mycobot
|
|
def init_mycobot(self):
|
|
|
|
for _ in range(5):
|
|
print _
|
|
self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1)
|
|
time.sleep(0.5)
|
|
|
|
def run(self):
|
|
global pump_y, pump_x
|
|
self.init_mycobot()
|
|
num = sum_x = sum_y = 0
|
|
while cv.waitKey(1) < 0:
|
|
success, img = self.cap.read()
|
|
if not success:
|
|
print("It seems that the image cannot be acquired correctly.")
|
|
break
|
|
|
|
# transfrom the img to model of gray
|
|
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
|
# Detect ArUco marker.
|
|
corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
|
|
gray, self.aruco_dict, parameters=self.aruco_params
|
|
)
|
|
|
|
if len(corners) > 0:
|
|
if ids is not None:
|
|
# get informations of aruco
|
|
ret = cv.aruco.estimatePoseSingleMarkers(
|
|
corners, 0.03, self.camera_matrix, self.dist_coeffs
|
|
)
|
|
# rvec:rotation offset,tvec:translation deviator
|
|
(rvec, tvec) = (ret[0], ret[1])
|
|
(rvec - tvec).any()
|
|
xyz = tvec[0, 0, :]
|
|
# calculate the coordinates of the aruco relative to the pump
|
|
xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]
|
|
* 1000+pump_x, 2), round(xyz[2]*1000, 2)]
|
|
|
|
for i in range(rvec.shape[0]):
|
|
# draw the aruco on img
|
|
cv.aruco.drawDetectedMarkers(img, corners)
|
|
cv.aruco.drawAxis(
|
|
img,
|
|
self.camera_matrix,
|
|
self.dist_coeffs,
|
|
rvec[i, :, :],
|
|
tvec[i, :, :],
|
|
0.03,
|
|
)
|
|
|
|
if num < 40:
|
|
if self.raspi:
|
|
sum_x -= 30
|
|
sum_x += xyz[1]
|
|
sum_y += xyz[0]
|
|
num += 1
|
|
elif num == 40:
|
|
self.decide_move(sum_x/40.0, sum_y/40.0)
|
|
num = sum_x = sum_y = 0
|
|
|
|
cv.imshow("encode_image", img)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
detect = Detect_marker()
|
|
detect.run()
|