280M5 repaired and optimized ros1 keyboard control movement effect, and changed to topic communication

mycobot_280/mycobot_280/CMakeLists.txt

@@ -26,6 +26,7 @@ catkin_install_python(PROGRAMS
   scripts/teleop_keyboard.py
   scripts/listen_real.py
   scripts/listen_real_of_topic.py
+  scripts/listen_real_of_topic_gripper.py
   scripts/simple_gui.py
   scripts/follow_display_gripper.py
   scripts/slider_control_gripper.py

mycobot_280/mycobot_280/launch/teleop_keyboard.launch

@@ -14,10 +14,10 @@
 	<!-- Show in Rviz ，显示在Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
-    <include file="$(find mycobot_communication)/launch/communication_service.launch">
+    <include file="$(find mycobot_communication)/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_280" type="listen_real.py" />
+	<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
 </launch>

mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch

@@ -16,13 +16,13 @@
 	<!-- Show in Rviz ，显示在Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
-    <include file="$(find mycobot_communication)/launch/communication_service.launch">
+    <include file="$(find mycobot_communication)/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_gripper" pkg="mycobot_280" type="listen_real_gripper.py" /> -->
-	<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" output="screen">
+	<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_of_topic_gripper.py" output="screen">
         <param name="port" value="$(arg port)" />
         <param name="baud" value="$(arg baud)" />
     </node>

mycobot_280/mycobot_280/launch/teleop_keyboard_pump.launch

@@ -14,10 +14,10 @@
 	<!-- Show in Rviz ，显示在Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
-    <include file="$(find mycobot_communication)/launch/communication_service.launch">
+    <include file="$(find mycobot_communication)/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_280" type="listen_real.py" />
+	<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
 </launch>

mycobot_280/mycobot_280/scripts/listen_real_of_topic.py

@@ -51,7 +51,7 @@ class Listener(object):
             data.joint_5 * (math.pi / 180),
             data.joint_6 * (math.pi / 180),
         ]
-        rospy.loginfo("res: {}".format(radians_list))
+        # rospy.loginfo("res: {}".format(radians_list))
 
         joint_state_send.position = radians_list
         self.pub.publish(joint_state_send)

mycobot_280/mycobot_280/scripts/listen_real_of_topic_gripper.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+
+import math
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from mycobot_communication.msg import MycobotAngles, MycobotGetGripperValue
+
+
+class Listener(object):
+    def __init__(self):
+        super(Listener, self).__init__()
+
+        rospy.loginfo("start ...")
+        rospy.init_node("real_listener_gripper", anonymous=True)
+        self.gripper_angle = 0.0
+        # init publisher.初始化发布者
+        self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+        # init subscriber.初始化订阅者
+        self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback)
+        self.sub2 = rospy.Subscriber("mycobot/gripper_angle_real", MycobotGetGripperValue, self.gripper_callback)
+        rospy.spin()
+
+    def gripper_callback(self, data):
+        """夹爪角度更新"""
+        self.gripper_angle = data.gripper_angle
+    
+    def callback(self, data):
+        """`mycobot/angles_real` subscriber callback method.
+
+        Args:
+            data (MycobotAngles): callback argument.
+        """
+        # ini publisher object. 初始化发布者对象
+        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",
+            "gripper_controller",
+        ]
+        joint_state_send.velocity = [0]
+        joint_state_send.effort = []
+        joint_state_send.header.stamp = rospy.Time.now()
+
+        # process callback data. 处理回调数据。
+        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),
+            self.gripper_angle * math.pi / 180.0,
+        ]
+        # rospy.loginfo("res: {}".format(radians_list))
+
+        joint_state_send.position = radians_list
+        self.pub.publish(joint_state_send)
+
+
+if __name__ == "__main__":
+    try:
+        Listener()
+    except rospy.ROSInterruptException:
+        pass

mycobot_280/mycobot_280/scripts/teleop_keyboard.py

@@ -1,49 +1,63 @@
 #!/usr/bin/env python3
+# -*- coding:utf-8 -*-
 from __future__ import print_function
-from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus, PumpStatus
-import rospy
 import sys
-import select
+import time
 import termios
 import tty
-import time
+import math
+import rospy
 
-import roslib
+from mycobot_communication.msg import (
+    MycobotAngles,
+    MycobotCoords,
+    MycobotSetAngles,
+    MycobotSetCoords,
+    MycobotGripperStatus,
+    MycobotPumpStatus,
+)
 
