diff --git a/CMakeLists.txt b/CMakeLists.txt
index 98ed77b..ef5393d 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,7 +16,12 @@ find_package(catkin REQUIRED COMPONENTS
)
add_message_files(FILES
- Gripper.msg
+ MycobotAngles.msg
+ MycobotCoords.msg
+ MycobotSetAngles.msg
+ MycobotSetCoords.msg
+ MycobotGripperStatus.msg
+ MycobotPumpStatus.msg
)
add_service_files(FILES
@@ -25,6 +30,7 @@ add_service_files(FILES
GetCoords.srv
SetCoords.srv
GripperStatus.srv
+ PumpStatus.srv
)
## Generate added messages and services
@@ -43,12 +49,15 @@ catkin_install_python(PROGRAMS
scripts/slider_control.py
scripts/control_marker.py
scripts/mycobot_services.py
+ scripts/mycobot_topics.py
scripts/listen_real.py
+ scripts/listen_real_of_topic.py
scripts/teleop_keyboard.py
scripts/sync_plan.py
scripts/client.py
scripts/detect_marker.py
scripts/following_marker.py
+ scripts/gripper_control.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
diff --git a/launch/communication_topic.launch b/launch/communication_topic.launch
new file mode 100644
index 0000000..9e062ec
--- /dev/null
+++ b/launch/communication_topic.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/mycobot_detect_marker_with_topic.launch b/launch/mycobot_detect_marker_with_topic.launch
new file mode 100644
index 0000000..5d98cc7
--- /dev/null
+++ b/launch/mycobot_detect_marker_with_topic.launch
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/msg/MycobotAngles.msg b/msg/MycobotAngles.msg
new file mode 100644
index 0000000..9aa7d97
--- /dev/null
+++ b/msg/MycobotAngles.msg
@@ -0,0 +1,6 @@
+float32 joint_1
+float32 joint_2
+float32 joint_3
+float32 joint_4
+float32 joint_5
+float32 joint_6
diff --git a/msg/MycobotCoords.msg b/msg/MycobotCoords.msg
new file mode 100644
index 0000000..08f6dbd
--- /dev/null
+++ b/msg/MycobotCoords.msg
@@ -0,0 +1,6 @@
+float32 x
+float32 y
+float32 z
+float32 rx
+float32 ry
+float32 rz
\ No newline at end of file
diff --git a/msg/Gripper.msg b/msg/MycobotGripperStatus.msg
similarity index 100%
rename from msg/Gripper.msg
rename to msg/MycobotGripperStatus.msg
diff --git a/msg/MycobotPumpStatus.msg b/msg/MycobotPumpStatus.msg
new file mode 100644
index 0000000..635a864
--- /dev/null
+++ b/msg/MycobotPumpStatus.msg
@@ -0,0 +1 @@
+bool Status
diff --git a/msg/MycobotSetAngles.msg b/msg/MycobotSetAngles.msg
new file mode 100644
index 0000000..3888b50
--- /dev/null
+++ b/msg/MycobotSetAngles.msg
@@ -0,0 +1,8 @@
+float32 joint_1
+float32 joint_2
+float32 joint_3
+float32 joint_4
+float32 joint_5
+float32 joint_6
+
+int8 speed
diff --git a/msg/MycobotSetCoords.msg b/msg/MycobotSetCoords.msg
new file mode 100644
index 0000000..d4c7e54
--- /dev/null
+++ b/msg/MycobotSetCoords.msg
@@ -0,0 +1,9 @@
+float32 x
+float32 y
+float32 z
+float32 rx
+float32 ry
+float32 rz
+
+int8 speed
+int8 model
diff --git a/scripts/detect_marker.py b/scripts/detect_marker.py
index 867a500..d6734fe 100755
--- a/scripts/detect_marker.py
+++ b/scripts/detect_marker.py
@@ -69,6 +69,8 @@ class ImageConverter:
# Just process first one detected.
xyz = tvec[0, 0, :]
+ xyz = [xyz[0] - 0.045, xyz[1], xyz[2] -0.03]
+
euler = rvec[0, 0, :]
tf_change = tf.transformations.quaternion_from_euler(euler[0], euler[1], euler[2])
print('tf_change:', tf_change)
@@ -91,4 +93,4 @@ if __name__ == '__main__':
rospy.spin()
except KeyboardInterrupt:
print "Shutting down cv_bridge_test node."
- cv.destroyAllWindows()
\ No newline at end of file
+ cv.destroyAllWindows()
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..b2fc2dd
--- /dev/null
+++ b/scripts/follow_and_pump.py
@@ -0,0 +1,150 @@
+#!/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 = 110
+
+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,17):
+ 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,8):
+ pub_coords(39.4, -174.7, put_z-i*5, -177.13, -4.13, -152.59,15,2)
+ time.sleep(.1)
+
+ pub_pump(False)
+
+ time.sleep(.5)
+
+ 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/following_marker.py b/scripts/following_marker.py
index 3a882b4..e2cb6a5 100755
--- a/scripts/following_marker.py
+++ b/scripts/following_marker.py
@@ -45,7 +45,7 @@ def talker():
# marker position initial
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
- marker_.pose.position.z = trans[2] - 0.02
+ marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
diff --git a/scripts/gripper_control.py b/scripts/gripper_control.py
new file mode 100755
index 0000000..c34ffe8
--- /dev/null
+++ b/scripts/gripper_control.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python2
+# license removed for brevity
+
+
+import rospy
+from rospy.timer import sleep
+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, PumpStatus)
+
+
+set_coords = None
+set_angles = None
+toggle_pump = None
+
+
+
+x_offset = 15
+y_offset = 30
+z_offset = 100
+
+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
+
+
+ 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)
+
+
+ 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/listen_real.py b/scripts/listen_real.py
index 8549633..97a1d2e 100755
--- a/scripts/listen_real.py
+++ b/scripts/listen_real.py
@@ -39,6 +39,8 @@ def talker():
joint_state_send.header.stamp = rospy.Time.now()
res = func()
+ if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
+ continue
radians_list = [
res.joint_1 * (math.pi / 180),
res.joint_2 * (math.pi / 180),
diff --git a/scripts/listen_real_of_topic.py b/scripts/listen_real_of_topic.py
new file mode 100755
index 0000000..7a20a0c
--- /dev/null
+++ b/scripts/listen_real_of_topic.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python2
+# license removed for brevity
+import time
+import math
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from mycobot_ros.msg import MycobotAngles
+
+
+class Listener(object):
+ def __init__(self):
+ super(Listener, self).__init__()
+
+ rospy.loginfo('start ...')
+ rospy.init_node('real_listener_1', anonymous=True)
+ self.pub = rospy.Publisher('joint_states', JointState, queue_size=10)
+ self.sub = rospy.Subscriber('mycobot/angles_real', MycobotAngles, self.callback)
+ rate = rospy.Rate(30) # 30hz
+ rospy.spin()
+
+ def callback(self, data):
+ # pub joint state
+ joint_state_send = JointState()
+ joint_state_send.header = Header()
+
+ joint_state_send.name = [
+ 'joint2_to_joint1',
+ 'joint3_to_joint2',
+ 'joint4_to_joint3',
+ 'joint5_to_joint4',
+ 'joint6_to_joint5',
+ 'joint6output_to_joint6'
+ ]
+ joint_state_send.velocity = [0]
+ joint_state_send.effort = []
+
+ 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()
+
+
+if __name__ == '__main__':
+ try:
+ Listener()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/scripts/mycobot_services.py b/scripts/mycobot_services.py
index b6f2940..5bd0cb3 100755
--- a/scripts/mycobot_services.py
+++ b/scripts/mycobot_services.py
@@ -24,6 +24,7 @@ def create_services():
rospy.Service('set_joint_coords', SetCoords, set_coords)
rospy.Service('get_joint_coords', GetCoords, get_coords)
rospy.Service('switch_gripper_status', GripperStatus, switch_status)
+ rospy.Service('switch_pump_status', PumpStatus, toggle_pump)
rospy.loginfo('ready')
rospy.spin()
@@ -42,6 +43,8 @@ def set_angles(req):
if mc:
mc.send_angles(angles, sp)
+ return SetAnglesResponse(True)
+
def get_angles(req):
if mc:
@@ -63,6 +66,8 @@ def set_coords(req):
if mc:
mc.send_coords(coords, sp, mod)
+
+ return SetCoordsResponse(True)
def get_coords(req):
@@ -78,6 +83,20 @@ def switch_status(req):
else:
mc.set_gripper_state(1, 80)
+ return GripperStatusResponse(True)
+
+
+def toggle_pump(req):
+ if mc:
+ if req.Status:
+ mc.set_basic_output(2, 0)
+ mc.set_basic_output(5, 0)
+ else:
+ mc.set_basic_output(2, 1)
+ mc.set_basic_output(5, 1)
+
+ return PumpStatusResponse(True)
+
robot_msg = '''
MyCobot Status
diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py
new file mode 100755
index 0000000..7c0426a
--- /dev/null
+++ b/scripts/mycobot_topics.py
@@ -0,0 +1,184 @@
+#!/usr/bin/env python2
+import time
+import os
+import sys
+import signal
+import threading
+
+import rospy
+
+from mycobot_ros.msg import (MycobotAngles, MycobotCoords, MycobotSetAngles, MycobotSetCoords, MycobotGripperStatus, MycobotPumpStatus)
+from sensor_msgs.msg import JointState
+
+from pymycobot.mycobot import MyCobot
+
+
+class Watcher:
+ """this class solves two problems with multithreaded
+ programs in Python, (1) a signal might be delivered
+ to any thread (which is just a malfeature) and (2) if
+ the thread that gets the signal is waiting, the signal
+ is ignored (which is a bug).
+
+ The watcher is a concurrent process (not thread) that
+ waits for a signal and the process that contains the
+ threads. See Appendix A of The Little Book of Semaphores.
+ http://greenteapress.com/semaphores/
+
+ I have only tested this on Linux. I would expect it to
+ work on the Macintosh and not work on Windows.
+ """
+
+ def __init__(self):
+ """ Creates a child thread, which returns. The parent
+ thread waits for a KeyboardInterrupt and then kills
+ the child thread.
+ """
+ self.child = os.fork()
+ if self.child == 0:
+ return
+ else:
+ self.watch()
+
+ def watch(self):
+ try:
+ os.wait()
+ except KeyboardInterrupt:
+ # I put the capital B in KeyBoardInterrupt so I can
+ # tell when the Watcher gets the SIGINT
+ print 'KeyBoardInterrupt'
+ self.kill()
+ sys.exit()
+
+ def kill(self):
+ try:
+ os.kill(self.child, signal.SIGKILL)
+ except OSError: pass
+
+
+class MycobotTopics(object):
+
+ def __init__(self):
+ super(MycobotTopics, self).__init__()
+
+ rospy.init_node('mycobot_topics')
+ rospy.loginfo('start ...')
+ port = rospy.get_param('~port', '/dev/ttyUSB0')
+ baud = rospy.get_param('~baud', 115200)
+ rospy.loginfo("%s,%s" % (port, baud))
+ self.mc = MyCobot(port, baud)
+ self.lock = threading.Lock()
+
+ def start(self):
+ pa = threading.Thread(target=self.pub_real_angles)
+ pb = threading.Thread(target=self.pub_real_coords)
+ sa = threading.Thread(target=self.sub_set_angles)
+ sb = threading.Thread(target=self.sub_set_coords)
+ sg = threading.Thread(target=self.sub_gripper_status)
+ sp = threading.Thread(target=self.sub_pump_status)
+
+ pa.setDaemon(True)
+ pa.start()
+ pb.setDaemon(True)
+ pb.start()
+ sa.setDaemon(True)
+ sa.start()
+ sb.setDaemon(True)
+ sb.start()
+ sg.setDaemon(True)
+ sg.start()
+ sp.setDaemon(True)
+ sp.start()
+
+ pa.join()
+ pb.join()
+ sa.join()
+ sb.join()
+ sg.join()
+ sp.join()
+
+ def pub_real_angles(self):
+ pub = rospy.Publisher('mycobot/angles_real', MycobotAngles, queue_size=5)
+ ma = MycobotAngles()
+ while not rospy.is_shutdown():
+ self.lock.acquire()
+ angles = self.mc.get_angles()
+ self.lock.release()
+ if angles:
+ ma.joint_1 = angles[0]
+ ma.joint_2 = angles[1]
+ ma.joint_3 = angles[2]
+ ma.joint_4 = angles[3]
+ ma.joint_5 = angles[4]
+ ma.joint_6 = angles[5]
+ pub.publish(ma)
+ time.sleep(.25)
+
+ def pub_real_coords(self):
+ pub = rospy.Publisher('mycobot/coords_real', MycobotCoords, queue_size=5)
+ ma = MycobotCoords()
+
+ while not rospy.is_shutdown():
+ self.lock.acquire()
+ coords = self.mc.get_coords()
+ self.lock.release()
+ if coords:
+ ma.x = coords[0]
+ ma.y = coords[1]
+ ma.z = coords[2]
+ ma.rx = coords[3]
+ ma.ry = coords[4]
+ ma.rz = coords[5]
+ pub.publish(ma)
+ time.sleep(.25)
+
+ def sub_set_angles(self):
+ def callback(data):
+ angles = [data.joint_1, data.joint_2, data.joint_3, data.joint_4, data.joint_5, data.joint_6]
+ sp = int(data.speed)
+ self.mc.send_angles(angles, sp)
+
+ sub = rospy.Subscriber('mycobot/angles_goal', MycobotSetAngles, callback=callback)
+ rospy.spin()
+
+ def sub_set_coords(self):
+ def callback(data):
+ angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
+ sp = int(data.speed)
+ model = int(data.model)
+ self.mc.send_coords(angles, sp, model)
+
+ sub = rospy.Subscriber('mycobot/coords_goal', MycobotSetCoords, callback=callback)
+ rospy.spin()
+
+ def sub_gripper_status(self):
+ def callback(data):
+ if data.Status:
+ self.mc.set_gripper_state(0, 80)
+ else:
+ self.mc.set_gripper_state(1, 80)
+
+ sub = rospy.Subscriber('mycobot/gripper_status', MycobotGripperStatus, callback=callback)
+ rospy.spin()
+
+ def sub_pump_status(self):
+ def callback(data):
+ if data.Status:
+ self.mc.set_basic_output(2, 0)
+ self.mc.set_basic_output(5, 0)
+ else:
+ self.mc.set_basic_output(2, 1)
+ self.mc.set_basic_output(5, 1)
+
+ sub = rospy.Subscriber('mycobot/pump_status', MycobotPumpStatus, callback=callback)
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ Watcher()
+ mc_topics = MycobotTopics()
+ mc_topics.start()
+ # while True:
+ # mc_topics.pub_real_coords()
+ # mc_topics.sub_set_angles()
+ pass
\ No newline at end of file
diff --git a/srv/GripperStatus.srv b/srv/GripperStatus.srv
index 424572f..3887d75 100644
--- a/srv/GripperStatus.srv
+++ b/srv/GripperStatus.srv
@@ -1,3 +1,5 @@
bool Status
----
\ No newline at end of file
+---
+
+bool Flag
\ No newline at end of file
diff --git a/srv/PumpStatus.srv b/srv/PumpStatus.srv
new file mode 100644
index 0000000..3887d75
--- /dev/null
+++ b/srv/PumpStatus.srv
@@ -0,0 +1,5 @@
+bool Status
+
+---
+
+bool Flag
\ No newline at end of file
diff --git a/srv/SetAngles.srv b/srv/SetAngles.srv
index 1ad93cd..6f8d877 100644
--- a/srv/SetAngles.srv
+++ b/srv/SetAngles.srv
@@ -7,4 +7,6 @@ float32 joint_6
int8 speed
----
\ No newline at end of file
+---
+
+bool Flag
\ No newline at end of file
diff --git a/srv/SetCoords.srv b/srv/SetCoords.srv
index b264760..e5b1e52 100644
--- a/srv/SetCoords.srv
+++ b/srv/SetCoords.srv
@@ -8,4 +8,6 @@ float32 rz
int8 speed
int8 model
----
\ No newline at end of file
+---
+
+bool Flag
\ No newline at end of file