kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

optimize

Browse source
This commit is contained in:
2929ss 2022-07-05 18:28:40 +08:00
parent 9a8146b51a
commit 1d9f71c297
12 changed files with 1415 additions and 86 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
mycobot_ai/myPalletizer_260/launch/vision_m5.launch
View file

@ -1,5 +1,5 @@
<launch>
<arg name="port" default="/dev/ttyUSB0" />
<arg name="port" default="/dev/ttyUSB1" />
<arg name="baud" default="115200" />
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260_aikit.urdf"/>

476
mycobot_ai/myPalletizer_260/scripts/combine_detect_obj_color.py Normal file
View file

@ -0,0 +1,476 @@
# -*- coding:utf-8 -*-
#!/usr/bin/env python2
from operator import imod
from tokenize import Pointfloat
import cv2
import numpy as np
import time
import json
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 = [
[141.2, -142.0, 210, -26.8], # above the red bucket
[234.3, -120, 210, -48.77], # above the green bucket
[100.9, 159.3, 248.6, -124.27], # above the blue bucket
[-17.6, 161.6, 238.4, -152.31], # 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]
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, 46]), np.array([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])],
}
# 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, 90, 0], 20, 0)
time.sleep(1.5)
# open pump
if "dev" in self.robot_m5:
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, -15, 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:
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_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 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)]
return frame
# detect cube color
def color_detect(self, img):
# set the arrangement of color'HSV
x = y = 0
for mycolor, item in self.HSV.items():
# print("mycolor:",mycolor)
redLower = np.array(item[0])
redUpper = np.array(item[1])
# transfrom the img to model of gray
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# print("hsv",hsv)
# 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":
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()

667
mycobot_ai/myPalletizer_260/scripts/combine_detect_obj_img_folder_opt.py Executable file
View file

@ -0,0 +1,667 @@
#!/usr/bin/env python2
# encoding:utf-8
from multiprocessing import Process, Pipe
from cgi import parse
from difflib import restore
# import queue
from sys import path
from tokenize import Pointfloat
from turtle import color
# from typing_extensions import Self
import cv2
import numpy as np
import time
import json
import os,sys
import rospy
from visualization_msgs.msg import Marker
from PIL import Image
from threading import Thread
import tkFileDialog as filedialog
import Tkinter as tk
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 = [
[141.2, -142.0, 210, -26.8], # above the red bucket
[234.3, -120, 210, -48.77], # above the green bucket
[100.9, 159.3, 248.6, -124.27], # above the blue bucket
[-17.6, 161.6, 238.4, -152.31], # above the gray bucket
]
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
self.raspi = False
# 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.gpio_status(False)
# self.Pin = [2, 5]
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]
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)
# 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
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, 90, 0], 20, 0)
time.sleep(1.5)
# open pump
if "dev" in self.robot_m5:
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, -15, 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:
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_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/myPalletizer_260/' + folder
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_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()
# 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()

18
mycobot_ai/myPalletizer_260/scripts/detect_obj_color.py → mycobot_ai/myPalletizer_260/scripts/pi_detect_obj_color.py Executable file → Normal file
View file

@ -8,7 +8,7 @@ import time
import json
import os,sys
import rospy
from visualization_msgs.msg import Marker
from visualization_msgs.msg import Marker
from pymycobot.mypalletizer import MyPalletizer
from moving_utils import Movement
@ -147,7 +147,7 @@ class Object_detect(Movement):
time.sleep(3)
# send coordinates to move mypal260
self.mc.send_coords([x, y, 160, 0], 20, 0) # -82.17
self.mc.send_coords([x, y, 160, 0], 20, 0)
time.sleep(1.5)
self.mc.send_coords([x, y, 90, 0], 20, 0)
time.sleep(1.5)
@ -167,14 +167,14 @@ class Object_detect(Movement):
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(4.5)
time.sleep(3)
# close pump
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
time.sleep(4)
time.sleep(6)
if color == 1:
self.pub_marker(
@ -183,6 +183,7 @@ class Object_detect(Movement):
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[1], 20)
time.sleep(1.5)
@ -200,13 +201,10 @@ class Object_detect(Movement):
# init mypal260
def run(self):
if "dev" in self.robot_m5:
self.mc = MyPalletizer(self.robot_m5,115200)
if "dev" in self.robot_raspi:
self.mc = MyPalletizer(self.robot_raspi,1000000)
self.mc = MyPalletizer("/dev/ttyAMA0",1000000) # ok
if not self.raspi:
self.pub_pump(False, self.Pin)
self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20)
self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) # ok
time.sleep(3)
# draw aruco