-# Terminal output prompt information. 终端输出提示信息
 msg = """\
-Mycobot Teleop Keyboard Controller
+Mycobot Teleop Keyboard Controller (ROS1 - Topic Version)
----------------------------
+---------------------------------------------------------
-Movimg options(control coordinations [x,y,z,rx,ry,rz]):
+Movement (Cartesian):
               w (x+)
-
+    a (y+)    s (x-)    d (y-)
-    a(y-)     s(x-)     d(y+)
-
               z (z-)    x (z+)
 
+Rotation (Euler angles):
     u (rx+)  i (ry+)  o (rz+)
     j (rx-)  k (ry-)  l (rz-)
 
-Gripper control:
+Movement Step:
-    g - open
+    + : Increase movement step size
-    h - close
+    - : Decrease movement step size
 
-Pump control:
+Gripper:
-    b - open
+    g - open    h - close
-    m - close
+
+Pump:
+    b - open    m - close
 
 Other:
     1 - Go to init pose
     2 - Go to home pose
-    3 - Resave home pose
+    3 - Save current pose as home
     q - Quit
 """
 
+COORD_LIMITS = {
+    'x': (-281.45, 281.45),
+    'y': (-281.45, 281.45),
+    'z': (-70, 412.67),
+    'rx': (-180, 180),
+    'ry': (-180, 180),
+    'rz': (-180, 180)
+}
 
 def vels(speed, turn):
     return "currently:\tspeed: %s\tchange percent: %s  " % (speed, turn)
 
-
 class Raw(object):
     def __init__(self, stream):
         self.stream = stream
@@ -57,129 +71,139 @@ class Raw(object):
         termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
 
 
-def teleop_keyboard():
+class MycobotTeleopTopic:
-    rospy.init_node("teleop_keyboard")
+    def __init__(self):
+        rospy.init_node("teleop_keyboard_topic")
 
-    model = 0
+        self.speed = rospy.get_param("~speed", 50)
-    speed = rospy.get_param("~speed", 50)
+        self.model = 1
-    change_percent = rospy.get_param("~change_percent", 5)
+        self.change_percent = rospy.get_param("~change_percent", 5)
+        self.change_len = 250 * self.change_percent / 100.0
+        self.change_angle = 180 * self.change_percent / 100.0
 
-    change_angle = 180 * change_percent / 100
+        # 当前坐标和角度
-    change_len = 250 * change_percent / 100
+        self.curr_coords = [0] * 6
+        self.curr_angles = [0] * 6
 
-    rospy.wait_for_service("get_joint_angles")
+        # 保存坐标作为目标值
-    rospy.wait_for_service("set_joint_angles")
+        self.record_coords = None
-    rospy.wait_for_service("get_joint_coords")
+        self.home_pose = [0, 8, -127, 40, 0, 0]
-    rospy.wait_for_service("set_joint_coords")
-    rospy.wait_for_service("switch_gripper_status")
-    rospy.wait_for_service("switch_pump_status")
-    print("service ready.")
-    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)
-        switch_gripper = rospy.ServiceProxy(
-            "switch_gripper_status", GripperStatus)
-        switch_pump = rospy.ServiceProxy(
-            "switch_pump_status", PumpStatus)
-    except:
-        print("start error ...")
-        exit(1)
 
-    init_pose = [0, 0, 0, 0, 0, 0, speed]
+        # 订阅器
-    home_pose = [0, 8, -127, 40, 0, 0, speed]
+        rospy.Subscriber("mycobot/coords_real", MycobotCoords, self.coords_callback)
+        rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.angles_callback)
 
-    # rsp = set_angles(*init_pose)
+        # 发布器
+        self.coords_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=1)
+        self.angles_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=1)
+        self.gripper_pub = rospy.Publisher("mycobot/gripper_status", MycobotGripperStatus, queue_size=1)
+        self.pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=1)
 
-    while True:
+        rospy.loginfo("Waiting to receive current coordinates...")
-        res = get_coords()
+        while self.curr_coords == [0] * 6 and not rospy.is_shutdown():
-        if res.x > 1:
-            break
             time.sleep(0.1)
 
-    record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
+        self.record_coords = list(self.curr_coords)
-    print(record_coords)
-
-    try:
         print(msg)
