Merge pull request #126 from elephantrobotics/cobotx_b450

Cobotx b450

Mercury/mercury_a1/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(cobotx_a450)
+project(mercury_a1)
 add_compile_options(-std=c++11)
 
 ## Find catkin and any catkin packages

Mercury/mercury_a1/launch/mycobot_follow.launch

@@ -1,7 +1,7 @@
 <launch>
 	<!-- Load URDF file,加载URDF文件 -->
-	<arg name="model" default="$(find mycobot_description)/urdf/cobotx_a450/cobotx_a450.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
-	<arg name="rvizconfig" default="$(find cobotx_a450)/config/cobotx_a450.rviz" />
+	<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
 	<!-- Combinejoin values to TF ，将值合并到TF-->

Mercury/mercury_a1/launch/simple_gui.launch

@@ -3,8 +3,8 @@
 	<arg name="port" default="/dev/ttyAMA1" />
 	<arg name="baud" default="115200" />
 
-	<arg name="model" default="$(find mycobot_description)/urdf/cobotx_a450/cobotx_a450.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
-	<arg name="rvizconfig" default="$(find cobotx_a450)/config/cobotx_a450.rviz" />
+	<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
 	<arg name="gui" default="false" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
@@ -14,10 +14,10 @@
 	<!-- Show in Rviz ，显示在Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
-    <include file="$(find cobotx_a450_communication)/launch/communication_service.launch">
+    <include file="$(find mercury_a1_communication)/launch/communication_service.launch">
 		<arg name="port" value="$(arg port)" />	
 		<arg name="baud" value="$(arg baud)" />	
     </include>
-	<node name="real_listener" pkg="cobotx_a450" type="listen_real.py" />
+	<node name="real_listener" pkg="mercury_a1" type="listen_real.py" />
-	<node name="simple_gui" pkg="cobotx_a450" type="simple_gui.py" />
+	<node name="simple_gui" pkg="mercury_a1" type="simple_gui.py" />
 </launch>

Mercury/mercury_a1/launch/slider_control.launch

@@ -2,8 +2,8 @@
 	<!-- <arg name="port" default="/dev/ttyUSB0" />
 	<arg name="baud" default="115200" /> -->
 	<!-- Load file model ，加载文件模型-->	
-	<arg name="model" default="$(find mycobot_description)/urdf/cobotx_a450/cobotx_a450.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
-	<arg name="rvizconfig" default="$(find cobotx_a450)/config/cobotx_a450.rviz" />
+	<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
 	<arg name="gui" default="true" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />

Mercury/mercury_a1/launch/teleop_keyboard.launch

@@ -3,8 +3,8 @@
 	<arg name="port" default="/dev/ttyAMA1" />
 	<arg name="baud" default="115200" />
 
-	<arg name="model" default="$(find mycobot_description)/urdf/cobotx_a450/cobotx_a450.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
-	<arg name="rvizconfig" default="$(find cobotx_a450)/config/cobotx_a450.rviz" />
+	<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
 	<arg name="gui" default="false" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
@@ -14,10 +14,10 @@
 	<!-- Show in Rviz ，显示在Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
-    <include file="$(find cobotx_a450_communication)/launch/communication_service.launch">
+    <include file="$(find mercury_a1_communication)/launch/communication_service.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="cobotx_a450" type="listen_real.py" />
+	<node name="real_listener" pkg="mercury_a1" type="listen_real.py" />
 </launch>

Mercury/mercury_a1/launch/test.launch

@@ -1,6 +1,6 @@
 <launch>
-	<arg name="model" default="$(find mycobot_description)/urdf/cobotx_a450/cobotx_a450.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
-	<arg name="rvizconfig" default="$(find cobotx_a450)/config/cobotx_a450.rviz" />
+	<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
 	<arg name="gui" default="true" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />

Mercury/mercury_a1/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>cobotx_a450</name>
+  <name>mercury_a1</name>
   <version>0.3.0</version>
-  <description>The cobotx_a450 package</description>
+  <description>The mercury_a1 package</description>
 
   <author email="weijian.wang@elephantrobotics.com">Wangweijian</author>
   <maintainer email="weijian.wang@elephantrobotics.com">Wangweijian</maintainer>
@@ -18,9 +18,9 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>mycobot_description</build_depend>
-  <build_depend>cobotx_a450_communication</build_depend>
+  <build_depend>mercury_a1_communication</build_depend>
 
-  <build_export_depend>cobotx_a450_communication</build_export_depend>
+  <build_export_depend>mercury_a1_communication</build_export_depend>
   <build_export_depend>mycobot_description</build_export_depend>
 
   <exec_depend>roscpp</exec_depend>
@@ -36,7 +36,7 @@
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>python-tk</exec_depend>
   <exec_depend>mycobot_description</exec_depend>
-  <exec_depend>cobotx_a450_communication</exec_depend>
+  <exec_depend>mercury_a1_communication</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

Mercury/mercury_a1/scripts/follow_display.py

@@ -6,24 +6,24 @@ from sensor_msgs.msg import JointState
 from std_msgs.msg import Header
 from visualization_msgs.msg import Marker
 
-from pymycobot.cobotx import CobotX
+from pymycobot.mercury import Mercury
 
 
 def talker():
     rospy.init_node("display", anonymous=True)
 
-    print("Try connect real CobotX...")
+    print("Try connect real Mercury...")
     port = rospy.get_param("~port", "/dev/ttyAMA1")
     baud = rospy.get_param("~baud", 115200)
     print("port: {}, baud: {}\n".format(port, baud))
     try:
-        mc = CobotX(port, baud)
+        mc = Mercury(port, baud)
     except Exception as e:
         print(e)
         print(
             """\
-            \rTry connect CobotX failed!
+            \rTry connect Mercury failed!
-            \rPlease check wether connected with CobotX.
+            \rPlease check wether connected with Mercury.
             \rPlease chckt wether the port or baud is right.
         """
         )

Mercury/mercury_a1/scripts/listen_real.py

@@ -7,7 +7,7 @@ import math
 import rospy
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Header
-from cobotx_a450_communication.srv import GetAngles
+from mercury_a1_communication.srv import GetAngles
 
 
 def talker():

Mercury/mercury_a1/scripts/listen_real_of_topic.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+
+import math
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from mercury_a1_communication.msg import MercuryAngles
+
+
+class Listener(object):
+    def __init__(self):
+        super(Listener, self).__init__()
+
+        rospy.loginfo("start ...")
+        rospy.init_node("real_listener_1", anonymous=True)
+        # init publisher.初始化发布者
+        self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+        # init subscriber.初始化订阅者
+        self.sub = rospy.Subscriber("myarm/angles_real", MercuryAngles, self.callback)
+        rospy.spin()
+
+    def callback(self, data):
+        """`mercury/angles_real` subscriber callback method.
+
+        Args:
+            data (MercuryAngles): callback argument.
+        """
+        # ini publisher object. 初始化发布者对象
+        joint_state_send = JointState()
+        joint_state_send.header = Header()
+
+        joint_state_send.name = [
+            "joint1_to_base",
+            "joint2_to_joint1",
+            "joint3_to_joint2",
+            "joint4_to_joint3",
+            "joint5_to_joint4",
+            "joint6_to_joint5",
+            "joint7_to_joint6",
+        ]
+        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),
+            data.joint_7 * (math.pi / 180),
+        ]
+        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