36
mycobot_ai/myPalletizer_260/scripts/detect_obj_img_folder_opt.py → mycobot_ai/myPalletizer_260/scripts/pi_detect_obj_img_folder_opt.py
View file

@ -1,6 +1,8 @@
# encoding:utf-8
#!/usr/bin/env python2
from multiprocessing import Process, Pipe
from cgi import parse
from difflib import restore
# import queue
@ -28,7 +30,7 @@ __version__ = "1.0" # Adaptive seeed
class Object_detect(Movement):
def __init__(self, camera_x= 160, camera_y = 10):
def __init__(self, camera_x = 160, camera_y = 10):
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
@ -142,34 +144,26 @@ class Object_detect(Movement):
# Grasping motion
def move(self, x, y, color):
# send Angle to move mycobot
# send Angle to move mypal260
self.mc.send_angles(self.move_angles[0], 20)
# self.pub_angles(self.move_angles[0], 20)
# time.sleep(1.5)
# self.pub_angles(self.move_angles[1], 20)
# time.sleep(1.5)
# self.pub_angles(self.move_angles[2], 20)
# time.sleep(1.5)
time.sleep(3)
# send coordinates to move mycobot 根据不同底板机械臂，调整吸泵高度
# self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1)
self.mc.send_coords([x, y, 160, -82.17], 20, 2)
time.sleep(2.5)
# self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1)
self.mc.send_coords([x, y, 90, -82.17], 20, 1)
# 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, 90, 0], 20, 0)
time.sleep(1.5)
# open pump
self.gpio_status(True)
time.sleep(0.5)
time.sleep(1.5)
self.mc.send_angle(2, 0, 20)
time.sleep(0.3)
self.mc.send_angle(3, -18, 20)
self.mc.send_angle(3, -15, 20)
time.sleep(2)
print(self.move_coords[color])
# self.pub_coords(self.move_coords[color], 20, 1)
self.mc.send_coords(self.move_coords[color], 20, 1)
self.pub_marker(self.move_coords[color][0] / 1000.0,
@ -179,10 +173,10 @@ class Object_detect(Movement):
# close pump
self.gpio_status(False)
time.sleep(4)
time.sleep(3)
self.mc.send_angles(self.move_angles[1], 20)
time.sleep(3)
time.sleep(1.5)
# decide whether grab cube
def decide_move(self, x, y, color):
@ -490,8 +484,8 @@ def run():
res_queue = [[], [], [], []]
res_queue[0] = parse_folder('res/red')
res_queue[1] = parse_folder('res/green')
res_queue[2] = parse_folder('res/gray')
res_queue[3] = parse_folder('res/blue')
res_queue[2] = parse_folder('res/blue')
res_queue[3] = parse_folder('res/gray')
# res_queue = []
# res_queue.extend(parse_folder('res/red'))

14
mycobot_ai/myPalletizer_260/scripts/test.py
View file

@ -2,9 +2,9 @@
from pymycobot.mypalletizer import MyPalletizer
from pymycobot.genre import Angle
from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
import time
import time,os
mc = MyPalletizer("/dev/ttyUSB0", 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("/dev/ttyUSB0", 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,0,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)
@ -26,8 +26,8 @@ mc = MyPalletizer("/dev/ttyUSB0", 115200)
# time.sleep(3)
# mc.send_angle(3,0,25)
print(mc.get_angles())
print(mc.get_coords())
# print(mc.get_angles())
# print(mc.get_coords())
# while True:
# print("angles:%s"%mc.get_angles())
@ -38,4 +38,4 @@ print(mc.get_coords())
# 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)

18
mycobot_description/urdf/260_urdf/mypal_260_aikit.urdf
View file

@ -3,7 +3,7 @@
<xacro:property name="width" value=".2" />
<link name="env">
<link name="env">
<visual>
<geometry>
<mesh filename="package://mycobot_description/urdf/260_urdf/suit_env.dae" scale="1.2 1.2 1.2"/>
@ -18,14 +18,16 @@
<!--- 0.0 0 -0.04 1.5708 3.14159-->
<mesh filename="package://mycobot_description/urdf/260_urdf/base.dae"/>
</geometry>
<origin xyz = " 0 -0.28 0.051 " rpy = " 1.5707963 0 -1.5707963"/>
<origin xyz = " 0 -0.28 0.051" rpy = " 1.5707963 0 -1.5707963"/>
<!-- <origin xyz = " 0 0 0 " rpy = " 0 0 0"/> -->
</visual>
<collision>
<geometry>
<!--- 0.0 0 -0.04 1.5708 3.14159-->
<mesh filename="package://mycobot_description/urdf/260_urdf/base.dae"/>
</geometry>
<origin xyz = " 0 -0.28 0.051 " rpy = " 1.5707963 0 -1.5707963"/>
<!-- <origin xyz = " 0 -0.28 0.051 " rpy = " 1.5707963 0 1.5707963"/> -->
<!-- <origin xyz = " 0 0 0 " rpy = " 0 0 3.1415926"/> -->
</collision>
</link>
@ -100,6 +102,7 @@
<geometry>
<!--- 0.0 0 -0.04 -->
<cylinder length="0.015" radius="0.0152"/>
<!-- <mesh filename="package://mycobot_description/urdf/260_urdf/link5.dae"/> -->
</geometry>
<material name = "grey">
<color rgba = "0.5 0.5 0.5 1"/>
@ -123,7 +126,6 @@
<origin xyz= "0 0 0.12" rpy = "0 0 0"/>
</joint>
<joint name="joint2_to_joint1" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-0.014" upper = "1.49" velocity = "0"/>
@ -132,18 +134,17 @@
<origin xyz= "0 0 0.0" rpy = " -1.5707963 0 0 "/>
</joint>
<joint name="joint3_to_joint2" type="revolute">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "-0.213" upper = "1.106" velocity = "0"/>
<limit effort = "1000.0" lower = "-0.213" upper = "0.872665" velocity = "0"/>
<parent link="link2"/>
<child link="link3"/>
<origin xyz= "0.0 -0.14 0 " rpy = "0 0 0 "/>
</joint>
<joint name="joint4_to_joint3" type="fixed">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "0" upper = "1.5707963" velocity = "0"/>
<!-- <axis xyz=" 0 0 1"/> -->
<!-- <limit effort = "1000.0" lower = "0" upper = "1.5707963" velocity = "0"/> -->
<parent link="link3"/>
<child link="link4"/>
<origin xyz= "0.134 -0.005 0.0" rpy = "3.1415926 0 0 "/>
@ -165,5 +166,4 @@
<child link="env"/>
<origin xyz= "0 0 0" rpy = "0 0 0"/>
</joint>
</robot>

239
mypalletizer_260/mypalletizer_260/config/mypal_260.rviz
View file

@ -9,7 +9,7 @@ Panels:
- /RobotModel1
- /TF1
Splitter Ratio: 0.5
Tree Height: 775
Tree Height: 601
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@ -18,7 +18,7 @@ Panels:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
@ -29,6 +29,8 @@ Panels:
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
@ -40,7 +42,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
@ -58,7 +60,7 @@ Visualization Manager:
Enabled: true
Links:
All Links Enabled: true
Expand link Details: false
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
@ -92,7 +94,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
@ -116,8 +117,7 @@ Visualization Manager:
Value: true
link5:
Value: true
Marker Scale: 0.300000012
Marker Scale: 0.30000001192092896
Name: TF
Show Arrows: true
Show Axes: true
@ -147,7 +147,10 @@ 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
@ -157,33 +160,33 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 1.20289087
Distance: 1.2028908729553223
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0.107583851
Z: 0.10758385062217712
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.440398335
Near Clip Distance: 0.009999999776482582
Pitch: 0.42039862275123596
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.430389732
Yaw: 1.1754094362258911
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Height: 892
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@ -192,6 +195,204 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24
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

2
mypalletizer_260/mypalletizer_260/scripts/listen_real_of_topic.py
View file

@ -23,7 +23,7 @@ class Listener(object):
"""`mypal260/angles_real` subscriber callback method.
Args:
data (MypalAngles): callback argument.
data (mypalAngles): callback argument.
"""
# ini publisher object.
# 初始化发布者对象

3
mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch
View file

@ -1,9 +1,8 @@
<launch>
<arg name="port" default="/dev/ttyUSB0" />
<arg name="port" default="/dev/ttyUSB1" />
<arg name="baud" default="115200" />
<!-- Open communication service -->
<!-- <node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics.py" output="screen"> -->
<node name="mypalletizer_services" pkg="mypalletizer_communication" type="mypal_topics.py" output="screen">
<param name="port" type="string" value="$(arg port)" />

23
mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py
View file

@ -1,5 +1,5 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
# -*- coding:utf-8 -*-
import time
import os
import sys
@ -7,7 +7,6 @@ import signal
import threading
import rospy
from pymycobot.mypalletizer import MyPalletizer
from mypalletizer_communication.msg import(
MypalAngles,
MypalCoords,
@ -66,9 +65,10 @@ class MypalTopics(object):
def __init__(self):
super(MypalTopics, self).__init__()
rospy.init_node("Mypal_topics")
rospy.init_node("mypal_topics")
rospy.loginfo("start ...")
port = rospy.get_param("~port", "/dev/ttyUSB0")
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)
@ -105,8 +105,7 @@ class MypalTopics(object):
def pub_real_angles(self):
"""Publish real angle"""
"""发布真实角度"""
pub = rospy.Publisher("Mypal/angles_real",
MypalAngles, queue_size=5)
pub = rospy.Publisher("mypal/angles_real", MypalAngles, queue_size=5)
ma = MypalAngles()
while not rospy.is_shutdown():
self.lock.acquire()
@ -125,8 +124,7 @@ class MypalTopics(object):
def pub_real_coords(self):
"""publish real coordinates"""
"""发布真实坐标"""
pub = rospy.Publisher("Mypal/coords_real",
MypalCoords, queue_size=5)
pub = rospy.Publisher("mypal/coords_real", MypalCoords, queue_size=5)
ma = MypalCoords()
while not rospy.is_shutdown():
self.lock.acquire()
@ -158,7 +156,7 @@ class MypalTopics(object):
self.mc.send_angles(angles, sp)
sub = rospy.Subscriber(
"Mypal/angles_goal", MypalSetAngles, callback=callback
"mypal/angles_goal", MypalSetAngles, callback=callback
)
rospy.spin()
@ -171,7 +169,7 @@ class MypalTopics(object):
self.mc.send_coords(angles, sp, model)
sub = rospy.Subscriber(
"Mypal/coords_goal", MypalSetCoords, callback=callback
"mypal/coords_goal", MypalSetCoords, callback=callback
)
rospy.spin()
@ -185,7 +183,7 @@ class MypalTopics(object):
self.mc.set_gripper_state(1, 80)
sub = rospy.Subscriber(
"Mypal/gripper_status", MypalGripperStatus, callback=callback
"mypal/gripper_status", MypalGripperStatus, callback=callback
)
rospy.spin()
@ -199,11 +197,10 @@ class MypalTopics(object):
self.mc.set_basic_output(data.Pin2, 1)
sub = rospy.Subscriber(
"Mypal/pump_status", MypalPumpStatus, callback=callback
"mypal/pump_status", MypalPumpStatus, callback=callback
)
rospy.spin()
if __name__ == "__main__":
Watcher()
mc_topics = MypalTopics()

3
mypalletizer_260/mypalletizer_communication/scripts/mypal_topics_pi.py
View file

@ -16,8 +16,6 @@ from mypalletizer_communication.msg import(
MypalPumpStatus,
)
# from pymycobot import MyCobotSocket
class Watcher:
"""this class solves two problems with multithreaded
@ -171,7 +169,6 @@ class MypalTopics(object):
def callback(data):
# angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
angles = [data.x, data.y, data.z, data.rx]
sp = int(data.speed)
model = int(data.model)
self.mc.send_coords(angles, sp, model)