-        print(vels(speed, change_percent))
+        print(vels(self.speed, self.change_percent))
-        # Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
+        rospy.loginfo("Current moving step: position %.1f mm, angle attitude %.1f°", self.change_len, self.change_angle)
-        while 1:
+
+    def coords_callback(self, msg):
+        self.curr_coords = [msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz]
+
+    def angles_callback(self, msg):
+        self.curr_angles = [
+            msg.joint_1, msg.joint_2, msg.joint_3,
+            msg.joint_4, msg.joint_5, msg.joint_6
+        ]
+
+    def send_coords(self):
+        goal = MycobotSetCoords()
+        goal.x, goal.y, goal.z, goal.rx, goal.ry, goal.rz = self.record_coords
+        goal.speed = self.speed
+        goal.model = self.model
+        self.coords_pub.publish(goal)
+
+    def send_angles(self, angles):
+        goal = MycobotSetAngles()
+        goal.joint_1, goal.joint_2, goal.joint_3, goal.joint_4, goal.joint_5, goal.joint_6 = angles
+        goal.speed = self.speed
+        self.angles_pub.publish(goal)
+
+    def run(self):
+        while not rospy.is_shutdown():
             try:
-                # print("\r current coords: %s" % record_coords, end="")
                 with Raw(sys.stdin):
                     key = sys.stdin.read(1)
-                if key == "q":
+            except Exception:
+                continue
+
+            try:
+                if key == 'q':
                     break
-                elif key in ["w", "W"]:
+                elif key == '+':
-                    record_coords[0] += change_len
+                    self.change_percent = min(self.change_percent + 1, 20)  # 最大20%
-                    set_coords(*record_coords)
+                    self.change_angle = 180 * self.change_percent / 100.0
-                elif key in ["s", "S"]:
+                    self.change_len = 250 * self.change_percent / 100.0
-                    record_coords[0] -= change_len
+                    rospy.loginfo("Increase change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
-                    set_coords(*record_coords)
+                        (self.change_percent, self.change_len, self.change_angle))
-                elif key in ["a", "A"]:
+                elif key == '-':
-                    record_coords[1] -= change_len
+                    self.change_percent = max(self.change_percent - 1, 1)  # 最小1%
-                    set_coords(*record_coords)
+                    self.change_angle = 180 * self.change_percent / 100.0
-                elif key in ["d", "D"]:
+                    self.change_len = 250 * self.change_percent / 100.0
-                    record_coords[1] += change_len
+                    rospy.loginfo("Decrease change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
-                    set_coords(*record_coords)
+                        (self.change_percent, self.change_len, self.change_angle))
-                elif key in ["z", "Z"]:
+                    continue
-                    record_coords[2] -= change_len
+                elif key in 'wW': self.record_coords[0] += self.change_len
-                    set_coords(*record_coords)
+                elif key in 'sS': self.record_coords[0] -= self.change_len
-                elif key in ["x", "X"]:
+                elif key in 'aA': self.record_coords[1] += self.change_len
-                    record_coords[2] += change_len
+                elif key in 'dD': self.record_coords[1] -= self.change_len
-                    set_coords(*record_coords)
+                elif key in 'zZ': self.record_coords[2] -= self.change_len
-                elif key in ["u", "U"]:
+                elif key in 'xX': self.record_coords[2] += self.change_len
-                    record_coords[3] += change_angle
+                elif key in 'uU': self.record_coords[3] += self.change_angle
-                    set_coords(*record_coords)
+                elif key in 'jJ': self.record_coords[3] -= self.change_angle
-                elif key in ["j", "J"]:
+                elif key in 'iI': self.record_coords[4] += self.change_angle
-                    record_coords[3] -= change_angle
+                elif key in 'kK': self.record_coords[4] -= self.change_angle
-                    set_coords(*record_coords)
+                elif key in 'oO': self.record_coords[5] += self.change_angle
-                elif key in ["i", "I"]:
+                elif key in 'lL': self.record_coords[5] -= self.change_angle
-                    record_coords[4] += change_angle
+                elif key in 'gG':
-                    set_coords(*record_coords)
+                    self.gripper_pub.publish(MycobotGripperStatus(Status=True))
-                elif key in ["k", "K"]:
+                elif key in 'hH':
-                    record_coords[4] -= change_angle
+                    self.gripper_pub.publish(MycobotGripperStatus(Status=False))
-                    set_coords(*record_coords)
+                elif key in 'bB':
-                elif key in ["o", "O"]:
+                    self.pump_pub.publish(MycobotPumpStatus(Status=True, Pin1=2, Pin2=5))
-                    record_coords[5] += change_angle
+                elif key in 'mM':
-                    set_coords(*record_coords)
+                    self.pump_pub.publish(MycobotPumpStatus(Status=False, Pin1=2, Pin2=5))
-                elif key in ["l", "L"]:
+                elif key == '1':
-                    record_coords[5] -= change_angle
+                    self.send_angles([0, 0, 0, 0, 0, 0])
-                    set_coords(*record_coords)
+                    time.sleep(2)
-                elif key in ["g", "G"]:
+                    self.record_coords = list(self.curr_coords)
-                    switch_gripper(True)
+                elif key == '2':
-                elif key in ["h", "H"]:
+                    self.send_angles(self.home_pose)
-                    switch_gripper(False)
+                    time.sleep(2)
-                elif key in ["b", "B"]:
+                    self.record_coords = list(self.curr_coords)
-                    switch_pump(True, 2, 5)
+                elif key == '3':
-                elif key in ["m", "M"]:
+                    self.home_pose = list(self.curr_angles)
-                    switch_pump(False, 2, 5)
-                elif key == "1":
-                    rsp = set_angles(*init_pose)
-                elif key in "2":
-                    rsp = set_angles(*home_pose)
-                elif key in "3":
-                    rep = get_angles()
-                    home_pose[0] = rep.joint_1
-                    home_pose[1] = rep.joint_2
-                    home_pose[2] = rep.joint_3
-                    home_pose[3] = rep.joint_4
-                    home_pose[5] = rep.joint_5
                 else:
                     continue
 