Mercury/mercury_a1/scripts/simple_gui.py

@@ -0,0 +1,501 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+try:
+    import tkinter as tk
+except ImportError:
+    import Tkinter as tk
+from mercury_a1_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
+import rospy
+import time
+from rospy import ServiceException
+
+
+class Window:
+    def __init__(self, handle):
+        self.win = handle
+        self.win.resizable(0, 0)  # fixed window size，固定窗口大小
+
+        self.model = 0
+        self.speed = rospy.get_param("~speed", 50)
+
+        # set default speed，设置默认速度
+        self.speed_d = tk.StringVar()
+        self.speed_d.set(str(self.speed))
+        # print(self.speed)
+        self.connect_ser()
+
+        # Get the data of the robotic arm，获取机械臂数据
+        self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model]
+        self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model]
+        self.get_date()
+
+        # get screen width and height.获取屏幕宽度和高度
+        self.ws = self.win.winfo_screenwidth()  # width of the screen
+        self.hs = self.win.winfo_screenheight()  # height of the screen
+        # calculate x and y coordinates for the Tk root window
+        # 计算 Tk 根窗口的 x 和 y 坐标
+        x = int((self.ws / 2) - 190)
+        y = int((self.hs / 2) - 250)
+        self.win.geometry("430x450+{}+{}".format(x, y))
+        # layout,布局
+        self.set_layout()
+        # input section,输入部分
+        self.need_input()
+        # Show part,展示部分
+        self.show_init()
+
+        # Set the joint buttons 设置joint按钮
+        tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid(
+            row=7, column=1, sticky="w", padx=3, pady=2
+        )
+
+        # coordination settings button,coordination 设置按钮
+        tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid(
+            row=7, column=1, sticky="w", padx=3, pady=2
+        )
+
+        # Gripper switch button,夹爪开关按钮
+        tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid(
+            row=1, column=0, sticky="w", padx=3, pady=50
+        )
+        tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid(
+            row=1, column=1, sticky="w", padx=3, pady=2
+        )
+
+    def connect_ser(self):
+        rospy.init_node("simple_gui", anonymous=True, disable_signals=True)
+
+        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_gripper_status")
+        try:
+            self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
+            self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
+            self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
+            self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
+            self.switch_gripper = rospy.ServiceProxy(
+                "switch_gripper_status", GripperStatus
+            )
+        except:
+            print("start error ...")
+            exit(1)
+
+        print("Connect service success.")
+
+    def set_layout(self):
+        self.frmLT = tk.Frame(width=200, height=200)
+        self.frmLC = tk.Frame(width=200, height=200)
+        self.frmLB = tk.Frame(width=200, height=200)
+        self.frmRT = tk.Frame(width=200, height=200)
+        self.frmLT.grid(row=0, column=0, padx=1, pady=3)
+        self.frmLC.grid(row=1, column=0, padx=1, pady=3)
+        self.frmLB.grid(row=1, column=1, padx=2, pady=3)
+        self.frmRT.grid(row=0, column=1, padx=2, pady=3)
+
+    def need_input(self):
+        # input hint,输入提示
+        tk.Label(self.frmLT, text="Joint 1 ").grid(row=0)
+        tk.Label(self.frmLT, text="Joint 2 ").grid(row=1)  # the second row,第二行
+        tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
+        tk.Label(self.frmLT, text="Joint 4 ").grid(row=3)
+        tk.Label(self.frmLT, text="Joint 5 ").grid(row=4)
+        tk.Label(self.frmLT, text="Joint 6 ").grid(row=5)
+        tk.Label(self.frmLT, text="Joint 7 ").grid(row=6)
+
+        tk.Label(self.frmRT, text=" x ").grid(row=0)
+        tk.Label(self.frmRT, text=" y ").grid(row=1)  # the second row,第二行
+        tk.Label(self.frmRT, text=" z ").grid(row=2)
+        tk.Label(self.frmRT, text=" rx ").grid(row=3)
+        tk.Label(self.frmRT, text=" ry ").grid(row=4)
+        tk.Label(self.frmRT, text=" rz ").grid(row=5)
+
+        # Set the default value of the input box,设置输入框的默认值
+        self.j1_default = tk.StringVar()
+        self.j1_default.set(self.res_angles[0])
+        self.j2_default = tk.StringVar()
+        self.j2_default.set(self.res_angles[1])
+        self.j3_default = tk.StringVar()
+        self.j3_default.set(self.res_angles[2])
+        self.j4_default = tk.StringVar()
+        self.j4_default.set(self.res_angles[3])
+        self.j5_default = tk.StringVar()
+        self.j5_default.set(self.res_angles[4])
+        self.j6_default = tk.StringVar()
+        self.j6_default.set(self.res_angles[5])
+        self.j7_default = tk.StringVar()
+        self.j7_default.set(self.res_angles[6])
+
+
+        self.x_default = tk.StringVar()
+        self.x_default.set(self.record_coords[0])
+        self.y_default = tk.StringVar()
+        self.y_default.set(self.record_coords[1])
+        self.z_default = tk.StringVar()
+        self.z_default.set(self.record_coords[2])
+        self.rx_default = tk.StringVar()
+        self.rx_default.set(self.record_coords[3])
+        self.ry_default = tk.StringVar()
+        self.ry_default.set(self.record_coords[4])
+        self.rz_default = tk.StringVar()
+        self.rz_default.set(self.record_coords[5])
+
+        # joint input box,joint 输入框
+        self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
+        self.J_1.grid(row=0, column=1, pady=3)
+        self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
+        self.J_2.grid(row=1, column=1, pady=3)
+        self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default)
+        self.J_3.grid(row=2, column=1, pady=3)
+        self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default)
+        self.J_4.grid(row=3, column=1, pady=3)
+        self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default)
+        self.J_5.grid(row=4, column=1, pady=3)
+        self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default)
+        self.J_6.grid(row=5, column=1, pady=3)
+        self.J_7 = tk.Entry(self.frmLT, textvariable=self.j7_default)
+        self.J_7.grid(row=6, column=1, pady=3)
+
+        # coord input box,coord 输入框
+        self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
+        self.x.grid(row=0, column=1, pady=3, padx=0)
+        self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
+        self.y.grid(row=1, column=1, pady=3)
+        self.z = tk.Entry(self.frmRT, textvariable=self.z_default)
+        self.z.grid(row=2, column=1, pady=3)
+        self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default)
+        self.rx.grid(row=3, column=1, pady=3)
+        self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default)
+        self.ry.grid(row=4, column=1, pady=3)
+        self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default)
+        self.rz.grid(row=5, column=1, pady=3)
+
+        # All input boxes, used to get the input data,所有输入框，用于拿输入的数据
+        self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6, self.J_7]
+        self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz]
+
+        # speed input box,速度输入框
+        tk.Label(
+            self.frmLB,
+            text="speed",
+        ).grid(row=0, column=0)
+        self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10)
+        self.get_speed.grid(row=0, column=1)
+
+    def show_init(self):
+        # show,显示
+        tk.Label(self.frmLC, text="Joint 1 ").grid(row=0)
+        tk.Label(self.frmLC, text="Joint 2 ").grid(row=1)  # the second row,第二行
+        tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
+        tk.Label(self.frmLC, text="Joint 4 ").grid(row=3)
+        tk.Label(self.frmLC, text="Joint 5 ").grid(row=4)
+        tk.Label(self.frmLC, text="Joint 6 ").grid(row=5)
+        tk.Label(self.frmLC, text="Joint 7 ").grid(row=6)
+
+        # get数据
+
+        # show,展示出来
+        self.cont_1 = tk.StringVar(self.frmLC)
+        self.cont_1.set(str(self.res_angles[0]) + "°")
+        self.cont_2 = tk.StringVar(self.frmLC)
+        self.cont_2.set(str(self.res_angles[1]) + "°")
+        self.cont_3 = tk.StringVar(self.frmLC)
+        self.cont_3.set(str(self.res_angles[2]) + "°")
+        self.cont_4 = tk.StringVar(self.frmLC)
+        self.cont_4.set(str(self.res_angles[3]) + "°")
+        self.cont_5 = tk.StringVar(self.frmLC)
+        self.cont_5.set(str(self.res_angles[4]) + "°")
+        self.cont_6 = tk.StringVar(self.frmLC)
+        self.cont_6.set(str(self.res_angles[5]) + "°")
+        self.cont_7 = tk.StringVar(self.frmLC)
+        self.cont_7.set(str(self.res_angles[6]) + "°")
+        self.cont_all = [
+            self.cont_1,
+            self.cont_2,
+            self.cont_3,
+            self.cont_4,
+            self.cont_5,
+            self.cont_6,
+            self.cont_7,
+            self.speed,
+            self.model,
+        ]
+
+        self.show_j1 = tk.Label(
+            self.frmLC,
+            textvariable=self.cont_1,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=0, column=1, padx=0, pady=5)
+
+        self.show_j2 = tk.Label(
+            self.frmLC,
+            textvariable=self.cont_2,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=1, column=1, padx=0, pady=5)
+        self.show_j3 = tk.Label(
+            self.frmLC,
+            textvariable=self.cont_3,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=2, column=1, padx=0, pady=5)
+        self.show_j4 = tk.Label(
+            self.frmLC,
+            textvariable=self.cont_4,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=3, column=1, padx=0, pady=5)
+        self.show_j5 = tk.Label(
+            self.frmLC,
+            textvariable=self.cont_5,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=4, column=1, padx=0, pady=5)
+        self.show_j6 = tk.Label(
+            self.frmLC,
+            textvariable=self.cont_6,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=5, column=1, padx=5, pady=5)
+        self.show_j7 = tk.Label(
+            self.frmLC,
+            textvariable=self.cont_7,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=6, column=1, padx=5, pady=5)
+
+        self.all_jo = [
+            self.show_j1,
+            self.show_j2,
+            self.show_j3,
+            self.show_j4,
+            self.show_j5,
+            self.show_j6,
+            self.show_j7,
+        ]
+
+        # show,显示
+        tk.Label(self.frmLC, text="  x ").grid(row=0, column=3)
+        tk.Label(self.frmLC, text="  y ").grid(row=1, column=3)  
+        tk.Label(self.frmLC, text="  z ").grid(row=2, column=3)
+        tk.Label(self.frmLC, text="  rx ").grid(row=3, column=3)
+        tk.Label(self.frmLC, text="  ry ").grid(row=4, column=3)
+        tk.Label(self.frmLC, text="  rz ").grid(row=5, column=3)
+        self.coord_x = tk.StringVar()
+        self.coord_x.set(str(self.record_coords[0]))
+        self.coord_y = tk.StringVar()
+        self.coord_y.set(str(self.record_coords[1]))
+        self.coord_z = tk.StringVar()
+        self.coord_z.set(str(self.record_coords[2]))
+        self.coord_rx = tk.StringVar()
+        self.coord_rx.set(str(self.record_coords[3]))
+        self.coord_ry = tk.StringVar()
+        self.coord_ry.set(str(self.record_coords[4]))
+        self.coord_rz = tk.StringVar()
+        self.coord_rz.set(str(self.record_coords[5]))
+
+        self.coord_all = [
+            self.coord_x,
+            self.coord_y,
+            self.coord_z,
+            self.coord_rx,
+            self.coord_ry,
+            self.coord_rz,
+            self.speed,
+            self.model,
+        ]
+
+        self.show_x = tk.Label(
+            self.frmLC,
+            textvariable=self.coord_x,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=0, column=4, padx=5, pady=5)
+        self.show_y = tk.Label(
+            self.frmLC,
+            textvariable=self.coord_y,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=1, column=4, padx=5, pady=5)
+        self.show_z = tk.Label(
+            self.frmLC,
+            textvariable=self.coord_z,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=2, column=4, padx=5, pady=5)
+        self.show_rx = tk.Label(
+            self.frmLC,
+            textvariable=self.coord_rx,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=3, column=4, padx=5, pady=5)
+        self.show_ry = tk.Label(
+            self.frmLC,
+            textvariable=self.coord_ry,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=4, column=4, padx=5, pady=5)
+        self.show_rz = tk.Label(
+            self.frmLC,
+            textvariable=self.coord_rz,
+            font=("Arial", 9),
+            width=7,
+            height=1,
+            bg="white",
+        ).grid(row=5, column=4, padx=5, pady=5)
+
+        # mm， Unit show，单位展示
+        self.unit = tk.StringVar()
+        self.unit.set("mm")
+        for i in range(6):
+            tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid(
+                row=i, column=5
+            )
+
+    def gripper_open(self):
+        try:
+            self.switch_gripper(True)
+        except ServiceException:
+            # Probably because the method has no return value, the service throws an unhandled error
+            # 可能由于该方法没有返回值，服务抛出无法处理的错误
+            pass
+
+    def gripper_close(self):
+        try:
+            self.switch_gripper(False)
+        except ServiceException:
+            pass
+
+    def get_coord_input(self):
+        # Get the data input by coord and send it to the robotic arm
+        # 获取 coord 输入的数据，发送给机械臂
+        c_value = []
+        for i in self.all_c:
+            # print(type(i.get()))
+            c_value.append(float(i.get()))
+        self.speed = (
+            int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
+        )
+        c_value.append(self.speed)
+        c_value.append(self.model)
+        # print(c_value)
+        try:
+            self.set_coords(*c_value)
+        except ServiceException:
+            pass
+        self.show_j_date(c_value[:-2], "coord")
+
+    def get_joint_input(self):
+        # Get the data input by the joint and send it to the robotic arm
+        # 获取joint输入的数据，发送给机械臂
+        j_value = []
+        for i in self.all_j:
+            # print(type(i.get()))
+            j_value.append(float(i.get()))
+        self.speed = (
+            int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
+        )
+        j_value.append(self.speed)
+
+        try:
+            self.set_angles(*j_value)
+        except ServiceException:
+            pass
+        self.show_j_date(j_value[:-1])
+        # return j_value,c_value,speed
+
+    def get_date(self):
+        # Take the data of the robotic arm for display.拿机械臂的数据，用于展示
+        t = time.time()
+        while time.time() - t < 2:
+            self.res = self.get_coords()
+            if self.res.x > 1:
+                break
+            time.sleep(0.1)
+
+        t = time.time()
+        while time.time() - t < 2:
+            self.angles = self.get_angles()
+            if self.angles.joint_1 > 1:
+                break
+            time.sleep(0.1)
+        # print(self.angles.joint_1)
+        self.record_coords = [
+            round(self.res.x, 2),
+            round(self.res.y, 2),
+            round(self.res.z, 2),
+            round(self.res.rx, 2),
+            round(self.res.ry, 2),
+            round(self.res.rz, 2),
+            self.speed,
+            self.model,
+        ]
+        self.res_angles = [
+            round(self.angles.joint_1, 2),
+            round(self.angles.joint_2, 2),
+            round(self.angles.joint_3, 2),
+            round(self.angles.joint_4, 2),
+            round(self.angles.joint_5, 2),
+            round(self.angles.joint_6, 2),
+            round(self.angles.joint_7, 2),
+        ]
+        # print('coord:',self.record_coords)
+        # print('angles:',self.res_angles)
+
+    # def send_input(self,dates):
+    def show_j_date(self, date, way=""):
+        # Show data,展示数据
+        if way == "coord":
+            for i, j in zip(date, self.coord_all):
+                # print(i)
+                j.set(str(i))
+        else:
+            for i, j in zip(date, self.cont_all):
+                j.set(str(i) + "°")
+
+    def run(self):
+        while True:
+            try:
+                self.win.update()
+                time.sleep(0.001)
+            except tk.TclError as e:
+                if "application has been destroyed" in str(e):
+                    break
+                else:
+                    raise
+
+
+def main():
+    window = tk.Tk()
+    window.title("Mercury ros GUI")
+    Window(window).run()
+
+
+if __name__ == "__main__":
+    main()

Mercury/mercury_a1/scripts/slider_control.py

@@ -12,7 +12,7 @@ import math
 import rospy
 from sensor_msgs.msg import JointState
 
-from pymycobot.cobotx import CobotX
+from pymycobot.mercury import Mercury
 
 
 mc = None
@@ -20,7 +20,9 @@ mc = None
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    print(data.position)
+    # print(data.position)
+    rounded_data_tuple = tuple(round(value, 2) for value in data.position)
+    rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple)
     data_list = []
     for index, value in enumerate(data.position):
         radians_to_angles = math.degrees(value)
@@ -39,7 +41,7 @@ def listener():
     port = rospy.get_param("~port", "/dev/ttyAMA1")
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
-    mc = CobotX(port, baud)
+    mc = Mercury(port, baud)
 
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止

Mercury/mercury_a1/scripts/teleop_keyboard.py

@@ -0,0 +1,177 @@
+#!/usr/bin/env python3
+from __future__ import print_function
+from mercury_a1_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
+import rospy
+import sys
+import termios
+import tty
+import time
+
+
+# Terminal output prompt information. 终端输出提示信息
+msg = """\
+Mercury Teleop Keyboard Controller
+---------------------------
+Movimg options(control coordinations [x,y,z,rx,ry,rz]):
+              w(x+)
+
+    a(y-)     s(x-)     d(y+)
+
+    z(z-) x(z+)
+
+u(rx+)   i(ry+)   o(rz+)
+j(rx-)   k(ry-)   l(rz-)
+
+Gripper control:
+    g - open
+    h - close
+
+Other:
+    1 - Go to init pose
+    2 - Go to home pose
+    3 - Resave home pose
+    q - Quit
+"""
+
+
+def vels(speed, turn):
+    return "currently:\tspeed: %s\tchange percent: %s  " % (speed, turn)
+
+
+class Raw(object):
+    def __init__(self, stream):
+        self.stream = stream
+        self.fd = self.stream.fileno()
+
+    def __enter__(self):
+        self.original_stty = termios.tcgetattr(self.stream)
+        tty.setcbreak(self.stream)
+
+    def __exit__(self, type, value, traceback):
+        termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
+
+
+def teleop_keyboard():
+    rospy.init_node("teleop_keyboard")
+
+    model = 1
+    speed = rospy.get_param("~speed", 50)
+    change_percent = rospy.get_param("~change_percent", 5)
+
+    change_angle = 180 * change_percent / 100
+    change_len = 250 * change_percent / 100
+
+    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_gripper_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)
+    except:
+        print("start error ...")
+        exit(1)
+
+    init_pose = [0, 0, 0, 0, 0, 0, 0, speed]
+    home_pose = [0, 0, 0, -90, 0, 90, 0, speed]
+
+    rsp = set_angles(*home_pose)
+
+    while True:
+        res = get_coords()
+        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]
+    print('init_coords:', record_coords)
+
+    try:
+        print(msg)
+        print(vels(speed, change_percent))
+        # Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
+        while 1:
+            try:
+                # print("\r current coords: %s" % record_coords, end="")
+                with Raw(sys.stdin):
+                    key = sys.stdin.read(1)
+                if key == "q":
+                    break
+                elif key in ["w", "W"]:
+                    record_coords[0] += change_len
+                    set_coords(*record_coords)
+                elif key in ["s", "S"]:
+                    record_coords[0] -= change_len
+                    set_coords(*record_coords)
+                elif key in ["a", "A"]:
+                    record_coords[1] -= change_len
+                    set_coords(*record_coords)
+                elif key in ["d", "D"]:
+                    record_coords[1] += change_len
+                    set_coords(*record_coords)
+                elif key in ["z", "Z"]:
+                    record_coords[2] -= change_len
+                    set_coords(*record_coords)
+                elif key in ["x", "X"]:
+                    record_coords[2] += change_len
+                    set_coords(*record_coords)
+                elif key in ["u", "U"]:
+                    record_coords[3] += change_angle
+                    set_coords(*record_coords)
+                elif key in ["j", "J"]:
+                    record_coords[3] -= change_angle
+                    set_coords(*record_coords)
+                elif key in ["i", "I"]:
+                    record_coords[4] += change_angle
+                    set_coords(*record_coords)
+                elif key in ["k", "K"]:
+                    record_coords[4] -= change_angle
+                    set_coords(*record_coords)
+                elif key in ["o", "O"]:
+                    record_coords[5] += change_angle
+                    set_coords(*record_coords)
+                elif key in ["l", "L"]:
+                    record_coords[5] -= change_angle
+                    set_coords(*record_coords)
+                elif key in ["g", "G"]:
+                    switch_gripper(True)
+                elif key in ["h", "H"]:
+                    switch_gripper(False)
+                elif key == "1":
+                    rsp = set_angles(*init_pose)
+                elif key in "2":
+                    rsp = set_angles(*home_pose)
+                    time.sleep(3)
+                    res = get_coords()
+                    time.sleep(0.1)
+                    record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
+                    print('home_coords:', record_coords)
+                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
+
+            except Exception as e:
+                # print(e)
+                continue
+
+    except Exception as e:
+        print(e)
+
+
+if __name__ == "__main__":
+    try:
+        teleop_keyboard()
+    except rospy.ROSInterruptException:
+        pass

Mercury/mercury_a1_communication/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(cobotx_a450_communication)
+project(mercury_a1_communication)
 add_compile_options(-std=c++11)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
@@ -50,12 +50,12 @@ find_package(catkin REQUIRED COMPONENTS
 
 ## Generate messages in the 'msg' folder
 add_message_files(FILES
-  CobotXAngles.msg
+  MercuryAngles.msg
-  CobotXCoords.msg
+  MercuryCoords.msg
-  CobotXSetAngles.msg
+  MercurySetAngles.msg
-  CobotXSetCoords.msg
+  MercurySetCoords.msg
-  CobotXGripperStatus.msg
+  MercuryGripperStatus.msg
-  CobotXPumpStatus.msg
+  MercuryPumpStatus.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -169,8 +169,8 @@ include_directories(
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
-  scripts/cobotx_services.py
+  scripts/mercury_services.py
-  scripts/cobotx_topics.py
+  scripts/mercury_topics.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

Mercury/mercury_a1_communication/launch/communication_service.launch

@@ -6,7 +6,7 @@
 
 
 	<!-- Open communication service ，开启通讯服务-->
-	<node name="cobotx_services" pkg="cobotx_a450_communication" type="cobotx_services.py" output="screen">
+	<node name="mercury_services" pkg="mercury_a1_communication" type="mercury_services.py" output="screen">
 		<param name="port" type="string" value="$(arg port)" />
 		<param name="baud" type="int" value="$(arg baud)" />
     </node>

Mercury/mercury_a1_communication/launch/communication_topic.launch

@@ -4,7 +4,7 @@
 	<arg name="baud" default="115200" />
 
 	<!-- Open communication service，开启通讯服务 -->
-	<node name="cobotx_services" pkg="cobotx_a450_communication" type="cobotx_topics.py" output="screen">
+	<node name="mercury_services" pkg="mercury_a1_communication" type="mercury_topics.py" output="screen">
 		<param name="port" type="string" value="$(arg port)" />
 		<param name="baud" type="int" value="$(arg baud)" />
     </node>

Mercury/mercury_a1_communication/launch/communication_topic_pi.launch

@@ -4,7 +4,7 @@
 	<arg name="baud" default="115200" />
 
 	<!-- Open communication service --><!-- 开启通讯服务 -->
-	<node name="cobotx_services" pkg="cobotx_a450_communication" type="cobotx_topics_pi.py" output="screen">
+	<node name="mercury_services" pkg="mercury_a1_communication" type="mercury_topics_pi.py" output="screen">
 		<param name="port" type="string" value="$(arg port)" />
 		<param name="baud" type="int" value="$(arg baud)" />
     	</node>

Mercury/mercury_a1_communication/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>cobotx_a450_communication</name>
+  <name>mercury_a1_communication</name>
   <version>0.1.0</version>
-  <description>The cobotx_a450_communication package</description>
+  <description>The mercury_a1_communication package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->

Mercury/mercury_a1_communication/scripts/mercury_services.py

@@ -4,9 +4,9 @@ import time
 import rospy
 import os
 import fcntl
-from cobotx_a450_communication.srv import *
+from mercury_a1_communication.srv import *
 
-from pymycobot.cobotx import CobotX
+from pymycobot.mercury import Mercury
 
 mc = None
 
@@ -51,12 +51,12 @@ def release(lock_file_fd):
 
 def create_handle():
     global mc
-    rospy.init_node("cobotx")
+    rospy.init_node("mercury")
     rospy.loginfo("start ...")
     port = rospy.get_param("~port")
     baud = rospy.get_param("~baud")
     rospy.loginfo("%s,%s" % (port, baud))
-    mc = CobotX(port, baud)
+    mc = Mercury(port, baud)
 
 
 def create_services():
@@ -84,7 +84,7 @@ def set_angles(req):
     sp = req.speed
 
     if mc:
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         mc.send_angles(angles, sp)
         release(lock)
 
@@ -94,7 +94,7 @@ def set_angles(req):
 def get_angles(req):
     """get angles,获取角度"""
     if mc:
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         angles = mc.get_angles()
         release(lock)
         return GetAnglesResponse(*angles)
@@ -113,7 +113,7 @@ def set_coords(req):
     mod = req.model
 
     if mc:
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         mc.send_coords(coords, sp, mod)
         release(lock)
 
@@ -122,7 +122,7 @@ def set_coords(req):
 
 def get_coords(req):
     if mc:
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         coords = mc.get_coords()
         release(lock)
         return GetCoordsResponse(*coords)
@@ -132,7 +132,7 @@ def switch_status(req):
     """Gripper switch status"""
     """夹爪开关状态"""
     if mc:
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         if req.Status:
             mc.set_gripper_state(0, 80)
         else:
@@ -144,7 +144,7 @@ def switch_status(req):
 
 def toggle_pump(req):
     if mc:
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         if req.Status:
             mc.set_basic_output(req.Pin1, 0)
             mc.set_basic_output(req.Pin2, 0)
@@ -158,7 +158,7 @@ def toggle_pump(req):
 
 
 robot_msg = """
-Cobotx Status
+Mercury Status
 --------------------------------
 Joint Limit:
     joint 1: -165 ~ +165
@@ -186,13 +186,13 @@ def output_robot_message():
     atom_version = "unknown"
 
     if mc:
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         cn = mc.is_controller_connected()
         release(lock)
         if cn == 1:
             connect_status = True
         time.sleep(0.1)
-        lock = acquire("/tmp/cobotx_lock")
+        lock = acquire("/tmp/mercury_lock")
         si = mc.is_all_servo_enable()
         release(lock)
         if si == 1:

Mercury/mercury_a1_communication/scripts/mercury_topics.py

@@ -8,16 +8,16 @@ import threading
 
 import rospy
 
-from cobotx_a450_communication.msg import (
+from mercury_a1_communication.msg import (
-    CobotXAngles,
+    MercuryAngles,
-    CobotXCoords,
+    MercuryCoords,
-    CobotXSetAngles,
+    MercurySetAngles,
-    CobotXSetCoords,
+    MercurySetCoords,
-    CobotXGripperStatus,
+    MercuryGripperStatus,
-    CobotXPumpStatus,
+    MercuryPumpStatus,
 )
 
-from pymycobot.cobotx import CobotX
+from pymycobot.mercury import Mercury
 
 
 class Watcher:
@@ -64,11 +64,11 @@ class Watcher:
             pass
 
 
-class CobotXTopics(object):
+class MercuryTopics(object):
     def __init__(self):
-        super(CobotXTopics, self).__init__()
+        super(MercuryTopics, self).__init__()
 
-        rospy.init_node("cobotx_topics")
+        rospy.init_node("mercury_topics")
         rospy.loginfo("start ...")
         # port = rospy.get_param("~port", os.popen("ls /dev/ttyAMA*").readline()[:-1])
         port = rospy.get_param("~port", "/dev/ttyAMA1")
@@ -76,7 +76,7 @@ class CobotXTopics(object):
             port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
         baud = rospy.get_param("~baud", 115200)
         rospy.loginfo("%s,%s" % (port, baud))
-        self.mc = CobotX(port, baud)
+        self.mc = Mercury(port, baud)
         self.lock = threading.Lock()
 
     def start(self):
@@ -110,9 +110,9 @@ class CobotXTopics(object):
     def pub_real_angles(self):
         """Publish real angle"""
         """发布真实角度"""
-        pub = rospy.Publisher("cobotx/angles_real",
+        pub = rospy.Publisher("mercury/angles_real",
-                              CobotXAngles, queue_size=5)
+                              MercuryAngles, queue_size=5)
-        ma = CobotXAngles()
+        ma = MercuryAngles()
         while not rospy.is_shutdown():
             self.lock.acquire()
             angles = self.mc.get_angles()
@@ -131,9 +131,9 @@ class CobotXTopics(object):
     def pub_real_coords(self):
         """publish real coordinates"""
         """发布真实坐标"""
-        pub = rospy.Publisher("cobotx/coords_real",
+        pub = rospy.Publisher("mercury/coords_real",
-                              CobotXCoords, queue_size=5)
+                              MercuryCoords, queue_size=5)
-        ma = CobotXCoords()
+        ma = MercuryCoords()
 
         while not rospy.is_shutdown():
             self.lock.acquire()
@@ -166,7 +166,7 @@ class CobotXTopics(object):
             self.mc.send_angles(angles, sp)
 
         sub = rospy.Subscriber(
-            "cobotx/angles_goal", CobotXSetAngles, callback=callback
+            "mercury/angles_goal", MercurySetAngles, callback=callback
         )
         rospy.spin()
 
@@ -178,7 +178,7 @@ class CobotXTopics(object):
             self.mc.send_coords(angles, sp, model)
 
         sub = rospy.Subscriber(
-            "cobotx/coords_goal", CobotXSetCoords, callback=callback
+            "mercury/coords_goal", MercurySetCoords, callback=callback
         )
         rospy.spin()
 
@@ -192,7 +192,7 @@ class CobotXTopics(object):
                 self.mc.set_gripper_state(1, 80)
 
         sub = rospy.Subscriber(
-            "cobotx/gripper_status", CobotXGripperStatus, callback=callback
+            "mercury/gripper_status", MercuryGripperStatus, callback=callback
         )
         rospy.spin()
 
@@ -206,14 +206,14 @@ class CobotXTopics(object):
                 self.mc.set_basic_output(data.Pin2, 1)
 
         sub = rospy.Subscriber(
-            "cobotx/pump_status", CobotXPumpStatus, callback=callback
+            "mercury/pump_status", MercuryPumpStatus, callback=callback
         )
         rospy.spin()
 
 
 if __name__ == "__main__":
     Watcher()
-    mc_topics = CobotXTopics()
+    mc_topics = MercuryTopics()
     mc_topics.start()
     # while True:
     #     mc_topics.pub_real_coords()

Mercury/mercury_a1_communication/scripts/mercury_topics_pi.py

@@ -8,17 +8,17 @@ import threading
 
 import rospy
 
-from cobotx_a450_communication.msg import (
+from mercury_a1_communication.msg import (
-    CobotXAngles,
+    MercuryAngles,
-    CobotXCoords,
+    MercuryCoords,
-    CobotXSetAngles,
+    MercurySetAngles,
-    CobotXSetCoords,
+    MercurySetCoords,
-    CobotXGripperStatus,
+    MercuryGripperStatus,
-    CobotXPumpStatus,
+    MercuryPumpStatus,
 )
 
 
-from pymycobot import CobotX
+from pymycobot.mercury import Mercury
 
 
 class Watcher:
@@ -66,18 +66,18 @@ class Watcher:
             pass
 
 
-class CobotXTopics(object):
+class MercuryTopics(object):
     def __init__(self):
-        super(CobotXTopics, self).__init__()
+        super(MercuryTopics, self).__init__()
 
-        rospy.init_node("cobotx_topics_pi")
+        rospy.init_node("mercury_topics_pi")
         rospy.loginfo("start ...")
         # problem
         # port = rospy.get_param("~port", os.popen("ls /dev/ttyAMA*").readline()[:-1])
         port = rospy.get_param("~port", "/dev/ttyAMA1")
         baud = rospy.get_param("~baud", 115200)
         rospy.loginfo("%s,%s" % (port, baud))
-        self.mc = CobotX(port, baud)
+        self.mc = Mercury(port, baud)
         self.lock = threading.Lock()
 
     def start(self):
@@ -111,8 +111,8 @@ class CobotXTopics(object):
     def pub_real_angles(self):
         """Publish real angle"""
         """发布真实角度"""
-        pub = rospy.Publisher("cobotx/angles_real", CobotXAngles, queue_size=5)
+        pub = rospy.Publisher("mercury/angles_real", MercuryAngles, queue_size=5)
-        ma = CobotXAngles()
+        ma = MercuryAngles()
         while not rospy.is_shutdown():
             self.lock.acquire()
             angles = self.mc.get_angles()
@@ -131,8 +131,8 @@ class CobotXTopics(object):
     def pub_real_coords(self):
         """publish real coordinates"""
         """发布真实坐标"""
-        pub = rospy.Publisher("cobotx/coords_real", CobotXCoords, queue_size=5)
+        pub = rospy.Publisher("mercury/coords_real", MercuryCoords, queue_size=5)
-        ma = CobotXCoords()
+        ma = MercuryCoords()
 
         while not rospy.is_shutdown():
             self.lock.acquire()
@@ -165,7 +165,7 @@ class CobotXTopics(object):
             self.mc.send_angles(angles, sp)
 
         sub = rospy.Subscriber(
-            "cobotx/angles_goal", CobotXSetAngles, callback=callback
+            "mercury/angles_goal", MercurySetAngles, callback=callback
         )
         rospy.spin()
 
@@ -177,7 +177,7 @@ class CobotXTopics(object):
             self.mc.send_coords(angles, sp, model)
 
         sub = rospy.Subscriber(
-            "cobotx/coords_goal", CobotXSetCoords, callback=callback
+            "mercury/coords_goal", MercurySetCoords, callback=callback
         )
         rospy.spin()
 
@@ -191,7 +191,7 @@ class CobotXTopics(object):
                 self.mc.set_gripper_state(1, 80)
 
         sub = rospy.Subscriber(
-            "cobotx/gripper_status", CobotXGripperStatus, callback=callback
+            "mercury/gripper_status", MercuryGripperStatus, callback=callback
         )
         rospy.spin()
 
@@ -205,13 +205,13 @@ class CobotXTopics(object):
                 self.mc.set_basic_output(data.Pin2, 1)
 
         sub = rospy.Subscriber(
-            "cobotx/pump_status", CobotXPumpStatus, callback=callback
+            "mercury/pump_status", MercuryPumpStatus, callback=callback
         )
         rospy.spin()
 
 if __name__ == "__main__":
     Watcher()
-    mc_topics = CobotXTopics()
+    mc_topics = MercuryTopics()
     mc_topics.start()
     # while True:
     #     mc_topics.pub_real_coords()

Mercury/mercury_a1_moveit/.setup_assistant

@@ -1,7 +1,7 @@
 moveit_setup_assistant_config:
   URDF:
     package: mycobot_description
-    relative_path: urdf/cobotx_a450/cobotx_a450.urdf
+    relative_path: urdf/mercury_a1/mercury_a1.urdf
     xacro_args: "--inorder "
   SRDF:
     relative_path: config/firefighter.srdf

Mercury/mercury_a1_moveit/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.1.3)
-project(cobotx_a450_moveit)
+project(mercury_a1_moveit)
 
 find_package(catkin REQUIRED)
 

Mercury/mercury_a1_moveit/launch/chomp_planning_pipeline.launch.xml

@@ -17,5 +17,5 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-  <rosparam command="load" file="$(find cobotx_a450_moveit)/config/chomp_planning.yaml" />
+  <rosparam command="load" file="$(find mercury_a1_moveit)/config/chomp_planning.yaml" />
 </launch>

Mercury/mercury_a1_moveit/launch/default_warehouse_db.launch

@@ -2,7 +2,7 @@
 
   <arg name="reset" default="false"/>
   <!-- If not specified, we'll use a default database location -->
-  <arg name="moveit_warehouse_database_path" default="$(find cobotx_a450_moveit)/default_warehouse_mongo_db" />
+  <arg name="moveit_warehouse_database_path" default="$(find mercury_a1_moveit)/default_warehouse_mongo_db" />
 
   <!-- Launch the warehouse with the configured database location -->
   <include file="$(dirname)/warehouse.launch">

Mercury/mercury_a1_moveit/launch/demo.launch

@@ -6,7 +6,7 @@
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find cobotx_a450_moveit)/default_warehouse_mongo_db" />
+  <arg name="db_path" default="$(find mercury_a1_moveit)/default_warehouse_mongo_db" />
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />

Mercury/mercury_a1_moveit/launch/fake_moveit_controller_manager.launch.xml

@@ -7,6 +7,6 @@
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam subst_value="true" file="$(find cobotx_a450_moveit)/config/fake_controllers.yaml"/>
+  <rosparam subst_value="true" file="$(find mercury_a1_moveit)/config/fake_controllers.yaml"/>
 
 </launch>

Mercury/mercury_a1_moveit/launch/firefighter_moveit_controller_manager.launch.xml

@@ -6,5 +6,5 @@
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
   <!-- loads ros_controllers to the param server -->
-  <rosparam file="$(find cobotx_a450_moveit)/config/ros_controllers.yaml"/>
+  <rosparam file="$(find mercury_a1_moveit)/config/ros_controllers.yaml"/>
 </launch>

Mercury/mercury_a1_moveit/launch/gazebo.launch

@@ -13,7 +13,7 @@
   </include>
 
   <!-- Set the robot urdf on the parameter server -->
-  <param name="robot_description" textfile="$(find mycobot_description)/urdf/myarm/myarm_urdf.urdf" />
+  <param name="robot_description" textfile="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf" />
 
   <!-- Unpause the simulation after loading the robot model -->
   <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
@@ -23,7 +23,7 @@
     respawn="false" output="screen" />
 
   <!-- Load the controller parameters onto the parameter server -->
-  <rosparam file="$(find cobotx_a450_moveit)/config/gazebo_controllers.yaml" />
+  <rosparam file="$(find mercury_a1_moveit)/config/gazebo_controllers.yaml" />
   <include file="$(dirname)/ros_controllers.launch"/>
 
   <!-- Spawn the Gazebo ROS controllers -->

Mercury/mercury_a1_moveit/launch/mercury_a1.launch

@@ -0,0 +1,67 @@
+<launch>
+
+  <!-- specify the planning pipeline -->
+  <arg name="pipeline" default="ompl" />
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find mercury_a1_moveit)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default, we will load or override the robot_description -->
+  <arg name="load_robot_description" default="true"/>
+
+  <!-- Choose controller manager: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" default="fake" />
+  <!-- Set execution mode for fake execution controllers -->
+  <arg name="fake_execution_type" default="interpolate" />
+
+  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
+  <arg name="use_gui" default="false" />
+  <arg name="use_rviz" default="true" />
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base" />
+
+  <group if="$(eval arg('moveit_controller_manager') == 'fake')">
+    <!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
+         MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    </node>
+    <!-- If desired, a GUI version is available allowing to move the simulated robot around manually
+         This corresponds to moving around the real robot without the use of MoveIt. -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
+      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    </node>
+
+    <!-- Given the published joint states, publish tf for the robot links -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+  </group>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(dirname)/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  </include>
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
+    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+</launch>

Mercury/mercury_a1_moveit/launch/mycobot_moveit.launch

@@ -5,7 +5,7 @@
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
    <!-- 允许用户指定数据库位置 -->
-  <arg name="db_path" default="$(find cobotx_a450_moveit)/default_warehouse_mongo_db" />
+  <arg name="db_path" default="$(find mercury_a1_moveit)/default_warehouse_mongo_db" />
 
   <!-- By default, we are not in debug mode -->  <!-- 默认情况下，我们不处于调试模式 -->
   <arg name="debug" default="false" />
@@ -22,7 +22,7 @@
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
    <!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
-  <include file="$(find cobotx_a450_moveit)/launch/planning_context.launch">
+  <include file="$(find mercury_a1_moveit)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
   </include>
 
@@ -42,7 +42,7 @@
 
   <!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
     <!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
-  <include file="$(find cobotx_a450_moveit)/launch/move_group.launch">
+  <include file="$(find mercury_a1_moveit)/launch/move_group.launch">
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="fake_execution" value="true"/>
     <arg name="info" value="true"/>
@@ -51,14 +51,14 @@
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
    <!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
-  <include file="$(find cobotx_a450_moveit)/launch/moveit_rviz.launch">
+  <include file="$(find mercury_a1_moveit)/launch/moveit_rviz.launch">
     <arg name="config" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
 
   <!-- If database loading was enabled, start mongodb as well -->
   <!-- 如果启用了数据库加载，也启动 mongodb -->
-  <include file="$(find cobotx_a450_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
+  <include file="$(find mercury_a1_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
     <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
   </include>
 

Mercury/mercury_a1_moveit/launch/ompl-chomp_planning_pipeline.launch.xml

@@ -1,6 +1,6 @@
 <launch>
   <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
-  <include file="$(find cobotx_a450_moveit)/launch/ompl_planning_pipeline.launch.xml">
+  <include file="$(find mercury_a1_moveit)/launch/ompl_planning_pipeline.launch.xml">
     <arg name="planning_adapters"
          default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
                   default_planner_request_adapters/AddTimeParameterization
@@ -13,7 +13,7 @@
   </include>
 
   <!-- load chomp config -->
-  <rosparam command="load" file="$(find cobotx_a450_moveit)/config/chomp_planning.yaml" />
+  <rosparam command="load" file="$(find mercury_a1_moveit)/config/chomp_planning.yaml" />
 
   <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
   <param name="trajectory_initialization_method" value="fillTrajectory"/>

Mercury/mercury_a1_moveit/launch/ompl_planning_pipeline.launch.xml

@@ -19,6 +19,6 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-  <rosparam command="load" file="$(find cobotx_a450_moveit)/config/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find mercury_a1_moveit)/config/ompl_planning.yaml"/>
 
 </launch>

Mercury/mercury_a1_moveit/launch/planning_context.launch

@@ -6,20 +6,20 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/cobotx_a450/cobotx_a450.urdf"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find cobotx_a450_moveit)/config/firefighter.srdf" />
+  <param name="$(arg robot_description)_semantic" textfile="$(find mercury_a1_moveit)/config/firefighter.srdf" />
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find cobotx_a450_moveit)/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find mercury_a1_moveit)/config/joint_limits.yaml"/>
-    <rosparam command="load" file="$(find cobotx_a450_moveit)/config/cartesian_limits.yaml"/>
+    <rosparam command="load" file="$(find mercury_a1_moveit)/config/cartesian_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <group ns="$(arg robot_description)_kinematics">
-    <rosparam command="load" file="$(find cobotx_a450_moveit)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find mercury_a1_moveit)/config/kinematics.yaml"/>
 
   </group>
 

Mercury/mercury_a1_moveit/launch/ros_controllers.launch

@@ -2,7 +2,7 @@
 <launch>
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find cobotx_a450_moveit)/config/ros_controllers.yaml" command="load"/>
+  <rosparam file="$(find mercury_a1_moveit)/config/ros_controllers.yaml" command="load"/>
 
   <!-- Load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

Mercury/mercury_a1_moveit/launch/run_benchmark_ompl.launch

@@ -15,7 +15,7 @@
 
   <!-- Start Benchmark Executable -->
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find cobotx_a450_moveit)/config/ompl_planning.yaml"/>
+    <rosparam command="load" file="$(find mercury_a1_moveit)/config/ompl_planning.yaml"/>
   </node>
 
 </launch>

Mercury/mercury_a1_moveit/launch/sensor_manager.launch.xml

@@ -3,7 +3,7 @@
   <!-- This file makes it easy to include the settings for sensor managers -->
 
   <!-- Params for 3D sensors config -->
-  <rosparam command="load" file="$(find cobotx_a450_moveit)/config/sensors_3d.yaml" />
+  <rosparam command="load" file="$(find mercury_a1_moveit)/config/sensors_3d.yaml" />
 
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->

Mercury/mercury_a1_moveit/launch/setup_assistant.launch

@@ -8,7 +8,7 @@
 
   <!-- Run -->
   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
-        args="--config_pkg=cobotx_a450_moveit"
+        args="--config_pkg=mercury_a1_moveit"
         launch-prefix="$(arg launch_prefix)"
         required="true"
         output="screen" />

Mercury/mercury_a1_moveit/launch/simple_moveit_controller_manager.launch.xml

@@ -3,6 +3,6 @@
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 
   <!-- Load controller list to the parameter server -->
-  <rosparam file="$(find cobotx_a450_moveit)/config/simple_moveit_controllers.yaml" />
+  <rosparam file="$(find mercury_a1_moveit)/config/simple_moveit_controllers.yaml" />
-  <rosparam file="$(find cobotx_a450_moveit)/config/ros_controllers.yaml" />
+  <rosparam file="$(find mercury_a1_moveit)/config/ros_controllers.yaml" />
 </launch>

Mercury/mercury_a1_moveit/launch/stomp_planning_pipeline.launch.xml

@@ -19,5 +19,5 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-  <rosparam command="load" file="$(find cobotx_a450_moveit)/config/stomp_planning.yaml"/>
+  <rosparam command="load" file="$(find mercury_a1_moveit)/config/stomp_planning.yaml"/>
 </launch>

Mercury/mercury_a1_moveit/package.xml

@@ -1,6 +1,6 @@
 <package>
 
-  <name>cobotx_a450_moveit</name>
+  <name>mercury_a1_moveit</name>
   <version>0.3.0</version>
   <description>
      An automatically generated package with all the configuration and launch files for using the firefighter with the MoveIt Motion Planning Framework

Mercury/mercury_a1_moveit/scripts/sync_plan.py

@@ -13,7 +13,7 @@ import math
 import rospy
 from sensor_msgs.msg import JointState
 
-from pymycobot.cobotx import CobotX
+from pymycobot.mercury import Mercury
 
 
 mc = None
@@ -21,10 +21,12 @@ mc = None
 
 def callback(data):
     # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    print(data.position)
+    # print(data.position)
+    rounded_data_tuple = tuple(round(value, 2) for value in data.position)
+    rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple)
     data_list = []
     for index, value in enumerate(data.position):
-        radians_to_angles = math.degrees(value)
+        radians_to_angles = round(math.degrees(value), 2)
         data_list.append(radians_to_angles)
 
     # mc.send_radians(data_list, 80)
@@ -40,7 +42,7 @@ def listener():
     port = rospy.get_param("~port", "/dev/ttyAMA1")
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
-    mc = CobotX(port, baud)
+    mc = Mercury(port, baud)
   
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止

Mercury/mercury_b1/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mercury_b1)
+add_compile_options(-std=c++11)
+
+## Find catkin and any catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  actionlib
+  image_transport
+  cv_bridge
+)
+
+## Declare a catkin package
+catkin_package(
+  CATKIN_DEPENDS std_msgs actionlib
+)
+
+## Build talker and listener
+include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+
+catkin_install_python(PROGRAMS
+  scripts/follow_display.py
+  scripts/slider_control.py
+  scripts/teleop_keyboard.py
+  scripts/listen_real.py
+  scripts/listen_real_of_topic.py
+  # scripts/detect_marker.py
+  # scripts/following_marker.py
+  # scripts/follow_and_pump.py
+  scripts/simple_gui.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+# find_package(OpenCV REQUIRED)
+# add_executable(opencv_camera src/opencv_camera)
+# target_link_libraries(opencv_camera ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+# add_executable(camera_display src/camera_display)
+# target_link_libraries(camera_display ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

Mercury/mercury_b1/LICENSE

@@ -0,0 +1,25 @@
+BSD 2-Clause License
+
+Copyright (c) 2020, Elephant Robotics
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Mercury/mercury_b1/config/mercury_b1.rviz

@@ -0,0 +1,313 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /RobotModel1/Links1/link1_L1
+        - /RobotModel1/Links1/link1_L1/Position1
+        - /Axes1
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 584
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link1_L:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link1_R:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2_L:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2_R:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3_L:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3_R:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4_L:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4_R:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5_L:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5_R:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link6_L:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link6_R:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link7_L:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link7_R:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_body:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_eye:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 0.20000000298023224
+      Name: Axes
+      Radius: 0.009999999776482582
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base:
+          Value: true
+        link1_L:
+          Value: true
+        link1_R:
+          Value: true
+        link2_L:
+          Value: true
+        link2_R:
+          Value: true
+        link3_L:
+          Value: true
+        link3_R:
+          Value: true
+        link4_L:
+          Value: true
+        link4_R:
+          Value: true
+        link5_L:
+          Value: true
+        link5_R:
+          Value: true
+        link6_L:
+          Value: true
+        link6_R:
+          Value: true
+        link7_L:
+          Value: true
+        link7_R:
+          Value: true
+        link_body:
+          Value: true
+        link_eye:
+          Value: true
+        link_head:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.10000000149011612
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base:
+          link_body:
+            link1_L:
+              link2_L:
+                link3_L:
+                  link4_L:
+                    link5_L:
+                      link6_L:
+                        link7_L:
+                          {}
+            link1_R:
+              link2_R:
+                link3_R:
+                  link4_R:
+                    link5_R:
+                      link6_R:
+                        link7_R:
+                          {}
+            link_head:
+              link_eye:
+                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.6387460231781006
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4097958207130432
+      Target Frame: <Fixed Frame>
+      Yaw: 6.260410308837891
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 906
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002d3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002d3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002d3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000057fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000002d300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

Mercury/mercury_b1/launch/mercury_b1_follow.launch

@@ -0,0 +1,13 @@
+<launch>
+	<!-- Load URDF file,加载URDF文件 -->
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_b1/mercury_b1.urdf"/>
+	<arg name="rvizconfig" default="$(find mercury_b1)/config/mercury_b1.rviz" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+	<!-- Combinejoin values to TF ，将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
+		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+	</node>
+	<!-- show in Rviza,显示在Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

Mercury/mercury_b1/launch/simple_gui.launch

@@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyAMA1" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
+	<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
+	<arg name="gui" default="false" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF ,将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+    <include file="$(find mercury_a1_communication)/launch/communication_service.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+	<node name="real_listener" pkg="mercury_a1" type="listen_real.py" />
+	<node name="simple_gui" pkg="mercury_a1" type="simple_gui.py" />
+</launch>

Mercury/mercury_b1/launch/slider_control.launch

@@ -0,0 +1,24 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mercury_b1/mercury_b1.urdf"/>
+	<arg name="rvizconfig" default="$(find mercury_b1)/config/mercury_b1.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>