#!/usr/bin/env python3 # -*- coding:utf-8 -*- """ mycobot_topics.py This ROS node manages the real-time state and control of a Pro450 robotic arm. It publishes: - Joint angles - End-effector coordinates It subscribes to: - Set joint angles - Set end-effector coordinates - Gripper status - Fresh mode status This node uses threading and locking to ensure safe access to the robot over serial communication. Author: WangWeiJian Date: 2025-09-08 """ import time import os import sys import signal import threading import traceback import rospy from std_msgs.msg import UInt8 from mycobot_pro450_communication.msg import ( MycobotAngles, MycobotCoords, MycobotSetAngles, MycobotSetCoords, MycobotGripperStatus, MycobotSetFreshMode, ) import pymycobot from packaging import version # Minimum required pymycobot version MIN_REQUIRE_VERSION = '3.9.9' current_verison = pymycobot.__version__ print('Current pymycobot library version: {}'.format(current_verison)) if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION): raise RuntimeError( 'The version of pymycobot library must be greater than {} or higher. ' 'The current version is {}. Please upgrade the library version.'.format( MIN_REQUIRE_VERSION, current_verison ) ) else: print('pymycobot library version meets the requirements!') from pymycobot import Pro450Client robot_msg = """ MyCobot Status -------------------------------- Joint Limit: joint 1: -165 ~ +165 joint 2: -120 ~ +120 joint 3: -158 ~ +158 joint 4: -165 ~ +165 joint 5: -165 ~ +165 joint 6: -175 ~ +175 """ class Watcher: """Watcher class handles KeyboardInterrupts in multithreaded Python programs. This approach prevents signals from being ignored in threads and ensures proper termination of child processes. Tested on Linux. """ def __init__(self): """Forks a child process to monitor signals.""" self.child = os.fork() if self.child == 0: return else: self.watch() def watch(self): """Wait for child process or handle KeyboardInterrupt to terminate child.""" try: os.wait() except KeyboardInterrupt: print("KeyboardInterrupt caught in Watcher") self.kill() sys.exit() def kill(self): """Kill the child process.""" try: os.kill(self.child, signal.SIGKILL) except OSError: pass class MycobotTopics: """ROS node class to manage MyCobot real-time topics.""" def __init__(self): """Initialize ROS node, connect to robot, and prepare lock.""" super(MycobotTopics, self).__init__() rospy.loginfo("Starting MyCobotTopics node...") rospy.init_node("mycobot_topics") ip = rospy.get_param("~ip", '192.168.0.232') port = rospy.get_param("~port", 4500) rospy.loginfo("%s,%s" % (ip, port)) self.mc = Pro450Client(ip, port) self.lock = threading.Lock() self.output_robot_message() def start(self): """Start all publisher and subscriber threads.""" threads = [ threading.Thread(target=self.pub_real_angles), threading.Thread(target=self.pub_real_coords), threading.Thread(target=self.sub_set_angles), threading.Thread(target=self.sub_set_coords), threading.Thread(target=self.sub_gripper_status), threading.Thread(target=self.sub_fresh_mode_status), ] for t in threads: t.setDaemon(True) t.start() for t in threads: t.join() def pub_real_angles(self): """Publish real joint angles to 'mycobot/angles_real' topic.""" pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5) ma = MycobotAngles() while not rospy.is_shutdown(): with self.lock: try: angles = self.mc.get_angles() if isinstance(angles, list) and len(angles) == 6 and all(c != -1 for c in angles): ma.joint_1, ma.joint_2, ma.joint_3, ma.joint_4, ma.joint_5, ma.joint_6 = angles pub.publish(ma) else: rospy.logwarn("Invalid angles received") except Exception: e = traceback.format_exc() rospy.logerr(f"SerialException: {e}") time.sleep(0.25) def pub_real_coords(self): """Publish real coordinates to 'mycobot/coords_real' topic.""" pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5) mc_msg = MycobotCoords() while not rospy.is_shutdown(): with self.lock: try: coords = self.mc.get_coords() if isinstance(coords, list) and len(coords) == 6 and all(c != -1 for c in coords): mc_msg.x, mc_msg.y, mc_msg.z = coords[0], coords[1], coords[2] mc_msg.rx, mc_msg.ry, mc_msg.rz = coords[3], coords[4], coords[5] pub.publish(mc_msg) else: rospy.logwarn("Invalid coordinates received") except Exception: e = traceback.format_exc() rospy.logerr(f"SerialException: {e}") time.sleep(0.25) def sub_set_angles(self): """Subscribe to 'mycobot/angles_goal' to receive target angles.""" def callback(data: MycobotSetAngles): 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) rospy.Subscriber("mycobot/angles_goal", MycobotSetAngles, callback) rospy.spin() def sub_set_coords(self): """Subscribe to 'mycobot/coords_goal' to receive target coordinates.""" def callback(data: MycobotSetCoords): coords = [data.x, data.y, data.z, data.rx, data.ry, data.rz] sp = int(data.speed) self.mc.send_coords(coords, sp) rospy.Subscriber("mycobot/coords_goal", MycobotSetCoords, callback) rospy.spin() def sub_gripper_status(self): """Subscribe to 'mycobot/gripper_status' to open/close gripper.""" def callback(data: MycobotGripperStatus): if data.Status: self.mc.set_pro_gripper_open() else: self.mc.set_pro_gripper_close() rospy.Subscriber("mycobot/gripper_status", MycobotGripperStatus, callback) rospy.spin() def sub_fresh_mode_status(self): """Subscribe to 'mycobot/fresh_mode_status' to switch fresh mode.""" def callback(data: MycobotSetFreshMode): self.mc.set_fresh_mode(1 if data.Status == 1 else 0) rospy.Subscriber("mycobot/fresh_mode_status", MycobotSetFreshMode, callback) rospy.spin() def output_robot_message(self): """Print robot joint limits and status information.""" connect_status = False servo_infomation = "unknown" servo_temperature = "unknown" atom_version = "unknown" print(robot_msg) if __name__ == "__main__": Watcher() mc_topics = MycobotTopics() mc_topics.start()