+                # 范围检查
+                for val, axis in zip(self.record_coords, ['x', 'y', 'z', 'rx', 'ry', 'rz']):
+                    min_v, max_v = COORD_LIMITS[axis]
+                    if not (min_v <= val <= max_v):
+                        rospy.logwarn(f"{axis} Out of range: {val} not in [{min_v}, {max_v}]")
+                        raise ValueError("Out of range of motion")
+
+                self.send_coords()
+
             except Exception as e:
-                # print(e)
+                rospy.logwarn("Execution failed: {}".format(e))
                 continue
 
-    except Exception as e:
-        print(e)
 
-
+if __name__ == '__main__':
-if __name__ == "__main__":
     try:
-        teleop_keyboard()
+        teleop = MycobotTeleopTopic()
+        teleop.run()
     except rospy.ROSInterruptException:
         pass

mycobot_communication/CMakeLists.txt

@@ -59,6 +59,7 @@ add_message_files(FILES
   MycobotSetEndType.msg
   MycobotSetFreshMode.msg
   MycobotSetToolReference.msg
+  MycobotGetGripperValue.msg
 )
 
 ## Generate services in the 'srv' folder

mycobot_communication/msg/MycobotGetGripperValue.msg

@@ -0,0 +1,2 @@
+int32 gripper_angle
+

mycobot_communication/scripts/mycobot_services.py

@@ -163,11 +163,15 @@ def toggle_pump(req):
     if mc:
         lock = acquire("/tmp/mycobot_lock")
         if req.Status:
-            mc.set_basic_output(2, 0)
             mc.set_basic_output(5, 0)
+            time.sleep(0.05)
         else:
-            mc.set_basic_output(2, 1)
             mc.set_basic_output(5, 1)
+            time.sleep(0.05)
+            mc.set_basic_output(2, 0)
+            time.sleep(0.05)
+            mc.set_basic_output(2, 1)
+            time.sleep(0.05)
         release(lock)
 
 
@@ -178,11 +182,11 @@ robot_msg = """
 MyCobot Status
 --------------------------------
 Joint Limit:
-    joint 1: -170 ~ +170
+    joint 1: -168 ~ +168
-    joint 2: -170 ~ +170
+    joint 2: -135 ~ +135
-    joint 3: -170 ~ +170
+    joint 3: -150 ~ +150
-    joint 4: -170 ~ +170
+    joint 4: -145 ~ +145
-    joint 5: -170 ~ +170
+    joint 5: -165 ~ +165
     joint 6: -180 ~ +180
 
 Connect Status: %s

mycobot_communication/scripts/mycobot_topics.py

@@ -18,6 +18,7 @@ from mycobot_communication.msg import (
     MycobotSetEndType,
     MycobotSetFreshMode,
     MycobotSetToolReference,
+    MycobotGetGripperValue,
 )
 from std_msgs.msg import UInt8
 import pymycobot
@@ -76,14 +77,31 @@ class Watcher:
             os.kill(self.child, signal.SIGKILL)
         except OSError:
             pass
+robot_msg = """
+MyCobot Status
+--------------------------------
+Joint Limit:
+    joint 1: -168 ~ +168
+    joint 2: -135 ~ +135
+    joint 3: -150 ~ +150
+    joint 4: -145 ~ +145
+    joint 5: -165 ~ +165
+    joint 6: -180 ~ +180
 
