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

Adaptation jetson nano

Browse source
This commit is contained in:
weiquan 2021-11-03 15:47:31 +08:00
parent eddef1364f
commit 9313608914
6 changed files with 169 additions and 116 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
.gitignore vendored
View file

@ -5,6 +5,7 @@
.vscode/ .vscode/
.history .history
*.pyc *.pyc
*.bat
# Editor directories and files # Editor directories and files
.idea .idea

79
mycobot_ai/scripts/ai_windows.py
View file

@ -20,15 +20,21 @@ class Application(object):
self.run_py = "" self.run_py = ""
# 判断通信口并给权限 # 判断通信口并给权限
try: try:
self.ports = os.popen("ls /dev/ttyUSB*").readline() self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
if "dev" not in self.ports: self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.ports = os.popen("ls /dev/ttyACM*").readline()[:-1] self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
self.command = '<arg name="port" default="{}" />'.format( self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
self.ports) if "dev" in self.robot_wio:
# 根据通信口修改ros启动文件 self.command = '<arg name="port" default="{}" />'.format(
os.system( self.robot_wio)
"sed -i '2c {}' /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/launch/vision.launch" # 根据通信口修改ros启动文件
.format(self.command)) os.system(
"sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
.format(self.command))
elif "dev" in self.robot_raspi:
self.change_file(self.robot_raspi)
elif "dev" in self.robot_jes:
self.change_file(self.robot_jes)
except Exception as e: except Exception as e:
pass pass
@ -36,9 +42,6 @@ class Application(object):
self.win.title("aikit启动工具") self.win.title("aikit启动工具")
self.win.geometry( self.win.geometry(
"550x350+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置 "550x350+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
# 打开ros按钮
self.btn = Button(self.win, text="open ROS", command=self.open_ros)
self.btn.grid(row=0)
self.chanse_code = Label(self.win, text="选择程序:", width=10) self.chanse_code = Label(self.win, text="选择程序:", width=10)
self.chanse_code.grid(row=1) self.chanse_code.grid(row=1)
@ -50,23 +53,7 @@ class Application(object):
self.add_btn = Button(self.win, text="添加新的物体图像", command=self.add_img) self.add_btn = Button(self.win, text="添加新的物体图像", command=self.add_img)
self.add_btn.grid(row=1, column=2) self.add_btn.grid(row=1, column=2)
# self.set_xy = Label(self.win, text="set_xy:", width=10) self.tips = "1、等待打开ros,大概需要15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可"
# self.set_xy.grid(row=1)
# self.x = Label(self.win, text="x:")
# self.x.grid(row=2)
# self.v1 = StringVar()
# self.e1 = Entry(self.win,textvariable=self.v1, width=10)
# self.e1.insert(0,0)
# self.e1.grid(row=2,column=1)
# self.y = Label(self.win, text="y:")
# self.y.grid(row=3)
# self.v2 = StringVar()
# self.e2 = Entry(self.win,textvariable=self.v2, width=10)
# self.e2.insert(0,0)
# self.e2.grid(row=3,column=1)
self.tips = "1、首先打开ros,大概需要等待15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可"
self.btn = Button(self.win, text="运行", command=self.start_run) self.btn = Button(self.win, text="运行", command=self.start_run)
self.btn.grid(row=5) self.btn.grid(row=5)
@ -78,10 +65,23 @@ class Application(object):
self.log_data = Text(self.win, width=66, height=10) self.log_data = Text(self.win, width=66, height=10)
self.log_data.grid(row=16, column=0, columnspan=10) self.log_data.grid(row=16, column=0, columnspan=10)
self.log_data.insert(END, self.tips) self.log_data.insert(END, self.tips)
# self.code_list = ttk.Combobox(self.win, width=15)
# self.code_list["value"] = ("颜色识别", "物体识别", "二维码识别") self.open_ros()
# self.code_list.current(0) self.win.protocol('WM_DELETE_WINDOW', self.close_rviz)
# self.code_list.grid(row=1, column=1)
def close_rviz(self):
os.system(
"ps -ef | grep -E mycobot.rviz | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
sys.exit(0)
def change_file(self, port):
command1 = '<arg name="port" default="{}" />'.format(port)
command2 = '<arg name="baud" default="{}" />'.format(1000000)
# 根据通信口修改ros启动文件
os.system(
"sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command1))
os.system(
"sed -i '3c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command2))
def start_run(self): def start_run(self):
try: try:
@ -108,28 +108,25 @@ class Application(object):
def open_py(self): def open_py(self):
os.system( os.system(
"python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_obj_img.py" "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_img.py"
) )
def open_py1(self): def open_py1(self):
os.system( os.system(
"python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_obj_color.py" "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_color.py"
) )
def open_py2(self): def open_py2(self):
os.system( os.system(
"python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_encode.py" "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_encode.py"
) )
def add_img(self): def add_img(self):
os.system( os.system(
"python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/add_img.py" "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/add_img.py"
) )
def open_ros(self): def open_ros(self):
if self.ros:
print("ros is opened")
return
t1 = threading.Thread(target=self.ross) t1 = threading.Thread(target=self.ross)
t1.setDaemon(True) t1.setDaemon(True)
t1.start() t1.start()
@ -137,7 +134,7 @@ class Application(object):
def ross(self): def ross(self):
os.system( os.system(
"roslaunch ~/catkin_mycobot/src/mycobot_ros/mycobot_ai/launch/vision.launch" "roslaunch ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
) )
def close_py(self): def close_py(self):

51
mycobot_ai/scripts/detect_encode.py
View file

@ -24,6 +24,7 @@ class Detect_marker(Movement):
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").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 self.raspi = False
if "dev" in self.robot_m5: if "dev" in self.robot_m5:
self.Pin = [2, 5] self.Pin = [2, 5]
@ -31,17 +32,21 @@ class Detect_marker(Movement):
self.Pin = [20, 21] self.Pin = [20, 21]
for i in self.move_coords: for i in self.move_coords:
i[2] -= 20 i[2] -= 20
elif "dev" in self.robot_raspi: elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO import RPi.GPIO as GPIO
self.GPIO = GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT) GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT) GPIO.setup(21, GPIO.OUT)
self.raspi = True self.raspi = True
pump_y -= 5 if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
# Creating a Camera Object # Creating a Camera Object
cap_num = 0 cap_num = 0
self.cap = cv.VideoCapture(cap_num) self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L)
# Get ArUco marker dict that can be detected. # Get ArUco marker dict that can be detected.
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
# Get ArUco marker params. # Get ArUco marker params.
@ -93,7 +98,7 @@ class Detect_marker(Movement):
def move(self, x, y): def move(self, x, y):
if self.raspi: if self.raspi:
coords = [ coords = [
[145.6, -64.9, 285.2, 179.88, 7.67, -177.06], [145.6, -64.9, 285.2, 179.88, 7.67, 179],
[130.1, -155.6, 243.9, 178.99, 5.38, -179.9] [130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
] ]
else: else:
@ -138,6 +143,7 @@ class Detect_marker(Movement):
else: else:
self.pub_pump(False, self.Pin) self.pub_pump(False, self.Pin)
# publish marker # publish marker
time.sleep(1)
self.marker.header.stamp = rospy.Time.now() self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = coords[1][0]/1000.0 self.marker.pose.position.x = coords[1][0]/1000.0
self.marker.pose.position.y = coords[1][1]/1000.0 self.marker.pose.position.y = coords[1][1]/1000.0
@ -148,11 +154,11 @@ class Detect_marker(Movement):
def gpio_status(self, flag): def gpio_status(self, flag):
if flag: if flag:
GPIO.output(20, 0) self.GPIO.output(20, 0)
GPIO.output(21, 0) self.GPIO.output(21, 0)
else: else:
GPIO.output(20, 1) self.GPIO.output(20, 1)
GPIO.output(21, 1) self.GPIO.output(21, 1)
# decide whether grab cube # decide whether grab cube
def decide_move(self, x, y): def decide_move(self, x, y):
@ -164,41 +170,44 @@ class Detect_marker(Movement):
return return
else: else:
self.cache_x = self.cache_y = 0 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) self.move(x, y)
# init mycobot # init mycobot
def init_mycobot(self): def init_mycobot(self):
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
for _ in range(5): for _ in range(5):
print _ print _
self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, -177.06], 20, 1), 20, 1) self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1)
time.sleep(0.5) time.sleep(0.5)
def run(self): def run(self):
global pump_y, pump_x global pump_y, pump_x
self.init_mycobot() self.init_mycobot()
num=sum_x=sum_y=0 num = sum_x = sum_y = 0
while cv.waitKey(1) < 0: while cv.waitKey(1) < 0:
success, img=self.cap.read() success, img = self.cap.read()
if not success: if not success:
print("It seems that the image cannot be acquired correctly.") print("It seems that the image cannot be acquired correctly.")
break break
# transfrom the img to model of gray # transfrom the img to model of gray
gray=cv.cvtColor(img, cv.COLOR_BGR2GRAY) gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Detect ArUco marker. # Detect ArUco marker.
corners, ids, rejectImaPoint=cv.aruco.detectMarkers( corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
gray, self.aruco_dict, parameters = self.aruco_params gray, self.aruco_dict, parameters=self.aruco_params
) )
if len(corners) > 0: if len(corners) > 0:
if ids is not None: if ids is not None:
# get informations of aruco # get informations of aruco
ret=cv.aruco.estimatePoseSingleMarkers( ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.03, self.camera_matrix, self.dist_coeffs corners, 0.03, self.camera_matrix, self.dist_coeffs
) )
# rvec:rotation offset,tvec:translation deviator # rvec:rotation offset,tvec:translation deviator

