diff --git a/launch/mycobot_detect_marker_with_services.launch b/launch/mycobot_detect_marker_with_services.launch
index a13c536..5d98cc7 100644
--- a/launch/mycobot_detect_marker_with_services.launch
+++ b/launch/mycobot_detect_marker_with_services.launch
@@ -8,14 +8,21 @@
-
-
-
+
+
+
+
+
+
+
+
+
-
-
+
+
+
diff --git a/scripts/follow_and_pump copy.py b/scripts/follow_and_pump copy.py
new file mode 100755
index 0000000..638cb99
--- /dev/null
+++ b/scripts/follow_and_pump copy.py
@@ -0,0 +1,133 @@
+#!/usr/bin/env python2
+import rospy
+from rospy.timer import sleep
+from visualization_msgs import msg
+from visualization_msgs.msg import Marker
+import time
+from mycobot_ros.srv import (
+ GetCoords, SetCoords, GetAngles, SetAngles, PumpStatus)
+
+
+set_coords = None
+set_angles = None
+toggle_pump = None
+
+x_offset = 15
+y_offset = 30
+z_offset = 100
+
+temp_x = 0
+temp_y = 0
+temp_z = 0
+follow_z = 220
+
+flag = False
+
+
+def compare_offset(old, new):
+ count = 0
+ for o, n in zip(old, new):
+ if abs(o - n) < 10:
+ count += 1
+
+ if count == len(old):
+ return True
+
+ return False
+
+
+
+def grippercallback(data):
+ global flag, temp_x, temp_y,temp_z
+ print(type(data))
+ 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'))
+
+ if (temp_x == temp_y == temp_z == 0.0) or (not compare_offset((temp_x, temp_y, temp_z), (x, y, z))):
+ set_coords(x, y, follow_z, -175, 0, -90, 70, 2)
+ temp_x, temp_y, temp_z = x, y, z
+ time.sleep(1)
+ return
+
+ else:
+ print(x, y, z)
+
+ # detect heigth + pump height + limit height + offset
+ x += x_offset
+ y += y_offset
+ z = z + 88 + 25 + z_offset
+
+
+ set_coords(x, y, z, -175, 0, -90, 70, 2)
+ time.sleep(1.5)
+
+ for i in range(1,5):
+
+ set_coords(x, y, z - i * 10, -175, 0, -90, 15, 2)
+ time.sleep(.3)
+
+ time.sleep(2)
+
+
+ toggle_pump(1)
+ # pump on
+
+ set_coords(x, y, z + 20, -165, 0, -90, 70, 2)
+ time.sleep(1.5)
+
+ set_angles(0, 30, -50, -40, 0, 0, 50)
+ time.sleep(1.5)
+
+ put_z = 140
+ set_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2)
+ time.sleep(1.5)
+
+ for i in range (1,5):
+ set_coords(39.4, -174.7, put_z-i*20, -177.13, -4.13, -152.59,15,2)
+ time.sleep(.3)
+
+ toggle_pump(0)
+
+ set_angles(0, 30, -50, -40, 0, 0, 50)
+ time.sleep(1.5)
+
+ # finally
+ flag = True
+
+def gipper_subscriber():
+ global set_coords, set_angles, toggle_pump
+ # 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')
+ rospy.wait_for_service('switch_pump_status')
+ 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)
+ toggle_pump =rospy.ServiceProxy('switch_pump_status', PumpStatus)
+ except:
+ print('start error ...')
+ exit(1)
+
+
+ set_angles(0, 30, -50, -40, 0, 0, 50)
+
+ 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()
+
\ No newline at end of file
diff --git a/scripts/follow_and_pump.py b/scripts/follow_and_pump.py
new file mode 100755
index 0000000..c043f8e
--- /dev/null
+++ b/scripts/follow_and_pump.py
@@ -0,0 +1,148 @@
+#!/usr/bin/env python2
+import rospy
+from visualization_msgs.msg import Marker
+import time
+import random
+
+from mycobot_ros.msg import (MycobotSetAngles, MycobotSetCoords,MycobotPumpStatus)
+
+
+rospy.init_node('gipper_subscriber',anonymous=True)
+angle_pub = rospy.Publisher('mycobot/angles_goal', MycobotSetAngles, queue_size=5)
+coord_pub = rospy.Publisher('mycobot/coords_goal', MycobotSetCoords, queue_size=5)
+pump_pub = rospy.Publisher('mycobot/pump_status', MycobotPumpStatus, queue_size=5)
+
+angles = MycobotSetAngles()
+coords = MycobotSetCoords()
+pump = MycobotPumpStatus()
+
+x_offset = -20
+y_offset = 20
+z_offset = 120
+
+flag = False
+
+temp_x = temp_y = temp_z = 0.0
+
+temp_time = time.time()
+
+
+def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70 , m=2):
+ coords.x = x
+ coords.y = y
+ coords.z = z
+ coords.rx = rx
+ coords.ry = ry
+ coords.rz = rz
+ coords.speed = 70
+ coords.model = m
+ # print(coords)
+ coord_pub.publish(coords)
+
+def pub_angles(a, b, c, d, e, f, sp):
+ angles.joint_1 = float(a)
+ angles.joint_2 = float(b)
+ angles.joint_3 = float(c)
+ angles.joint_4 = float(d)
+ angles.joint_5 = float(e)
+ angles.joint_6 = float(f)
+ angles.speed = sp
+ angle_pub.publish(angles)
+
+def pub_pump(flag):
+ pump.Status = flag
+ pump_pub.publish(pump)
+
+def target_is_moving(x, y, z):
+ count = 0
+ for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
+ print(o, n)
+ if abs(o - n) < 2:
+ count += 1
+ print(count)
+ if count == 3:
+ return False
+ return True
+
+
+def grippercallback(data):
+ global flag, temp_x, temp_y, temp_z
+ # rospy.loginfo('gripper_subscriber get date :%s', data)
+ 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'))
+
+ if time.time() - temp_time < 30 or (temp_x == temp_y == temp_z == 0.0) or target_is_moving(x - x_offset, y - y_offset ,z):
+
+ x -= x_offset
+ y -= y_offset
+ pub_coords(x, y, 280)
+ time.sleep(.1)
+
+ temp_x, temp_y, temp_z = x, y, z
+ return
+ else:
+ print(x, y, z)
+
+ # detect heigth + pump height + limit height + offset
+ x += x_offset
+ y += y_offset
+ z = z + 88 + 25 + z_offset
+
+
+ pub_coords(x, y, z)
+ time.sleep(2.5)
+
+ # down
+ for i in range(1,14):
+ pub_coords(x, y, z - i * 5,rx=-160, sp=10)
+ time.sleep(.1)
+
+ time.sleep(2)
+
+ pub_pump(True)
+ # pump on
+
+ pub_coords(x, y, z + 20, -165)
+ time.sleep(1.5)
+
+ pub_angles(0, 30, -50, -40, 0, 0, 50)
+ time.sleep(1.5)
+
+ put_z = 140
+ pub_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2)
+ time.sleep(1.5)
+
+ for i in range (1,7):
+ pub_coords(39.4, -174.7, put_z-i*5, -177.13, -4.13, -152.59,15,2)
+ time.sleep(.1)
+
+ pub_pump(False)
+
+ pub_angles(0, 30, -50, -40, 0, 0, 50)
+ time.sleep(1.5)
+
+ # finally
+ flag = True
+
+def main():
+ for _ in range(10):
+ # pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
+ pub_angles(0, 30, -50, -40, 0, 0, 50)
+ # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
+ time.sleep(.5)
+
+ pub_pump(False)
+ # time.sleep(2.5)
+ rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1)
+ print 'gripper test'
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
+
\ No newline at end of file
diff --git a/scripts/gripper_control.py b/scripts/gripper_control.py
index 6baac16..c34ffe8 100755
--- a/scripts/gripper_control.py
+++ b/scripts/gripper_control.py
@@ -3,6 +3,7 @@
import rospy
+from rospy.timer import sleep
from visualization_msgs import msg
from visualization_msgs.msg import Marker
# from pymycobot.mycobot import MyCobot
@@ -10,15 +11,18 @@ from visualization_msgs.msg import Marker
# 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)
+ GetCoords, SetCoords, GetAngles, SetAngles, PumpStatus)
set_coords = None
set_angles = None
+toggle_pump = None
-x_offset = 0
+
+
+x_offset = 15
y_offset = 30
-z_offset = 60
+z_offset = 100
flag = False
@@ -47,56 +51,63 @@ def grippercallback(data):
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
+
+ set_coords(x, y, z, -175, 0, -90, 70, 2)
+ time.sleep(2.5)
+
+ for i in range(1,5):
+
+ set_coords(x, y, z - i * 10, -175, 0, -90, 15, 2)
+ time.sleep(.3)
+
+ time.sleep(2)
- 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
-
+ toggle_pump(1)
# pump on
- try:
- set_coords(x, y, z + 20, -175, 0, -90, 70, 2)
- time.sleep(2.5)
- except Exception:
- pass
- # ...
+ set_coords(x, y, z + 20, -165, 0, -90, 70, 2)
+ time.sleep(1.5)
+
+ set_angles(0, 30, -50, -40, 0, 0, 50)
+ time.sleep(1.5)
+
+ put_z = 140
+ set_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2)
+ time.sleep(1.5)
+
+ for i in range (1,5):
+ set_coords(39.4, -174.7, put_z-i*20, -177.13, -4.13, -152.59,15,2)
+ time.sleep(.3)
+
+ toggle_pump(0)
+
+ set_angles(0, 30, -50, -40, 0, 0, 50)
+ time.sleep(1.5)
# finally
flag = True
-
-
-
-
-
def gipper_subscriber():
- global set_coords, set_angles
+ global set_coords, set_angles, toggle_pump
# 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')
+ rospy.wait_for_service('switch_pump_status')
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)
+ toggle_pump =rospy.ServiceProxy('switch_pump_status', PumpStatus)
except:
print('start error ...')
exit(1)
- try:
- set_angles(0, 30, -50, -40, 0, 0, 50)
- except Exception:
- pass
+
+ set_angles(0, 30, -50, -40, 0, 0, 50)
+
time.sleep(2.5)
rospy.init_node('gipper_subscriber',anonymous=True)
rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1)
diff --git a/scripts/listen_real_of_topic.py b/scripts/listen_real_of_topic.py
index cbcca0c..7a20a0c 100755
--- a/scripts/listen_real_of_topic.py
+++ b/scripts/listen_real_of_topic.py
@@ -36,21 +36,20 @@ class Listener(object):
joint_state_send.velocity = [0]
joint_state_send.effort = []
- while not rospy.is_shutdown():
- joint_state_send.header.stamp = rospy.Time.now()
- radians_list = [
- data.joint_1 * (math.pi / 180),
- data.joint_2 * (math.pi / 180),
- data.joint_3 * (math.pi / 180),
- data.joint_4 * (math.pi / 180),
- data.joint_5 * (math.pi / 180),
- data.joint_6 * (math.pi / 180),
- ]
- rospy.loginfo('res: {}'.format(radians_list))
+ joint_state_send.header.stamp = rospy.Time.now()
+ radians_list = [
+ data.joint_1 * (math.pi / 180),
+ data.joint_2 * (math.pi / 180),
+ data.joint_3 * (math.pi / 180),
+ data.joint_4 * (math.pi / 180),
+ data.joint_5 * (math.pi / 180),
+ data.joint_6 * (math.pi / 180),
+ ]
+ rospy.loginfo('res: {}'.format(radians_list))
- joint_state_send.position = radians_list
- self.pub.publish(joint_state_send)
- # rate.sleep()
+ joint_state_send.position = radians_list
+ self.pub.publish(joint_state_send)
+ # rate.sleep()
if __name__ == '__main__':
diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py
index 93e1f52..7c0426a 100755
--- a/scripts/mycobot_topics.py
+++ b/scripts/mycobot_topics.py
@@ -146,7 +146,7 @@ class MycobotTopics(object):
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
sp = int(data.speed)
model = int(data.model)
- self.mc.send_angles(angles, sp, model)
+ self.mc.send_coords(angles, sp, model)
sub = rospy.Subscriber('mycobot/coords_goal', MycobotSetCoords, callback=callback)
rospy.spin()