+Connect Status: %s
+
+Servo Infomation: %s
+
+Servo Temperature: %s
+
+Atom Version: %s
+"""
 
 class MycobotTopics(object):
     def __init__(self):
         super(MycobotTopics, self).__init__()
-
-        rospy.init_node("mycobot_topics")
         rospy.loginfo("start ...")
+        rospy.init_node("mycobot_topics")
         port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
         if not port:
             port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
@@ -91,6 +109,7 @@ class MycobotTopics(object):
         rospy.loginfo("%s,%s" % (port, baud))
         self.mc = MyCobot280(port, baud)
         self.lock = threading.Lock()
+        self.output_robot_message()
 
     def start(self):
         pa = threading.Thread(target=self.pub_real_angles)
@@ -103,6 +122,7 @@ class MycobotTopics(object):
         sfm = threading.Thread(target=self.sub_fresh_mode_status)
         set = threading.Thread(target=self.sub_end_type_status)
         str = threading.Thread(target=self.sub_set_tool_reference)
+        sgv = threading.Thread(target=self.sub_real_gripper_value)
 
         pa.setDaemon(True)
         pa.start()
@@ -123,6 +143,8 @@ class MycobotTopics(object):
         set.start()
         str.setDaemon(True)
         str.start()
+        sgv.setDaemon(True)
+        sgv.start()
 
         pa.join()
         pb.join()
@@ -134,6 +156,7 @@ class MycobotTopics(object):
         sfm.join()
         set.join()
         str.join()
+        sgv.join()
 
     def pub_real_angles(self):
         """Publish real angle"""
@@ -212,6 +235,22 @@ class MycobotTopics(object):
         )
         rospy.spin()
 
+    def sub_real_gripper_value(self):
+        """Get Gripper Value"""
+        pub = rospy.Publisher("mycobot/gripper_angle_real",
+                              MycobotGetGripperValue, queue_size=5)
+        ma = MycobotGetGripperValue()
+        while not rospy.is_shutdown():
+            with self.lock:
+                try:
+                    gripper_value = self.mc.get_gripper_value()
+                    if gripper_value:
+                        ma.gripper_angle = gripper_value
+                        pub.publish(ma)
+                except Exception as e:
+                    rospy.logerr(f"SerialException: {e}")
+            time.sleep(0.25)
+            
     def sub_gripper_status(self):
         """Subscribe to Gripper Status"""
         """订阅夹爪状态"""
@@ -229,11 +268,15 @@ class MycobotTopics(object):
     def sub_pump_status(self):
         def callback(data):
             if data.Status:
-                self.mc.set_basic_output(data.Pin1, 0)
                 self.mc.set_basic_output(data.Pin2, 0)
+                time.sleep(0.05)
             else:
-                self.mc.set_basic_output(data.Pin1, 1)
                 self.mc.set_basic_output(data.Pin2, 1)
+                time.sleep(0.05)
+                self.mc.set_basic_output(data.Pin1, 0)
+                time.sleep(0.05)
+                self.mc.set_basic_output(data.PIn1, 1)
+                time.sleep(0.05)
 
         sub = rospy.Subscriber(
             "mycobot/pump_status", MycobotPumpStatus, callback=callback
@@ -279,6 +322,30 @@ class MycobotTopics(object):
         rospy.spin()
         
 
+    def output_robot_message(self):
+        connect_status = False
+        servo_infomation = "unknown"
+        servo_temperature = "unknown"
+        atom_version = "unknown"
+
+        if self.mc:
+            cn = self.mc.is_controller_connected()
+            if cn == 1:
+                connect_status = True
+            time.sleep(0.1)
+            si = self.mc.is_all_servo_enable()
+            if si == 1:
+                servo_infomation = "all connected"
+            version = self.mc.get_system_version()
+            if version:
+                atom_version = version
+
+        print(
+            robot_msg % (connect_status, servo_infomation,
+                        servo_temperature, atom_version)
+        )
+
+
 if __name__ == "__main__":
     Watcher()
     mc_topics = MycobotTopics()