53
mycobot_ai/scripts/detect_obj_color.py
View file

@ -26,15 +26,16 @@ class Object_detect(Movement):
dir_path = os.path.dirname(__file__) dir_path = os.path.dirname(__file__)
# 移动角度 # 移动角度
self.move_angles = [ self.move_angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point [-7.11, -6.94, -55.01, -24.16, 0, 15], # init the point
[-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab [5, -10.63, -87.8, 9.05, -3.07, 15], # point to grab
[17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
] ]
# 移动坐标 # 移动坐标
self.move_coords = [ self.move_coords = [
[120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket
# above the yello bucket # above the yello bucket
[215.2, -127.8, 260.9, -157.51, -17.5, -71.18], #[215.2, -127.8, 260.9, -157.51, -17.5, -71.18],
[210.6, -130.5, 263.0, -150.99, -0.07, -107.35],
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38], [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47], [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3], [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
@ -43,6 +44,7 @@ class Object_detect(Movement):
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").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 self.raspi = False
if "dev" in self.robot_m5: if "dev" in self.robot_m5:
self.Pin = [2, 5] self.Pin = [2, 5]
@ -50,12 +52,20 @@ class Object_detect(Movement):
self.Pin = [20, 21] self.Pin = [20, 21]
for i in self.move_coords: for i in self.move_coords:
i[2] -= 20 i[2] -= 20
elif "dev" in self.robot_raspi: elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO import RPi.GPIO as GPIO
GPIO.setwarnings(False)
self.GPIO = GPIO
GPIO.setmode(GPIO.BCM) GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT) GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT) GPIO.setup(21, GPIO.OUT)
GPIO.output(20,1)
GPIO.output(21,1)
self.raspi = True self.raspi = True
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
# choose place to set cube # choose place to set cube
self.color = 0 self.color = 0
@ -120,11 +130,11 @@ class Object_detect(Movement):
def gpio_status(self, flag): def gpio_status(self, flag):
if flag: if flag:
GPIO.output(20, 0) self.GPIO.output(20, 0)
GPIO.output(21, 0) self.GPIO.output(21, 0)
else: else:
GPIO.output(20, 1) self.GPIO.output(20, 1)
GPIO.output(21, 1) self.GPIO.output(21, 1)
# Grasping motion # Grasping motion
def move(self, x, y, color): def move(self, x, y, color):
@ -151,6 +161,7 @@ class Object_detect(Movement):
elif x < 135: elif x < 135:
h = -20 h = -20
self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1) self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1)
time.sleep(1.5) time.sleep(1.5)
# open pump # open pump
if self.raspi: if self.raspi:
@ -182,6 +193,7 @@ class Object_detect(Movement):
self.gpio_status(False) self.gpio_status(False)
else: else:
self.pub_pump(False, self.Pin) self.pub_pump(False, self.Pin)
time.sleep(1)
if color == 1: if color == 1:
self.pub_marker( self.pub_marker(
self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
@ -203,7 +215,7 @@ class Object_detect(Movement):
else: else:
self.cache_x = self.cache_y = 0 self.cache_x = self.cache_y = 0
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
if "dev" not in self.robot_wio: if "dev" in self.robot_raspi:
if (y < -30 and x > 140) or (x > 150 and y < -10): if (y < -30 and x > 140) or (x > 150 and y < -10):
x -= 10 x -= 10
@ -213,26 +225,26 @@ class Object_detect(Movement):
elif x > 170: elif x > 170:
x -= 10 x -= 10
y += 10 y += 10
print x, y elif "dev" in self.robot_wio:
else:
if x > 160: if x > 160:
y += 10 y += 10
elif y < -20: elif y < -20:
x -= 10 x -= 10
y += 10 y += 10
elif "dev" in self.robot_jes:
y+=13
x+=4
print x, y
self.move(x, y, color) self.move(x, y, color)
# init mycobot # init mycobot
def run(self): def run(self):
for _ in range(5): for _ in range(5):
self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20) self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
print(_) print(_)
time.sleep(0.5) time.sleep(0.5)
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
# draw aruco # draw aruco
def draw_marker(self, img, x, y): def draw_marker(self, img, x, y):
@ -374,7 +386,8 @@ class Object_detect(Movement):
if __name__ == "__main__": if __name__ == "__main__":
# open the camera # open the camera
cap_num = 0 cap_num = 0
cap = cv2.VideoCapture(cap_num) cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
if not cap.isOpened(): if not cap.isOpened():
cap.open() cap.open()
# init a class of Object_detect # init a class of Object_detect

64
mycobot_ai/scripts/detect_obj_img.py
View file

@ -27,15 +27,16 @@ class Object_detect(Movement):
dir_path = os.path.dirname(__file__) dir_path = os.path.dirname(__file__)
# 移动角度 # 移动角度
self.move_angles = [ self.move_angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point [-7.11, -6.94, -55.01, -24.16, 0, 15], # init the point
[-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab [-1.14, -10.63, -87.8, 9.05, -3.07, 15], # point to grab
[17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
] ]
# 移动坐标 # 移动坐标
self.move_coords = [ self.move_coords = [
[120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket
# above the yello bucket # above the yello bucket
[208.2, -127.8, 260.9, -157.51, -17.5, -71.18], #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
[205.6, -130.5, 263.0, -150.99, -0.07, -107.35],
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38], [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47], [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3], [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
@ -44,6 +45,7 @@ class Object_detect(Movement):
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").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 self.raspi = False
if "dev" in self.robot_m5: if "dev" in self.robot_m5:
self.Pin = [2, 5] self.Pin = [2, 5]
@ -51,13 +53,19 @@ class Object_detect(Movement):
self.Pin = [20, 21] self.Pin = [20, 21]
for i in self.move_coords: for i in self.move_coords:
i[2] -= 20 i[2] -= 20
elif "dev" in self.robot_raspi: elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO import RPi.GPIO as GPIO
self.GPIO = GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT) GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT) GPIO.setup(21, GPIO.OUT)
self.raspi = True
self.raspi = True
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
# choose place to set cube # choose place to set cube
self.color = 0 self.color = 0
# parameters to calculate camera clipping parameters # parameters to calculate camera clipping parameters
@ -126,11 +134,11 @@ class Object_detect(Movement):
def gpio_status(self, flag): def gpio_status(self, flag):
if flag: if flag:
GPIO.output(20, 0) self.GPIO.output(20, 0)
GPIO.output(21, 0) self.GPIO.output(21, 0)
else: else:
GPIO.output(20, 1) self.GPIO.output(20, 1)
GPIO.output(21, 1) self.GPIO.output(21, 1)
# Grasping motion # Grasping motion
def move(self, x, y, color): def move(self, x, y, color):
@ -142,11 +150,12 @@ class Object_detect(Movement):
self.pub_angles(self.move_angles[2], 20) self.pub_angles(self.move_angles[2], 20)
time.sleep(1.5) time.sleep(1.5)
# send coordinates to move mycobot # send coordinates to move mycobot
self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1) self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1)
time.sleep(1.5) time.sleep(1.5)
# 根据不同底板机械臂，调整吸泵高度 # 根据不同底板机械臂，调整吸泵高度
if "dev" in self.robot_m5: if "dev" in self.robot_m5:
self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1) # m5 and jetson nano
self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1)
elif "dev" in self.robot_wio: elif "dev" in self.robot_wio:
h = 0 h = 0
if 165 < x < 180: if 165 < x < 180:
@ -155,7 +164,12 @@ class Object_detect(Movement):
h = 20 h = 20
elif x < 135: elif x < 135:
h = -20 h = -20
self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1) self.pub_coords([x, y, 31.9+h, -178.9, -1, -66], 20, 1)
elif "dev" in self.robot_jes:
h = 0
if x<130:
h=15
self.pub_coords([x, y, 90-h, -178.9, -1.57, -66], 25, 1)
time.sleep(1.5) time.sleep(1.5)
# open pump # open pump
if self.raspi: if self.raspi:
@ -177,7 +191,7 @@ class Object_detect(Movement):
time.sleep(1.5) time.sleep(1.5)
self.pub_marker( self.pub_marker(
self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0) self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0)
print self.move_coords[color]
self.pub_coords(self.move_coords[color], 20, 1) self.pub_coords(self.move_coords[color], 20, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color] self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
[1]/1000.0, self.move_coords[color][2]/1000.0) [1]/1000.0, self.move_coords[color][2]/1000.0)
@ -187,6 +201,7 @@ class Object_detect(Movement):
self.gpio_status(False) self.gpio_status(False)
else: else:
self.pub_pump(False, self.Pin) self.pub_pump(False, self.Pin)
time.sleep(1)
if color == 1: if color == 1:
self.pub_marker( self.pub_marker(
self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
@ -207,7 +222,7 @@ class Object_detect(Movement):
else: else:
self.cache_x = self.cache_y = 0 self.cache_x = self.cache_y = 0
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
if "dev" not in self.robot_wio: if "dev" in self.robot_wio:
if (y < -30 and x > 140) or (x > 150 and y < -10): if (y < -30 and x > 140) or (x > 150 and y < -10):
x -= 10 x -= 10
y += 10 y += 10
@ -216,22 +231,25 @@ class Object_detect(Movement):
elif x > 170: elif x > 170:
x -= 10 x -= 10
y += 10 y += 10
elif "dev" not in self.robot_m5: elif "dev" in self.robot_m5:
y += 10 y += 10
x -= 5 x -= 5
if y < -20: if y < -20:
y += 5 y += 5
# print x,y # print x,y
elif "dev" in self.robot_jes:
if y<0:
x+=5
y+=3
y+=10
print x,y
self.move(x, y, color) self.move(x, y, color)
# init mycobot # init mycobot
def run(self): def run(self):
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
for _ in range(5): for _ in range(5):
self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20) self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
print(_) print(_)
time.sleep(0.5) time.sleep(0.5)
@ -459,7 +477,7 @@ def run():
for l in k: for l in k:
goal.append(cv2.imread('local_photo/img/{}'.format(l))) goal.append(cv2.imread('local_photo/img/{}'.format(l)))
cap_num = 0 cap_num = 0
cap = cv2.VideoCapture(cap_num) cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
if not cap.isOpened(): if not cap.isOpened():
cap.open() cap.open()
# init a class of Object_detect # init a class of Object_detect

37
mycobot_ai/scripts/pump.py
View file

@ -5,19 +5,34 @@ import rospy
import time import time
from moving_utils import Movement from moving_utils import Movement
class Pump(Movement): class Pump(Movement):
def __init__(self): def __init__(self):
super(Pump, self).__init__() super(Pump, self).__init__()
rospy.init_node("pump", anonymous=True) rospy.init_node("pump", anonymous=True)
def run(self):
self.pub_pump(False)
time.sleep(1)
self.pub_pump(True)
time.sleep(5)
self.pub_pump(False)
def gpiod(self):
import RPi.GPIO as GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
GPIO.output(20, 0)
GPIO.output(21, 0)
time.sleep(3)
print "close"
GPIO.output(20, 1)
GPIO.output(21, 1)
def run(self):
self.pub_pump(False)
time.sleep(1)
self.pub_pump(True)
time.sleep(5)
self.pub_pump(False)
if __name__ == "__main__": if __name__ == "__main__":
pump = Pump() pump = Pump()
pump.run() pump.gpiod()