From 5cec7311587b76a71dabf1a3726b4a34994367fd Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Thu, 27 May 2021 11:51:42 +0800 Subject: [PATCH] feat: Improve mycobot topics, add launch file. - add listen real angle script of topic. - fix srv error. --- CMakeLists.txt | 2 ++ launch/communication_topic.launch | 10 ++++++ scripts/listen_real.py | 2 ++ scripts/listen_real_of_topic.py | 60 +++++++++++++++++++++++++++++++ scripts/mycobot_topics.py | 55 +++++++++++++++++++++++++--- srv/GetAngles.srv | 3 ++ 6 files changed, 128 insertions(+), 4 deletions(-) create mode 100644 launch/communication_topic.launch create mode 100755 scripts/listen_real_of_topic.py mode change 100644 => 100755 scripts/mycobot_topics.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 5eec60b..a932d7d 100755 --- a/CMakeLists.txt +++ b/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 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/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..cbcca0c --- /dev/null +++ b/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 diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py old mode 100644 new mode 100755 index d9738e5..93e1f52 --- a/scripts/mycobot_topics.py +++ b/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: diff --git a/srv/GetAngles.srv b/srv/GetAngles.srv index 9aa7d97..6992699 100644 --- a/srv/GetAngles.srv +++ b/srv/GetAngles.srv @@ -1,3 +1,6 @@ + +--- + float32 joint_1 float32 joint_2 float32 joint_3