Merge branch 'vision-dev-l' into main

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}
 )

launch/communication_topic.launch

@@ -0,0 +1,10 @@
+<launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<!-- Open communication service -->
+	<node name="mycobot_services" pkg="mycobot_ros" type="mycobot_topics.py" output="screen">
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+    </node>
+</launch>

launch/mycobot_detect_marker_with_topic.launch

@@ -0,0 +1,29 @@
+<launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_ros)/urdf/mycobot_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_ros)/config/mycobot_with_marker.rviz" />
+	<arg name="gui" default="false" />
+
+	<arg name="num" default="0" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+	<!-- mycobot-topics -->
+    <include file="$(find mycobot_ros)/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="mycobot_ros" type="listen_real_of_topic.py" />
+	<!-- vision node -->
+	<node name="opencv_camera" pkg="mycobot_ros" type="opencv_camera" args="$(arg num)"/>
+	<node name="detect_marker" pkg="mycobot_ros" type="detect_marker.py" />
+	<node name="following_marker" pkg="mycobot_ros" type="following_marker.py" />
+</launch>

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

msg/MycobotCoords.msg

@@ -0,0 +1,6 @@
+float32 x
+float32 y
+float32 z
+float32 rx
+float32 ry
+float32 rz

msg/MycobotPumpStatus.msg

@@ -0,0 +1 @@
+bool Status

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

msg/MycobotSetCoords.msg

@@ -0,0 +1,9 @@
+float32 x
+float32 y
+float32 z
+float32 rx
+float32 ry
+float32 rz
+
+int8 speed
+int8 model

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()
+        cv.destroyAllWindows()

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()
+    

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()
+    

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]

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()
+    

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),

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

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

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

srv/GripperStatus.srv

@@ -1,3 +1,5 @@
 bool Status
 
----
+---
+
+bool Flag

srv/PumpStatus.srv

@@ -0,0 +1,5 @@
+bool Status
+
+---
+
+bool Flag

srv/SetAngles.srv

@@ -7,4 +7,6 @@ float32 joint_6
 
 int8 speed
 
----
+---
+
+bool Flag

srv/SetCoords.srv

@@ -8,4 +8,6 @@ float32 rz
 int8 speed
 int8 model
 
----
+---
+
+bool Flag