mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
update
This commit is contained in:
2929ss
parent
6125a41439
commit
9740bc74e7
5 changed files with 108 additions and 85 deletions
|
|
@ -1,9 +1,9 @@
|
|||
<launch>
|
||||
<arg name="port" default="/dev/ttyUSB1" />
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<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="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)" />
|
||||
|
|
@ -13,14 +13,14 @@
|
|||
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- mypalletizer-topics -->
|
||||
<include file="$(find mypalletizer_communication)/launch/communication_topic.launch">
|
||||
<!-- mecharm-topics -->
|
||||
<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" />
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mecharm_pi/mecharm_pi.urdf"/>
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mecharm_pi/mecharm_pi_aikit.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mecharm_pi)/config/mecharm_pi.rviz" />
|
||||
<!-- <arg name="gui" default="false" /> -->
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
# -*- coding:utf-8 -*-
|
||||
#!/usr/bin/env python2
|
||||
# -*- coding:utf-8 -*-
|
||||
from operator import imod
|
||||
from tokenize import Pointfloat
|
||||
import cv2
|
||||
|
|
@ -9,7 +9,7 @@ import json
|
|||
import os,sys
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
from pymycobot.mypalletizer import MyPalletizer
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from moving_utils import Movement
|
||||
|
||||
|
||||
|
|
@ -20,7 +20,7 @@ __version__ = "1.0"
|
|||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 160, camera_y = 10):
|
||||
def __init__(self, camera_x = 145, camera_y = -5):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
|
|
@ -31,17 +31,16 @@ class Object_detect(Movement):
|
|||
|
||||
# 移动角度
|
||||
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
|
||||
[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 = [
|
||||
[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
|
||||
[92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
|
||||
[165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
|
||||
[88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
|
||||
[-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket
|
||||
]
|
||||
|
||||
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
|
||||
|
|
@ -154,17 +153,18 @@ class Object_detect(Movement):
|
|||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mypal260
|
||||
# send Angle to move 270
|
||||
print(color)
|
||||
self.mc.send_angles(self.move_angles[0], 20)
|
||||
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)
|
||||
|
||||
# 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)
|
||||
|
||||
|
||||
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:
|
||||
self.pump_on()
|
||||
|
|
@ -172,14 +172,23 @@ class Object_detect(Movement):
|
|||
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)
|
||||
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], 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)
|
||||
|
||||
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
|
||||
|
|
@ -196,7 +205,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.mc.send_angles(self.move_angles[1], 20)
|
||||
self.mc.send_angles(self.move_angles[1], 30)
|
||||
time.sleep(1.5)
|
||||
|
||||
# decide whether grab cube
|
||||
|
|
@ -214,12 +223,12 @@ class Object_detect(Movement):
|
|||
# init mypal260
|
||||
def run(self):
|
||||
if "dev" in self.robot_m5:
|
||||
self.mc = MyPalletizer(self.robot_m5, 115200)
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyPalletizer(self.robot_raspi, 1000000)
|
||||
self.mc = MyCobot(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)
|
||||
self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ from threading import Thread
|
|||
import tkFileDialog as filedialog
|
||||
import Tkinter as tk
|
||||
from moving_utils import Movement
|
||||
from pymycobot.mypalletizer import MyPalletizer
|
||||
from pymycobot.mycobot import MyCobot
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0" # Adaptive seeed
|
||||
|
|
@ -28,7 +28,7 @@ __version__ = "1.0" # Adaptive seeed
|
|||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 160, camera_y = 10):
|
||||
def __init__(self, camera_x = 145, camera_y = -5):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
|
|
@ -38,17 +38,16 @@ class Object_detect(Movement):
|
|||
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
|
||||
[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 = [
|
||||
[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
|
||||
[92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
|
||||
[165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
|
||||
[88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
|
||||
[-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket
|
||||
]
|
||||
|
||||
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
|
||||
|
|
@ -176,15 +175,16 @@ class Object_detect(Movement):
|
|||
|
||||
# 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 Angle to move 270
|
||||
self.mc.send_angles(self.move_angles[0], 30)
|
||||
time.sleep(4)
|
||||
|
||||
# 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)
|
||||
# send coordinates to move 270 根据不同底板机械臂，调整吸泵高度
|
||||
self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
|
||||
time.sleep(4)
|
||||
|
||||
self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
|
||||
time.sleep(4)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5:
|
||||
|
|
@ -193,12 +193,20 @@ class Object_detect(Movement):
|
|||
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)
|
||||
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], 20, 1)
|
||||
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)
|
||||
|
|
@ -211,7 +219,7 @@ class Object_detect(Movement):
|
|||
self.gpio_status(False)
|
||||
time.sleep(6)
|
||||
|
||||
self.mc.send_angles(self.move_angles[1], 20)
|
||||
self.mc.send_angles(self.move_angles[1], 30)
|
||||
time.sleep(1.5)
|
||||
|
||||
# decide whether grab cube
|
||||
|
|
@ -226,15 +234,15 @@ class Object_detect(Movement):
|
|||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mypal260
|
||||
# init 270
|
||||
def run(self):
|
||||
if "dev" in self.robot_m5:
|
||||
self.mc = MyPalletizer(self.robot_m5, 115200)
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyPalletizer(self.robot_raspi, 1000000)
|
||||
self.mc = MyCobot(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)
|
||||
self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
|
||||
time.sleep(3)
|
||||
|
||||
# draw aruco
|
||||
|
|
@ -420,8 +428,8 @@ class Object_detect(Movement):
|
|||
# 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
|
||||
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/mechArm_270/' + folder
|
||||
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/mechArm_270/' + folder
|
||||
|
||||
if os.path.exists(path1):
|
||||
path = path1
|
||||
|
|
|
|||
|
|
@ -1,38 +1,44 @@
|
|||
# -*- coding: utf-8 -*-
|
||||
#!/usr/bin/env python2
|
||||
# -*- coding:utf-8 -*-
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from pymycobot.mypalletizer import MyPalletizer
|
||||
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 = MyCobot("/dev/ttyAMA0", 1000000)
|
||||
# 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]
|
||||
# time.sleep(1.5)
|
||||
|
||||
# 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,0,0,90,0],30) # coords:[95.6, 0.5, 166.4, 179.12, -0.18, 179.46]
|
||||
# mc.send_angles([-3.25, 17.22, -32.51, 2.37, 92.54, -36.21],30)
|
||||
|
||||
# mc.send_coords([141.2, -142.0, 206.2, -26.8],25,1) # above the red bucket
|
||||
# time.sleep(2)
|
||||
# mc.send_coords([234.3, -120, 210, -48.77],25,1) # above the green bucket
|
||||
# time.sleep(2)
|
||||
# mc.send_coords([100.9, 159.3, 248.6, -124.27],20,1) # above the blue bucket
|
||||
# time.sleep(3)
|
||||
# mc.send_coords([-17.6, 161.6, 238.4, -152.31],20,1) # above the gray bucket
|
||||
time.sleep(4)
|
||||
|
||||
# mc.send_coords([92.3, -104.9, 211.4, -179.6, 28.91, 131.29], 30, 0) # above the red bucket
|
||||
# time.sleep(4)
|
||||
|
||||
# mc.send_coords([165.0, -93.6, 201.4, -173.43, 46.23, 160.65],30,0) # above the green bucket
|
||||
# time.sleep(4)
|
||||
|
||||
# mc.send_coords([88.1, 126.3, 193.4, 162.15, 2.23, 156.02],30,0) # above the blue bucket
|
||||
# time.sleep(4)
|
||||
|
||||
# mc.send_coords([-5.4, 120.6, 204.6, 162.66, -6.96, 159.93],30,0) # above the gray bucket
|
||||
# time.sleep(3)
|
||||
|
||||
# mc.send_coords([80, 0, 130, 0 ,0, 0], 30, 0)
|
||||
|
||||
# mc.send_angle(3,0,25)
|
||||
# print(mc.get_angles())
|
||||
# print(mc.get_coords())
|
||||
|
||||
# mc.release_all_servos()
|
||||
# while True:
|
||||
# print("angles:%s"%mc.get_angles())
|
||||
# print("coords:%s"%mc.get_coords())
|
||||
# print("\n")
|
||||
# print("angles:%s"% mc.get_angles())
|
||||
# print("coords:%s"% mc.get_coords())
|
||||
# print("\n")
|
||||
|
||||
# mc.release_all_servos()
|
||||
# mc.set_servo_calibration(1)
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue