mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
111 lines
No EOL
2.5 KiB
Python
Executable file
111 lines
No EOL
2.5 KiB
Python
Executable file
#!/usr/bin/env python2
|
|
# license removed for brevity
|
|
|
|
|
|
import rospy
|
|
from visualization_msgs import msg
|
|
from visualization_msgs.msg import Marker
|
|
# from pymycobot.mycobot import MyCobot
|
|
# from pymycobot.genre import Coord
|
|
# from pymycobot import PI_PORT, PI_BAUD # For raspberry pi version of mycobot.
|
|
import time
|
|
from mycobot_ros.srv import (
|
|
GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus)
|
|
|
|
|
|
set_coords = None
|
|
set_angles = None
|
|
|
|
x_offset = 0
|
|
y_offset = 30
|
|
z_offset = 60
|
|
|
|
flag = False
|
|
|
|
|
|
def grippercallback(data):
|
|
global flag
|
|
print(type(data))
|
|
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
|
# print(Marker.)
|
|
# coord_datas = mc.get_coords()
|
|
# print(coord_datas)
|
|
if flag:
|
|
return
|
|
|
|
|
|
# pump lenght: 88mm
|
|
x = float(format(data.pose.position.x*1000, '.2f'))
|
|
y = float(format(data.pose.position.y*1000, '.2f'))
|
|
z = float(format(data.pose.position.z*1000, '.2f'))
|
|
|
|
|
|
print(x, y, z)
|
|
|
|
# detect heigth + pump height + limit height + offset
|
|
x += x_offset
|
|
y += y_offset
|
|
z = z + 88 + 25 + z_offset
|
|
|
|
try:
|
|
set_coords(x, y, z, -175, 0, -90, 70, 2)
|
|
time.sleep(2.5)
|
|
except Exception:
|
|
pass
|
|
|
|
|
|
for i in range(1,4):
|
|
try:
|
|
set_coords(x, y, z - i * 10, -175, 0, -90, 70, 2)
|
|
time.sleep(.2)
|
|
except Exception:
|
|
pass
|
|
|
|
# pump on
|
|
try:
|
|
set_coords(x, y, z + 20, -175, 0, -90, 70, 2)
|
|
time.sleep(2.5)
|
|
except Exception:
|
|
pass
|
|
|
|
# ...
|
|
|
|
# finally
|
|
flag = True
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def gipper_subscriber():
|
|
global set_coords, set_angles
|
|
# rospy.wait_for_service('get_joint_angles')
|
|
rospy.wait_for_service('set_joint_angles')
|
|
# rospy.wait_for_service('get_joint_coords')
|
|
rospy.wait_for_service('set_joint_coords')
|
|
try:
|
|
# get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
|
|
set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
|
|
# get_angles = rospy.ServiceProxy('get_joint_angles', GetAngles)
|
|
set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles)
|
|
except:
|
|
print('start error ...')
|
|
exit(1)
|
|
|
|
try:
|
|
set_angles(0, 30, -50, -40, 0, 0, 50)
|
|
except Exception:
|
|
pass
|
|
time.sleep(2.5)
|
|
rospy.init_node('gipper_subscriber',anonymous=True)
|
|
rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1)
|
|
print 'gripper test'
|
|
rospy.spin()
|
|
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
gipper_subscriber()
|
|
|