feat: Improve mycobot topics, add launch file.

- add listen real angle script of topic.
- fix srv error.

CMakeLists.txt

@@ -49,7 +49,9 @@ 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

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>

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,60 @@
+#!/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 = []
+
+        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.position = radians_list
+            self.pub.publish(joint_state_send)
+            # rate.sleep()
+        
+
+if __name__ == '__main__':
+    try:
+        Listener()
+    except rospy.ROSInterruptException:
+        pass

scripts/mycobot_topics.py

@@ -1,5 +1,8 @@
 #!/usr/bin/env python2
 import time
+import os
+import sys
+import signal
 import threading
 
 import rospy
@@ -10,6 +13,49 @@ 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):
@@ -17,8 +63,8 @@ class MycobotTopics(object):
 
         rospy.init_node('mycobot_topics')
         rospy.loginfo('start ...')
-        port = rospy.get_param('~port')
-        baud = rospy.get_param('~baud')
+        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()
@@ -54,7 +100,7 @@ class MycobotTopics(object):
     def pub_real_angles(self):
         pub = rospy.Publisher('mycobot/angles_real', MycobotAngles, queue_size=5)
         ma = MycobotAngles()
-        while True:
+        while not rospy.is_shutdown():
             self.lock.acquire()
             angles = self.mc.get_angles()
             self.lock.release()
@@ -72,7 +118,7 @@ class MycobotTopics(object):
         pub = rospy.Publisher('mycobot/coords_real', MycobotCoords, queue_size=5)
         ma = MycobotCoords()
 
-        while True:
+        while not rospy.is_shutdown():
             self.lock.acquire()
             coords = self.mc.get_coords()
             self.lock.release()
@@ -129,6 +175,7 @@ class MycobotTopics(object):
 
 
 if __name__ == '__main__':
+    Watcher()
     mc_topics = MycobotTopics()
     mc_topics.start()
     # while True:

srv/GetAngles.srv

@@ -1,3 +1,6 @@
+
+---
+
 float32 joint_1
 float32 joint_2
 float32 joint_3