Merge pull request #130 from elephantrobotics/myarm-c650

Myarm c650
2026-07-05 19:47:04 +00:00 · 2024-05-28 09:43:28 +08:00 · 2024-05-28 09:43:28 +08:00 · 321fff56d2
commit 321fff56d2
parent fb3d215662 e6be9c3262
85 changed files with 40766 additions and 77 deletions
--- a/debug_1.py
+++ b/debug_1.py
@ -0,0 +1,45 @@
+#!/usr/bin/env python
+
+import rospy
+from sensor_msgs.msg import JointState
+import time, random, subprocess,rospy
+from pymycobot.mycobot import MyCobot
+from pymycobot.genre import Angle, Coord
+from pymycobot.myarmc import MyArmC
+
+def get_joint_states():
+    # 这个函数应该返回机械臂的实时关节状态
+    # 这里只是一个示例，你需要根据你的实际情况来实现这个函数
+    return [random.uniform(-180, 180) for _ in range(7)]
+
+def talker():
+    # 初始化节点
+    rospy.init_node('joint_state_publisher')
+
+    # 创建发布器，发布到'joint_states'话题，消息类型为JointState
+    pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
+
+    # 设置发布频率
+    rate = rospy.Rate(10) 
+
+    while not rospy.is_shutdown():
+        # 创建JointState消息对象
+        msg = JointState()
+
+        # 获取机械臂的实时关节状态
+        joint_states = get_joint_states()
+
+        # 将关节状态赋值给消息对象
+        msg.position = joint_states
+
+        # 发布消息
+        pub.publish(msg)
+
+        # 按照设定的频率延时
+        rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
--- a/myArm/myarm_c650/CMakeLists.txt
+++ b/myArm/myarm_c650/CMakeLists.txt
@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(myarm_c650)
+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})
--- a/myArm/myarm_c650/LICENSE
+++ b/myArm/myarm_c650/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.
--- a/myArm/myarm_c650/config/myarm_c650.rviz
+++ b/myArm/myarm_c650/config/myarm_c650.rviz
@ -0,0 +1,237 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Axes1
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 523
+  - 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
+        gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5:
+          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.10000000149011612
+      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: false
+        gripper:
+          Value: false
+        gripper_left:
+          Value: false
+        gripper_right:
+          Value: false
+        link1:
+          Value: false
+        link2:
+          Value: false
+        link3:
+          Value: false
+        link4:
+          Value: false
+        link5:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base:
+          link1:
+            link2:
+              link3:
+                link4:
+                  link5:
+                    gripper:
+                      gripper_left:
+                        {}
+                      gripper_right:
+                        {}
+      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.1106241941452026
+      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.31777650117874146
+        Y: 0.11575467139482498
+        Z: 0.21179701387882233
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6647977232933044
+      Target Frame: <Fixed Frame>
+      Yaw: 4.894510269165039
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 820
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000296fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000296000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000296fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000296000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007320000003efc0100000002fb0000000800540069006d0065010000000000000732000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c10000029600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1842
+  X: 72
+  Y: 27
--- a/myArm/myarm_c650/config/myarm_with_marker.rviz
+++ b/myArm/myarm_c650/config/myarm_with_marker.rviz
@ -0,0 +1,216 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /TF1
+        - /Marker1
+        - /Marker1/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+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.0299999993
+        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
+        joint1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6_flange:
+          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
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        basic_shapes:
+          Value: true
+        joint1:
+          Value: true
+        joint2:
+          Value: true
+        joint3:
+          Value: true
+        joint4:
+          Value: true
+        joint5:
+          Value: true
+        joint6:
+          Value: true
+        joint6_flange:
+          Value: true
+      Marker Scale: 0.300000012
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        joint1:
+          joint2:
+            joint3:
+              joint4:
+                joint5:
+                  joint6:
+                    joint6_flange:
+                      basic_shapes:
+                        {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /visualization_marker
+      Name: Marker
+      Namespaces:
+        basic_cube: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: joint1
+    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
+      Topic: /initialpose
+    - 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: 2.11990476
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.0706475973
+        Y: -0.0814988762
+        Z: 0.107583851
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.375398338
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.235389769
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24
--- a/myArm/myarm_c650/launch/detect_marker.launch
+++ b/myArm/myarm_c650/launch/detect_marker.launch
@ -0,0 +1,18 @@
+<launch>
+	<!-- Load file model ，加载文件模型-->
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_with_marker.rviz" />
+	<arg name="gui" default="true" />
+
+	<arg name="num" default="0" />
+
+    <include file="$(find myarm_c650)/launch/slider_control.launch">
+		<arg name="model" value="$(arg model)" />
+		<arg name="rvizconfig" value="$(arg rvizconfig)" />
+		<arg name="gui" value="$(arg gui)" />
+    </include>
+	<!-- vision node -->
+	<node name="opencv_camera" pkg="myarm_c650" type="opencv_camera" args="$(arg num)"/>
+	<node name="detect_marker" pkg="myarm_c650" type="detect_marker.py" />
+	<node name="following_marker" pkg="myarm_c650" type="following_marker.py" />
+</launch>
--- a/myArm/myarm_c650/launch/detect_marker_with_topic.launch
+++ b/myArm/myarm_c650/launch/detect_marker_with_topic.launch
@ -0,0 +1,30 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="1000000" />
+	<!-- Load file model ，加载文件模型-->
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_with_marker.rviz" />
+	<arg name="gui" default="false" />
+
+	<arg name="num" default="0" />
+
+	<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 -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+	<!-- mycobot-topics ，mycobot-话题-->
+    <include file="$(find myarm_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="myarm_c650" type="listen_real_of_topic.py" />
+	<!-- vision node -->
+	<node name="opencv_camera" pkg="myarm_c650" type="opencv_camera" args="$(arg num)"/>
+	<node name="detect_marker" pkg="myarm_c650" type="detect_marker.py" />
+	<node name="following_marker" pkg="myarm_c650" type="following_marker.py" />
+</launch>
--- a/myArm/myarm_c650/launch/mycobot_follow.launch
+++ b/myArm/myarm_c650/launch/mycobot_follow.launch
@ -0,0 +1,13 @@
+<launch>
+	<!-- Load URDF file,加载URDF文件 -->
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_c650.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>
--- a/myArm/myarm_c650/launch/simple_gui.launch
+++ b/myArm/myarm_c650/launch/simple_gui.launch
@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="1000000" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_c650.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 myarm_communication)/launch/communication_service.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+	<node name="real_listener" pkg="myarm_c650" type="listen_real.py" />
+	<node name="simple_gui" pkg="myarm_c650" type="simple_gui.py" />
+</launch>
--- a/myArm/myarm_c650/launch/slider_control.launch
+++ b/myArm/myarm_c650/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/myarm_c650/myarm_c650.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_c650.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>
--- a/myArm/myarm_c650/launch/teleop_keyboard.launch
+++ b/myArm/myarm_c650/launch/teleop_keyboard.launch
@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="1000000" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_c650.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 myarm_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="myarm_c650" type="listen_real.py" />
+</launch>
--- a/myArm/myarm_c650/launch/test.launch
+++ b/myArm/myarm_c650/launch/test.launch
@ -0,0 +1,20 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_c650.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to 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> -->
+	<!-- <node name="test" pkg="myarm_c650" type="test.py" /> -->
+
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+	<!-- <node name="rviz" pkg="rviz" type="rviz" /> -->
+</launch>
--- a/myArm/myarm_c650/package.xml
+++ b/myArm/myarm_c650/package.xml
@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>myarm_c650</name>
+  <version>0.3.0</version>
+  <description>The myarm_c650 package</description>
+
+  <author email="zhi.jiang@elephantrobotics.com">jiangzhi</author>
+  <maintainer email="zhi.jiang@elephantrobotics.com">jiangzhi</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/elephantrobotics/mycobot_ros</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>mycobot_description</build_depend>
+  <build_depend>myarm_communication</build_depend>
+
+  <build_export_depend>myarm_communication</build_export_depend>
+  <build_export_depend>mycobot_description</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>python-tk</exec_depend>
+  <exec_depend>mycobot_description</exec_depend>
+  <exec_depend>myarm_communication</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+    
+  </export>
+</package>
--- a/myArm/myarm_c650/scripts/debug.py
+++ b/myArm/myarm_c650/scripts/debug.py
@ -0,0 +1,54 @@
+
+#!/usr/bin/env python
+
+"""
+This package need `pymycobot`.
+This file for test the API if right.
+
+Just can run in Linux.
+"""
+
+
+from math import pi
+import time
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from pymycobot.myarmc import MyArmC
+from pymycobot.myarmm import MyArmM
+import rosnode
+import rospy, sys
+import moveit_commander
+from geometry_msgs.msg import Pose
+
+global mam
+mam = MyArmM('/dev/ttyACM1', debug=False)
+angle = mam.get_joints_angle()
+print(angle)
+# mam.set_tool_led_color(255,255,255)
+# for i in range(8):
+#     mam.set_servo_calibrate(i)
+#     time.sleep(0.5)
+
+# def linear_transform(x):
+#     # 两个已知数据点
+#     x1, y1 = -89.5, 0.022
+#     x2, y2 = 0, 0
+    
+#     # 计算斜率
+#     m = (y2 - y1) / (x2 - x1)
+    
+#     # 计算截距
+#     c = y1 - m * x1
+    
+#     # 应用线性变换
+#     y = m * x + c
+    
+#     return y
+
+# # 测试
+# print(linear_transform(-89.5))  # 应该输出0.022
+# print(linear_transform(0))      # 应该输出0
+
+
+
+
--- a/myArm/myarm_c650/scripts/detect_marker.py
+++ b/myArm/myarm_c650/scripts/detect_marker.py
@ -0,0 +1,123 @@
+#!/usr/bin/env python
+import rospy
+import cv2 as cv
+import numpy as np
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+import tf
+from tf.broadcaster import TransformBroadcaster
+import tf_conversions
+from mycobot_communication.srv import (
+    GetCoords,
+    SetCoords,
+    GetAngles,
+    SetAngles,
+    GripperStatus,
+)
+
+
+class ImageConverter:
+    def __init__(self):
+        self.br = TransformBroadcaster()
+        self.bridge = CvBridge()
+        self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
+        self.aruo_params = cv.aruco.DetectorParameters_create()
+        calibrationParams = cv.FileStorage(
+            "calibrationFileName.xml", cv.FILE_STORAGE_READ
+        )
+        self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
+        self.camera_matrix = None
+        # subscriber, listen wether has img come in. 订阅者，监听是否有img
+        self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
+
+    def callback(self, data):
+        """Callback function.
+
+        Process image with OpenCV, detect Mark to get the pose. Then acccording the
+        pose to transforming.
+        """
+        try:
+            # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+        except CvBridgeError as e:
+            print(e)
+        size = cv_image.shape
+        focal_length = size[1]
+        center = [size[1] / 2, size[0] / 2]
+        if self.camera_matrix is None:
+            # calc the camera matrix, if don't have.如果没有，则计算相机矩阵
+            self.camera_matrix = np.array(
+                [
+                    [focal_length, 0, center[0]],
+                    [0, focal_length, center[1]],
+                    [0, 0, 1],
+                ],
+                dtype=np.float32,
+            )
+        gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
+        # detect aruco marker.检测 aruco 标记
+        ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
+        corners, ids = ret[0], ret[1]
+        # process marker data.处理标记数据
+        if len(corners) > 0:
+            if ids is not None:
+                # print('corners:', corners, 'ids:', ids)
+
+                # detect marker pose. 检测marker位姿。
+                # argument:
+                #   marker corners,标记角
+                #   marker size (meter),标记尺寸（米）
+                ret = cv.aruco.estimatePoseSingleMarkers(
+                    corners, 0.05, self.camera_matrix, self.dist_coeffs
+                )
+                (rvec, tvec) = (ret[0], ret[1])
+                (rvec - tvec).any()
+
+                print("rvec:", rvec, "tvec:", tvec)
+
+                # just select first one detected marker.只需选择第一个检测到的标记。
+                for i in range(rvec.shape[0]):
+                    cv.aruco.drawDetectedMarkers(cv_image, corners)
+                    cv.aruco.drawAxis(
+                        cv_image,
+                        self.camera_matrix,
+                        self.dist_coeffs,
+                        rvec[i, :, :],
+                        tvec[i, :, :],
+                        0.03,
+                    )
+
+                xyz = tvec[0, 0, :]
+                xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
+
+                # get quaternion for ros. 为ros获取四元数
+                euler = rvec[0, 0, :]
+                tf_change = tf.transformations.quaternion_from_euler(
+                    euler[0], euler[1], euler[2]
+                )
+                print("tf_change:", tf_change)
+
+                # trans pose according [joint1]，根据 [joint1] 变换姿势
+                self.br.sendTransform(
+                    xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
+                )
+
+        # [x, y, z, -172, 3, -46.8]
+        cv.imshow("Image", cv_image)
+
+        cv.waitKey(3)
+        try:
+            pass
+        except CvBridgeError as e:
+            print(e)
+
+
+if __name__ == "__main__":
+    try:
+        rospy.init_node("detect_marker")
+        rospy.loginfo("Starting cv_bridge_test node")
+        ImageConverter()
+        rospy.spin()
+    except KeyboardInterrupt:
+        print("Shutting down cv_bridge_test node.")
+        cv.destroyAllWindows()
--- a/myArm/myarm_c650/scripts/follow_and_pump.py
+++ b/myArm/myarm_c650/scripts/follow_and_pump.py
@ -0,0 +1,194 @@
+#!/usr/bin/env python2
+# coding:utf-8
+import rospy
+from visualization_msgs.msg import Marker
+import time
+import os
+
+# Type of message communicated with mycobot，与 mycobot 通信的消息类型
+from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
+
+
+rospy.init_node("gipper_subscriber", anonymous=True)
+
+# Control the topic of mycobot, followed by angle, coordinates, gripper
+# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
+angle_pub = rospy.Publisher("mycobot/angles_goal",
+                            MycobotSetAngles, queue_size=5)
+coord_pub = rospy.Publisher("mycobot/coords_goal",
+                            MycobotSetCoords, queue_size=5)
+# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
+robot = os.popen("ls /dev/ttyUSB*").readline()
+
+if "dev" in robot:
+    Pin = [2, 5]
+else:
+    Pin = [20, 21]
+
+pump_pub = rospy.Publisher("mycobot/pump_status",
+                           MycobotPumpStatus, queue_size=5)
+
+# instantiate the message object，实例化消息对象
+angles = MycobotSetAngles()
+coords = MycobotSetCoords()
+pump = MycobotPumpStatus()
+
+# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
+x_offset = -20
+y_offset = 20
+z_offset = 110
+
+# With this variable limit, the fetching behavior is only done once
+# 通过该变量限制，抓取行为只做一次
+flag = False
+
+# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
+temp_x = temp_y = temp_z = 0.0
+
+temp_time = time.time()
+
+
+def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
+    """Post coordinates,发布坐标"""
+    coords.x = x
+    coords.y = y
+    coords.z = z
+    coords.rx = rx
+    coords.ry = ry
+    coords.rz = rz
+    coords.speed = 70
+    coords.model = m
+    # print(coords)
+    coord_pub.publish(coords)
+
+
+def pub_angles(a, b, c, d, e, f, sp):
+    """Publishing angle,发布角度"""
+    angles.joint_1 = float(a)
+    angles.joint_2 = float(b)
+    angles.joint_3 = float(c)
+    angles.joint_4 = float(d)
+    angles.joint_5 = float(e)
+    angles.joint_6 = float(f)
+    angles.speed = sp
+    angle_pub.publish(angles)
+
+
+def pub_pump(flag, Pin):
+    """Publish gripper status,发布夹爪状态"""
+    pump.Status = flag
+    pump.Pin1 = Pin[0]
+    pump.Pin2 = Pin[1]
+    pump_pub.publish(pump)
+
+
+def target_is_moving(x, y, z):
+    """Determine whether the target moves"""
+    """判断目标是否移动"""
+    count = 0
+    for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
+        print(o, n)
+        if abs(o - n) < 2:
+            count += 1
+    print(count)
+    if count == 3:
+        return False
+    return True
+
+
+def grippercallback(data):
+    """callback function,回调函数"""
+    global flag, temp_x, temp_y, temp_z
+    # rospy.loginfo('gripper_subscriber get date :%s', data)
+    if flag:
+        return
+
+    # Parse out the coordinate value,解析出坐标值
+    # pump length: 88mm
+    x = float(format(data.pose.position.x * 1000, ".2f"))
+    y = float(format(data.pose.position.y * 1000, ".2f"))
+    z = float(format(data.pose.position.z * 1000, ".2f"))
+
+    # When the running time is less than 30s, or the target position is still changing, perform tracking behavior
+    # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
+    if (
+        time.time() - temp_time < 30
+        or (temp_x == temp_y == temp_z == 0.0)
+        or target_is_moving(x - x_offset, y - y_offset, z)
+    ):
+
+        x -= x_offset
+        y -= y_offset
+        pub_coords(x - 20, y, 280)
+        time.sleep(0.1)
+
+        temp_x, temp_y, temp_z = x, y, z
+        return
+    else:  #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
+
+        print(x, y, z)
+
+        # detect heigth + pump height + limit height + offset
+        x += x_offset
+        y += y_offset
+        z = z + 88 + z_offset
+
+        pub_coords(x, y, z)
+        time.sleep(2.5)
+
+        # down
+        for i in range(1, 17):
+            pub_coords(x, y, z - i * 5, rx=-160, sp=10)
+            time.sleep(0.1)
+
+        time.sleep(2)
+
+        pub_pump(True, Pin)
+        # pump on
+
+        pub_coords(x, y, z + 20, -165)
+        time.sleep(1.5)
+
+        pub_angles(0, 30, -50, -40, 0, 0, 50)
+        time.sleep(1.5)
+
+        put_z = 140
+        pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
+        time.sleep(1.5)
+
+        for i in range(1, 8):
+            pub_coords(39.4, -174.7, put_z - i * 5, -
+                       177.13, -4.13, -152.59, 15, 2)
+            time.sleep(0.1)
+
+        pub_pump(False, Pin)
+
+        time.sleep(0.5)
+
+        pub_angles(0, 30, -50, -40, 0, 0, 50)
+        time.sleep(1.5)
+
+        # finally
+        flag = True
+
+
+def main():
+    for _ in range(10):
+        # pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
+        pub_angles(0, 30, -50, -40, 0, 0, 50)
+        #     pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
+        time.sleep(0.5)
+
+    pub_pump(False, Pin)
+    # time.sleep(2.5)
+
+    # mark 信息的订阅者,subscribers to mark information
+    rospy.Subscriber("visualization_marker", Marker,
+                     grippercallback, queue_size=1)
+
+    print("gripper test")
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()
--- a/myArm/myarm_c650/scripts/follow_display.py
+++ b/myArm/myarm_c650/scripts/follow_display.py
@ -0,0 +1,104 @@
+#!/usr/bin/env python2
+import time
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from visualization_msgs.msg import Marker
+
+# from pymycobot.myarm import MyArm
+from pymycobot.myarmc import MyArmC
+
+
+def talker():
+    rospy.init_node("display", anonymous=True)
+
+    print("Try connect real mycobot...")
+    port = rospy.get_param("~port", "/dev/ttyACM0")
+    baud = rospy.get_param("~baud", 1000000)
+    print("port: {}, baud: {}\n".format(port, baud))
+    try:
+        mycobot = MyArmC(port, baud)
+    except Exception as e:
+        print(e)
+        print(
+            """\
+            \rTry connect mycobot failed!
+            \rPlease check wether connected with mycobot.
+            \rPlease chckt wether the port or baud is right.
+        """
+        )
+        exit(1)
+    mycobot.release_all_servos(0)
+    time.sleep(0.1)
+    print("Rlease all servos over.\n")
+
+    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+    pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
+    rate = rospy.Rate(30)  # 30hz
+
+    # pub joint state
+    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",
+        "endeffector_to_joint5",
+    ]
+    joint_state_send.velocity = [0]
+    joint_state_send.effort = []
+
+    marker_ = Marker()
+    marker_.header.frame_id = "/base"
+    marker_.ns = "my_namespace"
+
+    print("publishing ...")
+    while not rospy.is_shutdown():
+        joint_state_send.header.stamp = rospy.Time.now()
+
+        angles = mycobot.get_radians()
+        data_list = []
+        for index, value in enumerate(angles):
+            data_list.append(value)
+
+        # rospy.loginfo('{}'.format(data_list))
+        joint_state_send.position = data_list
+
+        pub.publish(joint_state_send)
+
+        coords = mycobot.get_coords()
+
+        # marker
+        marker_.header.stamp = rospy.Time.now()
+        marker_.type = marker_.SPHERE
+        marker_.action = marker_.ADD
+        marker_.scale.x = 0.04
+        marker_.scale.y = 0.04
+        marker_.scale.z = 0.04
+
+        # marker position initial.标记位置初始
+        # print(coords)
+        if not coords:
+            coords = [0, 0, 0, 0, 0, 0]
+            rospy.loginfo("error [101]: can not get coord values")
+
+        marker_.pose.position.x = coords[1] / 1000 * -1
+        marker_.pose.position.y = coords[0] / 1000
+        marker_.pose.position.z = coords[2] / 1000
+
+        marker_.color.a = 1.0
+        marker_.color.g = 1.0
+        pub_marker.publish(marker_)
+
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
--- a/myArm/myarm_c650/scripts/following_marker.py
+++ b/myArm/myarm_c650/scripts/following_marker.py
@ -0,0 +1,64 @@
+#!/usr/bin/env python2
+import time
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from visualization_msgs.msg import Marker
+import tf
+
+
+def talker():
+    rospy.init_node("following_marker", anonymous=True)
+
+    pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
+    rate = rospy.Rate(20)
+
+    listener = tf.TransformListener()
+
+    marker_ = Marker()
+    marker_.header.frame_id = "/base"
+    marker_.ns = "basic_cube"
+
+    print("publishing ...")
+    while not rospy.is_shutdown():
+        now = rospy.Time.now() - rospy.Duration(0.1)
+
+        try:
+            trans, rot = listener.lookupTransform("base", "basic_shapes", now)
+        except Exception as e:
+            print(e)
+            continue
+
+        print(type(trans), trans)
+        print(type(rot), rot)
+
+        # marker
+        marker_.header.stamp = now
+        marker_.type = marker_.CUBE
+        marker_.action = marker_.ADD
+        marker_.scale.x = 0.04
+        marker_.scale.y = 0.04
+        marker_.scale.z = 0.04
+
+        # marker position initial,标记位置初始化
+        marker_.pose.position.x = trans[0]
+        marker_.pose.position.y = trans[1]
+        marker_.pose.position.z = trans[2]
+        marker_.pose.orientation.x = rot[0]
+        marker_.pose.orientation.y = rot[1]
+        marker_.pose.orientation.z = rot[2]
+        marker_.pose.orientation.w = rot[3]
+
+        marker_.color.a = 1.0
+        marker_.color.g = 1.0
+        pub_marker.publish(marker_)
+
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
--- a/myArm/myarm_c650/scripts/listen_real.py
+++ b/myArm/myarm_c650/scripts/listen_real.py
@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+# license removed for brevity
+import time
+import math
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from myarm_communication.srv import GetAngles
+
+
+def talker():
+    rospy.loginfo("start ...")
+    rospy.init_node("real_listener", anonymous=True)
+    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+    rate = rospy.Rate(30)  # 30hz
+
+    # pub joint state，发布关节状态
+    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",
+        "endeffector_to_joint5",
+    ]
+    joint_state_send.velocity = [0]
+    joint_state_send.effort = []
+
+    # waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
+    rospy.loginfo("wait service")
+    rospy.wait_for_service("get_joint_angles")
+    func = rospy.ServiceProxy("get_joint_angles", GetAngles)
+
+    rospy.loginfo("start loop ...")
+    while not rospy.is_shutdown():
+        # get real angles from server.从服务器获得真实的角度。
+        res = func()
+        if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
+            continue
+        radians_list = [
+            res.joint_1 * (math.pi / 180),
+            res.joint_2 * (math.pi / 180),
+            res.joint_3 * (math.pi / 180),
+            res.joint_4 * (math.pi / 180),
+            res.joint_5 * (math.pi / 180),
+            res.endeffector * (math.pi / 180),
+        ]
+        rospy.loginfo("res: {}".format(radians_list))
+
+        # publish angles.发布角度
+        joint_state_send.header.stamp = rospy.Time.now()
+        joint_state_send.position = radians_list
+        pub.publish(joint_state_send)
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
--- a/myArm/myarm_c650/scripts/listen_real_of_topic.py
+++ b/myArm/myarm_c650/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 myarm_communication.msg import MyArmAngles
+
+
+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", MyArmAngles, self.callback)
+        rospy.spin()
+
+    def callback(self, data):
+        """`myarm/angles_real` subscriber callback method.
+
+        Args:
+            data (MyArmAngles): 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
--- a/myArm/myarm_c650/scripts/simple_gui.py
+++ b/myArm/myarm_c650/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 myarm_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("myarmc ros GUI")
+    Window(window).run()
+
+
+if __name__ == "__main__":
+    main()
--- a/myArm/myarm_c650/scripts/slider_control.py
+++ b/myArm/myarm_c650/scripts/slider_control.py
@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+
+"""[summary]
+This file obtains the joint angle of the manipulator in ROS,
+and then sends it directly to the real manipulator using `pymycobot` API.
+This file is [slider_control.launch] related script.
+Passable parameters:
+    port: serial prot string. Defaults is '/dev/ttyUSB0'
+    baud: serial prot baudrate. Defaults is 115200.
+"""
+import time
+import math
+import rospy
+from sensor_msgs.msg import JointState
+
+from pymycobot.myarm import MyArm
+from pymycobot.myarmc import MyArmC
+
+
+mc = None
+
+
+def callback(data):
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+
+    data_list = []
+    for index, value in enumerate(data.position):
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
+
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
+
+
+
+def listener():
+    global mc
+    rospy.init_node("control_slider", anonymous=True)
+
+    rospy.Subscriber("joint_states", JointState, callback)
+    port = rospy.get_param("~port", "/dev/ttyACM0")
+    baud = rospy.get_param("~baud", 1000000)
+    print(port, baud)
+    mc = MyArmC(port, baud)
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
+    # spin() simply keeps python from exiting until this node is stopped
+    # spin()只是阻止python退出，直到该节点停止
+    print("spin ...")
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    listener()
--- a/myArm/myarm_c650/scripts/teleop_keyboard.py
+++ b/myArm/myarm_c650/scripts/teleop_keyboard.py
@ -0,0 +1,179 @@
+#!/usr/bin/env python
+from __future__ import print_function
+from myarm_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
+import rospy
+import sys
+import select
+import termios
+import tty
+import time
+
+import roslib
+
+# Terminal output prompt information. 终端输出提示信息
+msg = """\
+MyArmC 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 = 0
+    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:
+        print("start service...")
+        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(*init_pose)
+
+    while True:
+        res = get_coords()
+        print('current coords: {}'.format(res))
+        if res.x > 1:
+            break
+        else:
+            print('res.X < 1 {}'.format(res))
+            print('Please adjust the joint position of the robot and run program again !!!')
+        time.sleep(0.1)
+
+    record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
+    print(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)
+                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
--- a/myArm/myarm_c650/scripts/test.py
+++ b/myArm/myarm_c650/scripts/test.py
@ -0,0 +1,69 @@
+#!/usr/bin/env python
+
+"""
+This package need `pymycobot`.
+This file for test the API if right.
+
+Just can run in Linux.
+"""
+
+import rospy
+from math import pi
+import time
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from pymycobot.myarmc import MyArmC
+import rosnode
+    
+def linear_transform(x):
+    # 两个已知数据点
+    x1, y1 = -89.5, 0.022
+    x2, y2 = 0, 0
+    
+    # 计算斜率
+    m = (y2 - y1) / (x2 - x1)
+    
+    # 计算截距
+    c = y1 - m * x1
+    
+    # 应用线性变换
+    y = m * x + c
+    
+    return y
+
+global mam
+mam = MyArmC('/dev/ttyACM0', debug=False)
+angle = mam.get_joints_angle()
+angle[1] += 20
+# 初始化ROS节点
+rospy.init_node("pub_data")
+rosnode.kill_nodes('joint_state_publisher_gui')
+rospy.loginfo("已成功杀死节点")
+# 创建发布者
+pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
+# 设置发布频率
+rate = rospy.Rate(100) # 100Hz
+# 消息实例
+joint_state = JointState()
+# 发布消息
+while not rospy.is_shutdown():
+    joint_state.header = Header()
+    # 填充消息内容，例如关节名称、位置、速度和力
+    joint_state.header.stamp = rospy.Time.now()
+    joint_state.name = ['joint1', 'joint2', 'joint3','joint4', 'joint5', 'joint6','gripper']
+    angles = mam.get_joints_angle()
+    gripper_angle =  angles.pop(6)
+    angle = [a/180*pi for a in angles]
+    gripper_angle = linear_transform(gripper_angle)
+    # gripper_angle = linear_transform(-107.66)
+    angle.append(gripper_angle)
+    
+    joint_state.position = angle
+    joint_state.effort = []
+    joint_state.velocity = []
+    # 发布消息
+    pub.publish(joint_state)
+    rospy.loginfo('消息成功发布')
+    # 等待，允许其他节点处理
+    rate.sleep()
+
--- a/myArm/myarm_c650/src/camera_display.cpp
+++ b/myArm/myarm_c650/src/camera_display.cpp
@ -0,0 +1,29 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+void imageCallback(const sensor_msgs::ImageConstPtr &msg)
+{
+	try
+	{
+		cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
+		cv::waitKey(30);
+	}
+	catch (cv_bridge::Exception &e)
+	{
+		ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
+	}
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "image_listener");
+	ros::NodeHandle nh;
+	cv::namedWindow("view");
+	cv::startWindowThread();
+	image_transport::ImageTransport it(nh);
+	image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
+	ros::spin();
+	cv::destroyWindow("view");
+}
--- a/myArm/myarm_c650/src/opencv_camera.cpp
+++ b/myArm/myarm_c650/src/opencv_camera.cpp
@ -0,0 +1,60 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sstream> // for converting the command line parameter to integer,用于将命令行参数转换为整数
+
+int main(int argc, char **argv)
+{
+    // Check if video source has been passed as a parameter,检查视频源是否已作为参数传递
+    if (argv[1] == NULL)
+    {
+        ROS_INFO("argv[1]=NULL\n");
+        return 1;
+    }
+
+    ros::init(argc, argv, "image_publisher"); // Initialize node,初始化节点
+    ros::NodeHandle nh;
+    image_transport::ImageTransport it(nh);
+    image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic，发布话题
+
+    ros::Rate loop_rate(200); // refresh Hz.
+
+    // Convert the passed as command line parameter index for the video device to an integer，
+    // 将作为命令行参数传递的视频设备索引转换为整数
+    std::istringstream video_sourceCmd(argv[1]);
+    int video_source;
+    // Check if it is indeed a number，检查它是否确实是一个数字
+    if (!(video_sourceCmd >> video_source))
+    {
+        ROS_INFO("video_sourceCmd is %d\n", video_source);
+        return 1;
+    }
+
+    cv::VideoCapture cap(video_source);
+    // Check if video device can be opened with the given index，检查是否可以使用给定的索引打开视频设备
+    if (!cap.isOpened())
+    {
+        ROS_INFO("can not opencv video device\n");
+        return 1;
+    }
+    cv::Mat frame;
+    sensor_msgs::ImagePtr msg;
+
+    while (nh.ok())
+    {
+        cap >> frame;
+        // cv::imshow("veiwer", frame);
+        // Check if grabbed frame is actually full with some content，检查抓取的帧是否实际上充满了一些内容
+        if (!frame.empty())
+        {
+            msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
+            pub.publish(msg);
+            //cv::Wait(1);
+        }
+        ros::spinOnce();
+        loop_rate.sleep();
+        // if(cv::waitKey(2) >= 0)
+        // break;
+    }
+}
--- a/myArm/myarm_communication/launch/communication_service.launch
+++ b/myArm/myarm_communication/launch/communication_service.launch
@ -1,8 +1,8 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->

-	<arg name="port" default="/dev/ttyAMA0" />
-	<arg name="baud" default="115200" />
+	<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="1000000" />


 	<!-- Open communication service ，开启通讯服务-->
--- a/myArm/myarm_communication/launch/communication_topic.launch
+++ b/myArm/myarm_communication/launch/communication_topic.launch
@ -1,7 +1,7 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->
-	<arg name="port" default="/dev/ttyAMA0" />
-	<arg name="baud" default="115200" />
+	<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="1000000" />

 	<!-- Open communication service，开启通讯服务 -->
 	<node name="myarm_services" pkg="myarm_communication" type="myarm_topics.py" output="screen">
--- a/myArm/myarm_communication/scripts/myarm_services.py
+++ b/myArm/myarm_communication/scripts/myarm_services.py
@ -7,6 +7,7 @@ import fcntl
 from myarm_communication.srv import *

 from pymycobot.myarm import MyArm
+from pymycobot.myarmc import MyArmC

 mc = None

@ -51,12 +52,12 @@ def release(lock_file_fd):

 def create_handle():
    global mc
-    rospy.init_node("myarm")
+    rospy.init_node("myarmC")
    rospy.loginfo("start ...")
    port = rospy.get_param("~port")
    baud = rospy.get_param("~baud")
    rospy.loginfo("%s,%s" % (port, baud))
-    mc = MyArm(port, baud)
+    mc = MyArmC(port, baud)


 def create_services():
@ -78,8 +79,7 @@ def set_angles(req):
        req.joint_3,
        req.joint_4,
        req.joint_5,
-        req.joint_6,
-        req.joint_7,
+        req.endeffector,
    ]
    sp = req.speed

--- a/myArm/myarm_m/CMakeLists.txt
+++ b/myArm/myarm_m/CMakeLists.txt
@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(myarm_m)
+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})
--- a/myArm/myarm_m/LICENSE
+++ b/myArm/myarm_m/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.
--- a/myArm/myarm_m/config/myarm_m.rviz
+++ b/myArm/myarm_m/config/myarm_m.rviz
@ -0,0 +1,224 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Axes1
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 603
+  - 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
+        joint1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint7:
+          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.10000000149011612
+      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: true
+        base:
+          Value: true
+        joint1:
+          Value: true
+        joint2:
+          Value: true
+        joint3:
+          Value: true
+        joint4:
+          Value: true
+        joint5:
+          Value: true
+
+      Marker Alpha: 1
+      Marker Scale: 0.20000000298023224
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base:
+          joint1:
+            joint2:
+              joint3:
+                joint4:
+                  joint5:
+
+                        {}
+      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.2526283264160156
+      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.2103991061449051
+      Target Frame: <Fixed Frame>
+      Yaw: 2.6453990936279297
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 900
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000002e600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27
--- a/myArm/myarm_m/config/myarm_m_with_myarm_c650.rviz
+++ b/myArm/myarm_m/config/myarm_m_with_myarm_c650.rviz
@ -0,0 +1,238 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - 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
+    - Class: rviz/TF
+      Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - 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
+        endeffector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: myarm_m/robot_description
+      TF Prefix: myarm_m
+      Update Interval: 0
+      Value: true
+      Visual Enabled: 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
+        endeffector:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: myarm_c650/robot_description
+      TF Prefix: myarm_c650
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: 3.326688051223755
+      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.5303982496261597
+      Target Frame: <Fixed Frame>
+      Yaw: 0.2653948962688446
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 72
+  Y: 27
--- a/myArm/myarm_m/config/mycobot_with_marker.rviz
+++ b/myArm/myarm_m/config/mycobot_with_marker.rviz
@ -0,0 +1,216 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /TF1
+        - /Marker1
+        - /Marker1/Namespaces1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+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.0299999993
+        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
+        joint1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6_flange:
+          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
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        basic_shapes:
+          Value: true
+        joint1:
+          Value: true
+        joint2:
+          Value: true
+        joint3:
+          Value: true
+        joint4:
+          Value: true
+        joint5:
+          Value: true
+        joint6:
+          Value: true
+        joint6_flange:
+          Value: true
+      Marker Scale: 0.300000012
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        joint1:
+          joint2:
+            joint3:
+              joint4:
+                joint5:
+                  joint6:
+                    joint6_flange:
+                      basic_shapes:
+                        {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /visualization_marker
+      Name: Marker
+      Namespaces:
+        basic_cube: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: joint1
+    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
+      Topic: /initialpose
+    - 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: 2.11990476
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.0706475973
+        Y: -0.0814988762
+        Z: 0.107583851
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.375398338
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.235389769
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24
--- a/myArm/myarm_m/launch/combined_control.launch
+++ b/myArm/myarm_m/launch/combined_control.launch
@ -0,0 +1,41 @@
+<launch>
+    <arg name="rvizconfig" default="$(find myarm_m)/config/myarm_m_with_myarm_c650.rviz" />
+    <arg name="gui" default="true" />
+    <arg name="ns1" default="myarm_m" />
+    <arg name="ns2" default="myarm_c650" />
+
+    <!-- myarm_m -->
+    <group ns="$(arg ns1)">
+        <param name="robot_description" command="$(find xacro)/xacro --inorder $(find mycobot_description)/urdf/myarm_m/myarm_m.urdf.xacro ns:=$(arg ns1)" />
+        <!-- 关节控制gui -->
+        <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
+            <param name="use_gui" value="$(arg gui)" />
+        </node>
+        <!-- 发布tf -->
+        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
+        	<param name="publish_frequency" type="double" value="50.0" />
+	    	<param name="tf_prefix" value="$(arg ns1)" />
+		</node>
+        <!-- 设置map和机器人基座的坐标关系 -->
+        <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher1" args="0 0.5 0 0 0 0 /map /$(arg ns1)/base" />        
+    </group>
+
+    <!-- myarm_c650 -->
+    <group ns="$(arg ns2)">
+        <param name="robot_description" command="$(find xacro)/xacro --inorder $(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf.xacro ns:=$(arg ns2)" />
+        <!-- 关节控制gui -->
+        <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
+            <param name="use_gui" value="$(arg gui)" />
+        </node>
+        <!-- 发布tf -->
+        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
+        	<param name="publish_frequency" type="double" value="50.0" />
+	    	<param name="tf_prefix" value="$(arg ns2)" />
+		</node>
+        <!-- 设置map和机器人基座的坐标关系 -->
+        <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher1" args="0 -0.5 0 0 0 0 /map /$(arg ns2)/base" />        
+    </group>
+
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
--- a/myArm/myarm_m/launch/demo_rviz.launch
+++ b/myArm/myarm_m/launch/demo_rviz.launch
@ -0,0 +1,18 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_m/myarm_m.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm_m)/config/myarm_m.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to 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>
+
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+	
+</launch>
--- a/myArm/myarm_m/launch/detect_marker.launch
+++ b/myArm/myarm_m/launch/detect_marker.launch
@ -0,0 +1,18 @@
+<launch>
+	<!-- Load file model ，加载文件模型-->
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm)/config/mycobot_with_marker.rviz" />
+	<arg name="gui" default="true" />
+
+	<arg name="num" default="0" />
+
+    <include file="$(find myarm)/launch/slider_control.launch">
+		<arg name="model" value="$(arg model)" />
+		<arg name="rvizconfig" value="$(arg rvizconfig)" />
+		<arg name="gui" value="$(arg gui)" />
+    </include>
+	<!-- vision node -->
+	<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
+	<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
+	<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
+</launch>
--- a/myArm/myarm_m/launch/detect_marker_with_topic.launch
+++ b/myArm/myarm_m/launch/detect_marker_with_topic.launch
@ -0,0 +1,30 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyAMA0" />
+	<arg name="baud" default="115200" />
+	<!-- Load file model ，加载文件模型-->
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
+	<arg name="gui" default="false" />
+
+	<arg name="num" default="0" />
+
+	<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 -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+	<!-- mycobot-topics ，mycobot-话题-->
+    <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_of_topic.py" />
+	<!-- vision node -->
+	<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
+	<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
+	<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
+</launch>
--- a/myArm/myarm_m/launch/mycobot_follow.launch
+++ b/myArm/myarm_m/launch/mycobot_follow.launch
@ -0,0 +1,13 @@
+<launch>
+	<!-- Load URDF file,加载URDF文件 -->
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm)/config/myarm.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>
--- a/myArm/myarm_m/launch/simple_gui.launch
+++ b/myArm/myarm_m/launch/simple_gui.launch
@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyAMA0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm)/config/myarm.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 myarm_communication)/launch/communication_service.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+	<node name="real_listener" pkg="myarm" type="listen_real.py" />
+	<node name="simple_gui" pkg="myarm" type="simple_gui.py" />
+</launch>
--- a/myArm/myarm_m/launch/slider_control.launch
+++ b/myArm/myarm_m/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/myarm_300_pi/myarm_300_pi.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm)/config/myarm.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>
--- a/myArm/myarm_m/launch/teleop_keyboard.launch
+++ b/myArm/myarm_m/launch/teleop_keyboard.launch
@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyAMA0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
+	<arg name="rvizconfig" default="$(find myarm)/config/myarm.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 myarm_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="myarm" type="listen_real.py" />
+</launch>
--- a/myArm/myarm_m/launch/test02.launch
+++ b/myArm/myarm_m/launch/test02.launch
@ -0,0 +1,22 @@
+<launch>
+    <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find mycobot_description)/urdf/myarmM/myarmM.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <!-- <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find tw_robot)/urdf.rviz" /> -->
+  <node name="rviz" pkg="rviz" type="rviz" />
+
+</launch>
--- a/myArm/myarm_m/package.xml
+++ b/myArm/myarm_m/package.xml
@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>myarm_m</name>
+  <version>0.3.0</version>
+  <description>The myarm_m package</description>
+
+  <author email="shengjia.chen@elephantrobotics.com">Chenshengjia</author>
+  <maintainer email="shengjia.chen@elephantrobotics.com">Chenshengjia</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/elephantrobotics/mycobot_ros</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>mycobot_description</build_depend>
+  <build_depend>myarm_communication</build_depend>
+
+  <build_export_depend>myarm_communication</build_export_depend>
+  <build_export_depend>mycobot_description</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>python-tk</exec_depend>
+  <exec_depend>mycobot_description</exec_depend>
+  <exec_depend>myarm_communication</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/myArm/myarm_m/scripts/all_time_get_angle.py
+++ b/myArm/myarm_m/scripts/all_time_get_angle.py
@ -0,0 +1,18 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+from pymycobot import MyArmM
+from sensor_msgs.msg import JointState
+import rospy
+from math import pi
+import time
+# 1：[-174, 167]
+# 2:[-92, 93]
+# 3:[-91,103]
+# 4:[-170, 170]
+# 5:[-96,89]
+# 6:[-170,170]
+
+global mam
+mam = MyArmM('/dev/ttyACM0', debug=False)
+while True:
+    print('当前角度：', mam.get_joints_angle())
--- a/myArm/myarm_m/scripts/combined_control.py
+++ b/myArm/myarm_m/scripts/combined_control.py
@ -0,0 +1,117 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+from pymycobot import MyArmM, MyArmC
+from sensor_msgs.msg import JointState
+import rospy
+from std_msgs.msg import Header
+from math import pi
+import subprocess
+import sys
+import time
+import datetime
+import copy
+def shutdown_ros_node(node_name):
+    try:
+        subprocess.run(['rosnode', 'kill', node_name])
+        print(f"Node {node_name} has been shutdown.")
+    except subprocess.CalledProcessError as e:
+        print(f"Error: {e}")
+
+
+def linear_transform(x):
+    # 两个已知数据点
+    x1, y1 = -89.5, 0.022
+    x2, y2 = 0, 0
+    
+    # 计算斜率
+    m = (y2 - y1) / (x2 - x1)
+    
+    # 计算截距
+    c = y1 - m * x1
+    
+    # 应用线性变换
+    y = m * x + c
+    
+    return y
+
+
+def set_angle(mam, angle, speed=40):
+    # 弧度转角度
+    # angle = [int(a*180/pi) for a in angles]
+    # angle.append(0)
+    # joint_id = {0:1, 1:2, 2:3, 3:4, 4:5, 5:6}
+    # joint_id = [0,1,2,3,4,5,6]
+    mam.set_joints_angle(angle, speed)
+    
+    # for i in range(6):
+    #     # if joint_id[i] == 2 or joint_id[i] == 5:
+    #     #     angle[i] *= -1
+    #     mam.set_joint_angle(joint_id[i], -angle[i], 50)
+def main():
+    # 初始化ROS节点
+    rospy.init_node("combined_control", anonymous=True)
+    # 关闭节点
+    shutdown_ros_node('myarm_m/joint_state_publisher_gui')
+    shutdown_ros_node('myarm_c650/joint_state_publisher_gui')
+    # 发布对象
+    pub_m = rospy.Publisher('myarm_m/joint_states', JointState, queue_size=10)
+    pub_c = rospy.Publisher('myarm_c650/joint_states', JointState, queue_size=10)
+    # 设置发布频率
+    rate = rospy.Rate(50)
+    # 消息实例
+    joint_state = JointState()
+    # 初始化api对象
+    myarm_m = MyArmM('/dev/ttyACM1', debug=False)
+    myarm_c = MyArmC('/dev/ttyACM0', debug=False)
+    # 使能
+    for i in range(8):
+        myarm_m.set_servo_enabled(i, 1)
+        time.sleep(0.2)
+
+    while not rospy.is_shutdown():
+        joint_state.header = Header()
+        # 填充消息内容，例如关节名称、位置、速度和力
+        joint_state.header.stamp = rospy.Time.now()
+        joint_state.name = ['joint1', 'joint2', 'joint3','joint4', 'joint5', 'joint6','gripper']
+        anglesc = myarm_c.get_joints_angle()
+        anglesm = copy.deepcopy(anglesc)
+        
+        gripper_angle_c_real = anglesc.pop(6)
+        angle_c = [a/180*pi for a in anglesc]
+        gripper_angle_c_sim = linear_transform(gripper_angle_c_real)
+        angle_c.append(gripper_angle_c_sim)
+        
+        gripper_angle_c_real = anglesm.pop(6) # 原来的夹角
+        gripper_angle_m_sim = gripper_angle_c_sim/0.022*0.0345
+        angle_m = [a*pi/180 for a in anglesm]
+        angle_m.append(gripper_angle_m_sim)
+        # gripper_angle_m /= -3500
+        # angle:弧度 angles:角度
+        
+        angle_m[2]*=-1
+        joint_state.position = angle_m
+        joint_state.effort = []
+        joint_state.velocity = []
+        # 发布消息
+        pub_m.publish(joint_state)
+        current_time1 = datetime.datetime.now()
+        joint_state.position = angle_c
+        pub_c.publish(joint_state)
+        current_time2 = datetime.datetime.now()
+        gripper_angle_m_sim = angle_m.pop(6)
+        gripper_angle_m_real = gripper_angle_m_sim*(-3500)
+        angle_m = [a*180/pi for a in angle_m]
+        angle_m.append(gripper_angle_m_real)
+        myarm_m.set_joints_angle(angle_m, 30)
+        current_time = current_time2-current_time1
+        print(angle_m)
+        rospy.loginfo('消息成功发布')
+        rospy.loginfo(current_time)
+        # 等待，允许其他节点处理
+        rate.sleep()
+        
+
+
+
+if __name__ == '__main__':
+    main()
--- a/myArm/myarm_m/scripts/control_by_rviz.py
+++ b/myArm/myarm_m/scripts/control_by_rviz.py
@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+# 订阅RVIZ数据，控制实际机器人运动
+from pymycobot import MyArmM
+from sensor_msgs.msg import JointState
+import rospy
+from math import pi
+import time
+# 1：[-174, 167]
+# 2:[-92, 93]
+# 3:[-91,103]
+# 4:[-170, 170]
+# 5:[-96,89]
+# 6:[-170,170]
+
+global mam
+mam = MyArmM('/dev/ttyACM0', debug=False)
+for i in range(8):
+    mam.set_servo_enabled(i, 1)
+    time.sleep(0.2)
+
+def callback_fun(msg): # 回调函数
+    # print(type(msg.position))
+    angles = list(msg.position)
+    angles.pop(7)
+    gripper_angle = angles.pop(6)
+    angle = [a*180/pi for a in angles]
+    angle.append(gripper_angle*(-3500))
+    rospy.loginfo("The Subscriber Data:%s", angle)
+    set_angle(mam, angle, 10)
+    # set_angle(angle, 20)
+    # for i in range(6):
+        # mam.set_joint_angle(i, angle[i]*180/pi, 50) # 角度直接控制
+        # set_angle_by_encoder(i, angle[i]*180/pi, 50) # 电位值控制
+
+def main():
+    rospy.init_node("pub_rviz_data", anonymous=True)
+    rospy.Subscriber('/joint_states', JointState, callback_fun)
+    rospy.spin()
+
+def set_angle(mam, angles, speed):
+    # 弧度转角度
+    mam.set_joints_angle(angles, speed)
+
+# def set_angle_by_encoder(servo_id, angle, speed): # 输入角度 通过电位值控制机器人
+#     # 1:left 2:right 3:left 4:left 5:left 6:right 7:left
+#     joint_id = {0:1, 1:2, 2:4, 3:5, 4:6, 5:7}
+#     # right_axis = [0, ]
+#     servo_id = joint_id[servo_id]
+#     if servo_id == 2 or servo_id == 6:
+#         pass
+#     else:
+#         angle *= -1
+#     encoder = int(2024 + 2024/180*angle)
+#     # encoder =
+#     mam.set_servo_encoder(servo_id, encoder, speed)
+
+if __name__ == '__main__':
+    main()
--- a/myArm/myarm_m/scripts/detect_marker.py
+++ b/myArm/myarm_m/scripts/detect_marker.py
@ -0,0 +1,123 @@
+#!/usr/bin/env python
+import rospy
+import cv2 as cv
+import numpy as np
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+import tf
+from tf.broadcaster import TransformBroadcaster
+import tf_conversions
+from mycobot_communication.srv import (
+    GetCoords,
+    SetCoords,
+    GetAngles,
+    SetAngles,
+    GripperStatus,
+)
+
+
+class ImageConverter:
+    def __init__(self):
+        self.br = TransformBroadcaster()
+        self.bridge = CvBridge()
+        self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
+        self.aruo_params = cv.aruco.DetectorParameters_create()
+        calibrationParams = cv.FileStorage(
+            "calibrationFileName.xml", cv.FILE_STORAGE_READ
+        )
+        self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
+        self.camera_matrix = None
+        # subscriber, listen wether has img come in. 订阅者，监听是否有img
+        self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
+
+    def callback(self, data):
+        """Callback function.
+
+        Process image with OpenCV, detect Mark to get the pose. Then acccording the
+        pose to transforming.
+        """
+        try:
+            # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+        except CvBridgeError as e:
+            print(e)
+        size = cv_image.shape
+        focal_length = size[1]
+        center = [size[1] / 2, size[0] / 2]
+        if self.camera_matrix is None:
+            # calc the camera matrix, if don't have.如果没有，则计算相机矩阵
+            self.camera_matrix = np.array(
+                [
+                    [focal_length, 0, center[0]],
+                    [0, focal_length, center[1]],
+                    [0, 0, 1],
+                ],
+                dtype=np.float32,
+            )
+        gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
+        # detect aruco marker.检测 aruco 标记
+        ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
+        corners, ids = ret[0], ret[1]
+        # process marker data.处理标记数据
+        if len(corners) > 0:
+            if ids is not None:
+                # print('corners:', corners, 'ids:', ids)
+
+                # detect marker pose. 检测marker位姿。
+                # argument:
+                #   marker corners,标记角
+                #   marker size (meter),标记尺寸（米）
+                ret = cv.aruco.estimatePoseSingleMarkers(
+                    corners, 0.05, self.camera_matrix, self.dist_coeffs
+                )
+                (rvec, tvec) = (ret[0], ret[1])
+                (rvec - tvec).any()
+
+                print("rvec:", rvec, "tvec:", tvec)
+
+                # just select first one detected marker.只需选择第一个检测到的标记。
+                for i in range(rvec.shape[0]):
+                    cv.aruco.drawDetectedMarkers(cv_image, corners)
+                    cv.aruco.drawAxis(
+                        cv_image,
+                        self.camera_matrix,
+                        self.dist_coeffs,
+                        rvec[i, :, :],
+                        tvec[i, :, :],
+                        0.03,
+                    )
+
+                xyz = tvec[0, 0, :]
+                xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
+
+                # get quaternion for ros. 为ros获取四元数
+                euler = rvec[0, 0, :]
+                tf_change = tf.transformations.quaternion_from_euler(
+                    euler[0], euler[1], euler[2]
+                )
+                print("tf_change:", tf_change)
+
+                # trans pose according [joint1]，根据 [joint1] 变换姿势
+                self.br.sendTransform(
+                    xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
+                )
+
+        # [x, y, z, -172, 3, -46.8]
+        cv.imshow("Image", cv_image)
+
+        cv.waitKey(3)
+        try:
+            pass
+        except CvBridgeError as e:
+            print(e)
+
+
+if __name__ == "__main__":
+    try:
+        rospy.init_node("detect_marker")
+        rospy.loginfo("Starting cv_bridge_test node")
+        ImageConverter()
+        rospy.spin()
+    except KeyboardInterrupt:
+        print("Shutting down cv_bridge_test node.")
+        cv.destroyAllWindows()
--- a/myArm/myarm_m/scripts/follow_and_pump.py
+++ b/myArm/myarm_m/scripts/follow_and_pump.py
@ -0,0 +1,194 @@
+#!/usr/bin/env python2
+# coding:utf-8
+import rospy
+from visualization_msgs.msg import Marker
+import time
+import os
+
+# Type of message communicated with mycobot，与 mycobot 通信的消息类型
+from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
+
+
+rospy.init_node("gipper_subscriber", anonymous=True)
+
+# Control the topic of mycobot, followed by angle, coordinates, gripper
+# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
+angle_pub = rospy.Publisher("mycobot/angles_goal",
+                            MycobotSetAngles, queue_size=5)
+coord_pub = rospy.Publisher("mycobot/coords_goal",
+                            MycobotSetCoords, queue_size=5)
+# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
+robot = os.popen("ls /dev/ttyUSB*").readline()
+
+if "dev" in robot:
+    Pin = [2, 5]
+else:
+    Pin = [20, 21]
+
+pump_pub = rospy.Publisher("mycobot/pump_status",
+                           MycobotPumpStatus, queue_size=5)
+
+# instantiate the message object，实例化消息对象
+angles = MycobotSetAngles()
+coords = MycobotSetCoords()
+pump = MycobotPumpStatus()
+
+# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
+x_offset = -20
+y_offset = 20
+z_offset = 110
+
+# With this variable limit, the fetching behavior is only done once
+# 通过该变量限制，抓取行为只做一次
+flag = False
+
+# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
+temp_x = temp_y = temp_z = 0.0
+
+temp_time = time.time()
+
+
+def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
+    """Post coordinates,发布坐标"""
+    coords.x = x
+    coords.y = y
+    coords.z = z
+    coords.rx = rx
+    coords.ry = ry
+    coords.rz = rz
+    coords.speed = 70
+    coords.model = m
+    # print(coords)
+    coord_pub.publish(coords)
+
+
+def pub_angles(a, b, c, d, e, f, sp):
+    """Publishing angle,发布角度"""
+    angles.joint_1 = float(a)
+    angles.joint_2 = float(b)
+    angles.joint_3 = float(c)
+    angles.joint_4 = float(d)
+    angles.joint_5 = float(e)
+    angles.joint_6 = float(f)
+    angles.speed = sp
+    angle_pub.publish(angles)
+
+
+def pub_pump(flag, Pin):
+    """Publish gripper status,发布夹爪状态"""
+    pump.Status = flag
+    pump.Pin1 = Pin[0]
+    pump.Pin2 = Pin[1]
+    pump_pub.publish(pump)
+
+
+def target_is_moving(x, y, z):
+    """Determine whether the target moves"""
+    """判断目标是否移动"""
+    count = 0
+    for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
+        print(o, n)
+        if abs(o - n) < 2:
+            count += 1
+    print(count)
+    if count == 3:
+        return False
+    return True
+
+
+def grippercallback(data):
+    """callback function,回调函数"""
+    global flag, temp_x, temp_y, temp_z
+    # rospy.loginfo('gripper_subscriber get date :%s', data)
+    if flag:
+        return
+
+    # Parse out the coordinate value,解析出坐标值
+    # pump length: 88mm
+    x = float(format(data.pose.position.x * 1000, ".2f"))
+    y = float(format(data.pose.position.y * 1000, ".2f"))
+    z = float(format(data.pose.position.z * 1000, ".2f"))
+
+    # When the running time is less than 30s, or the target position is still changing, perform tracking behavior
+    # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
+    if (
+        time.time() - temp_time < 30
+        or (temp_x == temp_y == temp_z == 0.0)
+        or target_is_moving(x - x_offset, y - y_offset, z)
+    ):
+
+        x -= x_offset
+        y -= y_offset
+        pub_coords(x - 20, y, 280)
+        time.sleep(0.1)
+
+        temp_x, temp_y, temp_z = x, y, z
+        return
+    else:  #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
+
+        print(x, y, z)
+
+        # detect heigth + pump height + limit height + offset
+        x += x_offset
+        y += y_offset
+        z = z + 88 + z_offset
+
+        pub_coords(x, y, z)
+        time.sleep(2.5)
+
+        # down
+        for i in range(1, 17):
+            pub_coords(x, y, z - i * 5, rx=-160, sp=10)
+            time.sleep(0.1)
+
+        time.sleep(2)
+
+        pub_pump(True, Pin)
+        # pump on
+
+        pub_coords(x, y, z + 20, -165)
+        time.sleep(1.5)
+
+        pub_angles(0, 30, -50, -40, 0, 0, 50)
+        time.sleep(1.5)
+
+        put_z = 140
+        pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
+        time.sleep(1.5)
+
+        for i in range(1, 8):
+            pub_coords(39.4, -174.7, put_z - i * 5, -
+                       177.13, -4.13, -152.59, 15, 2)
+            time.sleep(0.1)
+
+        pub_pump(False, Pin)
+
+        time.sleep(0.5)
+
+        pub_angles(0, 30, -50, -40, 0, 0, 50)
+        time.sleep(1.5)
+
+        # finally
+        flag = True
+
+
+def main():
+    for _ in range(10):
+        # pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
+        pub_angles(0, 30, -50, -40, 0, 0, 50)
+        #     pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
+        time.sleep(0.5)
+
+    pub_pump(False, Pin)
+    # time.sleep(2.5)
+
+    # mark 信息的订阅者,subscribers to mark information
+    rospy.Subscriber("visualization_marker", Marker,
+                     grippercallback, queue_size=1)
+
+    print("gripper test")
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()
--- a/myArm/myarm_m/scripts/follow_display.py
+++ b/myArm/myarm_m/scripts/follow_display.py
@ -0,0 +1,104 @@
+#!/usr/bin/env python2
+import time
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from visualization_msgs.msg import Marker
+
+from pymycobot.myarm import MyArm
+
+
+def talker():
+    rospy.init_node("display", anonymous=True)
+
+    print("Try connect real mycobot...")
+    port = rospy.get_param("~port", "/dev/ttyAMA0")
+    baud = rospy.get_param("~baud", 115200)
+    print("port: {}, baud: {}\n".format(port, baud))
+    try:
+        mycobot = MyArm(port, baud)
+    except Exception as e:
+        print(e)
+        print(
+            """\
+            \rTry connect mycobot failed!
+            \rPlease check wether connected with mycobot.
+            \rPlease chckt wether the port or baud is right.
+        """
+        )
+        exit(1)
+    mycobot.release_all_servos(0)
+    time.sleep(0.1)
+    print("Rlease all servos over.\n")
+
+    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+    pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
+    rate = rospy.Rate(30)  # 30hz
+
+    # pub joint state
+    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 = []
+
+    marker_ = Marker()
+    marker_.header.frame_id = "/base"
+    marker_.ns = "my_namespace"
+
+    print("publishing ...")
+    while not rospy.is_shutdown():
+        joint_state_send.header.stamp = rospy.Time.now()
+
+        angles = mycobot.get_radians()
+        data_list = []
+        for index, value in enumerate(angles):
+            data_list.append(value)
+
+        # rospy.loginfo('{}'.format(data_list))
+        joint_state_send.position = data_list
+
+        pub.publish(joint_state_send)
+
+        coords = mycobot.get_coords()
+
+        # marker
+        marker_.header.stamp = rospy.Time.now()
+        marker_.type = marker_.SPHERE
+        marker_.action = marker_.ADD
+        marker_.scale.x = 0.04
+        marker_.scale.y = 0.04
+        marker_.scale.z = 0.04
+
+        # marker position initial.标记位置初始
+        # print(coords)
+        if not coords:
+            coords = [0, 0, 0, 0, 0, 0]
+            rospy.loginfo("error [101]: can not get coord values")
+
+        marker_.pose.position.x = coords[1] / 1000 * -1
+        marker_.pose.position.y = coords[0] / 1000
+        marker_.pose.position.z = coords[2] / 1000
+
+        marker_.color.a = 1.0
+        marker_.color.g = 1.0
+        pub_marker.publish(marker_)
+
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
--- a/myArm/myarm_m/scripts/following_marker.py
+++ b/myArm/myarm_m/scripts/following_marker.py
@ -0,0 +1,64 @@
+#!/usr/bin/env python2
+import time
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from visualization_msgs.msg import Marker
+import tf
+
+
+def talker():
+    rospy.init_node("following_marker", anonymous=True)
+
+    pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
+    rate = rospy.Rate(20)
+
+    listener = tf.TransformListener()
+
+    marker_ = Marker()
+    marker_.header.frame_id = "/base"
+    marker_.ns = "basic_cube"
+
+    print("publishing ...")
+    while not rospy.is_shutdown():
+        now = rospy.Time.now() - rospy.Duration(0.1)
+
+        try:
+            trans, rot = listener.lookupTransform("base", "basic_shapes", now)
+        except Exception as e:
+            print(e)
+            continue
+
+        print(type(trans), trans)
+        print(type(rot), rot)
+
+        # marker
+        marker_.header.stamp = now
+        marker_.type = marker_.CUBE
+        marker_.action = marker_.ADD
+        marker_.scale.x = 0.04
+        marker_.scale.y = 0.04
+        marker_.scale.z = 0.04
+
+        # marker position initial,标记位置初始化
+        marker_.pose.position.x = trans[0]
+        marker_.pose.position.y = trans[1]
+        marker_.pose.position.z = trans[2]
+        marker_.pose.orientation.x = rot[0]
+        marker_.pose.orientation.y = rot[1]
+        marker_.pose.orientation.z = rot[2]
+        marker_.pose.orientation.w = rot[3]
+
+        marker_.color.a = 1.0
+        marker_.color.g = 1.0
+        pub_marker.publish(marker_)
+
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
--- a/myArm/myarm_m/scripts/listen_real.py
+++ b/myArm/myarm_m/scripts/listen_real.py
@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+# license removed for brevity
+import time
+import math
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from myarm_communication.srv import GetAngles
+
+
+def talker():
+    rospy.loginfo("start ...")
+    rospy.init_node("real_listener", anonymous=True)
+    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+    rate = rospy.Rate(30)  # 30hz
+
+    # pub joint state，发布关节状态
+    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",
+        "endeffector_to_joint5",
+    ]
+    joint_state_send.velocity = [0]
+    joint_state_send.effort = []
+
+    # waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
+    rospy.loginfo("wait service")
+    rospy.wait_for_service("get_joint_angles")
+    func = rospy.ServiceProxy("get_joint_angles", GetAngles)
+
+    rospy.loginfo("start loop ...")
+    while not rospy.is_shutdown():
+        # get real angles from server.从服务器获得真实的角度。
+        res = func()
+        if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
+            continue
+        radians_list = [
+            res.joint_1 * (math.pi / 180),
+            res.joint_2 * (math.pi / 180),
+            res.joint_3 * (math.pi / 180),
+            res.joint_4 * (math.pi / 180),
+            res.joint_5 * (math.pi / 180),
+            res.joint_6 * (math.pi / 180),
+        ]
+        rospy.loginfo("res: {}".format(radians_list))
+
+        # publish angles.发布角度
+        joint_state_send.header.stamp = rospy.Time.now()
+        joint_state_send.position = radians_list
+        pub.publish(joint_state_send)
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass
--- a/myArm/myarm_m/scripts/listen_real_of_topic.py
+++ b/myArm/myarm_m/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 myarm_communication.msg import MyArmAngles
+
+
+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", MyArmAngles, self.callback)
+        rospy.spin()
+
+    def callback(self, data):
+        """`myarm/angles_real` subscriber callback method.
+
+        Args:
+            data (MyArmAngles): 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
--- a/myArm/myarm_m/scripts/read_control.py
+++ b/myArm/myarm_m/scripts/read_control.py
@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+# 读取机器人关节角数据，发送给RVIZ
+
+from pymycobot import MyArmM
+import rospy
+from math import pi
+import time
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+import subprocess
+# 1：[-174, 167]
+# 2:[-92, 93]
+# 3:[-91,103]
+# 4:[-170, 170]
+# 5:[-96,89]
+# 6:[-170,170]
+def shutdown_ros_node(node_name):
+    try:
+        subprocess.run(['rosnode', 'kill', node_name])
+        print(f"Node {node_name} has been shutdown.")
+    except subprocess.CalledProcessError as e:
+        print(f"Error: {e}")
+
+global mam
+mam = MyArmM('/dev/ttyACM1', debug=False)
+# 1 3 4 6
+# 放松关节
+for i in range(8):
+    mam.set_servo_enabled(i, 0)
+    time.sleep(0.2)
+
+# 初始化ROS节点
+rospy.init_node("pub_data")
+shutdown_ros_node('joint_state_publisher_gui')
+rospy.loginfo("已成功杀死节点")
+# 创建发布者
+pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
+# 设置发布频率
+rate = rospy.Rate(50) # 50Hz
+# 消息实例
+joint_state = JointState()
+# 发布消息
+while not rospy.is_shutdown():
+    joint_state.header = Header()
+    # 填充消息内容，例如关节名称、位置、速度和力
+    joint_state.header.stamp = rospy.Time.now()
+    joint_state.name = ['joint1', 'joint2', 'joint3','joint4', 'joint5', 'joint6', 'gripper']
+    angles = mam.get_joints_angle()
+    gripper_angle = angles.pop(6)
+    gripper_angle /= -3500
+    # angle:弧度 angles:角度
+    angle = [a/180*pi for a in angles]
+    angle.append(gripper_angle)
+    print(angle)
+    joint_state.position = angle
+    joint_state.effort = []
+    joint_state.velocity = []
+    # 发布消息
+    pub.publish(joint_state)
+    rospy.loginfo('消息成功发布')
+    # 等待，允许其他节点处理
+    rate.sleep()
+
+
+# print(mam.get_servos_encoder())
--- a/myArm/myarm_m/scripts/simple_gui.py
+++ b/myArm/myarm_m/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 myarm_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("myarm ros GUI")
+    Window(window).run()
+
+
+if __name__ == "__main__":
+    main()
--- a/myArm/myarm_m/scripts/slider_control.py
+++ b/myArm/myarm_m/scripts/slider_control.py
@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+
+"""[summary]
+This file obtains the joint angle of the manipulator in ROS,
+and then sends it directly to the real manipulator using `pymycobot` API.
+This file is [slider_control.launch] related script.
+Passable parameters:
+    port: serial prot string. Defaults is '/dev/ttyUSB0'
+    baud: serial prot baudrate. Defaults is 115200.
+"""
+import time
+import math
+import rospy
+from sensor_msgs.msg import JointState
+
+from pymycobot.myarm import MyArm
+
+
+mc = None
+
+
+def callback(data):
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+
+    data_list = []
+    for index, value in enumerate(data.position):
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
+
+    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
+    mc.send_angles(data_list, 25)
+
+
+
+def listener():
+    global mc
+    rospy.init_node("control_slider", anonymous=True)
+
+    rospy.Subscriber("joint_states", JointState, callback)
+    port = rospy.get_param("~port", "/dev/ttyAMA0")
+    baud = rospy.get_param("~baud", 115200)
+    print(port, baud)
+    mc = MyArm(port, baud)
+    time.sleep(0.05)
+    mc.set_free_mode(1)
+    time.sleep(0.05)
+    # spin() simply keeps python from exiting until this node is stopped
+    # spin()只是阻止python退出，直到该节点停止
+    print("spin ...")
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    listener()
--- a/myArm/myarm_m/scripts/teleop_keyboard.py
+++ b/myArm/myarm_m/scripts/teleop_keyboard.py
@ -0,0 +1,179 @@
+#!/usr/bin/env python
+from __future__ import print_function
+from myarm_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
+import rospy
+import sys
+import select
+import termios
+import tty
+import time
+
+import roslib
+
+# Terminal output prompt information. 终端输出提示信息
+msg = """\
+MyArm 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 = 0
+    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:
+        print("start service...")
+        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(*init_pose)
+
+    while True:
+        res = get_coords()
+        print('current coords: {}'.format(res))
+        if res.x > 1:
+            break
+        else:
+            print('res.X < 1 {}'.format(res))
+            print('Please adjust the joint position of the robot and run program again !!!')
+        time.sleep(0.1)
+
+    record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
+    print(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)
+                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
--- a/myArm/myarm_m/src/camera_display.cpp
+++ b/myArm/myarm_m/src/camera_display.cpp
@ -0,0 +1,29 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+void imageCallback(const sensor_msgs::ImageConstPtr &msg)
+{
+	try
+	{
+		cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
+		cv::waitKey(30);
+	}
+	catch (cv_bridge::Exception &e)
+	{
+		ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
+	}
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "image_listener");
+	ros::NodeHandle nh;
+	cv::namedWindow("view");
+	cv::startWindowThread();
+	image_transport::ImageTransport it(nh);
+	image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
+	ros::spin();
+	cv::destroyWindow("view");
+}
--- a/myArm/myarm_m/src/opencv_camera.cpp
+++ b/myArm/myarm_m/src/opencv_camera.cpp
@ -0,0 +1,60 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sstream> // for converting the command line parameter to integer,用于将命令行参数转换为整数
+
+int main(int argc, char **argv)
+{
+    // Check if video source has been passed as a parameter,检查视频源是否已作为参数传递
+    if (argv[1] == NULL)
+    {
+        ROS_INFO("argv[1]=NULL\n");
+        return 1;
+    }
+
+    ros::init(argc, argv, "image_publisher"); // Initialize node,初始化节点
+    ros::NodeHandle nh;
+    image_transport::ImageTransport it(nh);
+    image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic，发布话题
+
+    ros::Rate loop_rate(200); // refresh Hz.
+
+    // Convert the passed as command line parameter index for the video device to an integer，
+    // 将作为命令行参数传递的视频设备索引转换为整数
+    std::istringstream video_sourceCmd(argv[1]);
+    int video_source;
+    // Check if it is indeed a number，检查它是否确实是一个数字
+    if (!(video_sourceCmd >> video_source))
+    {
+        ROS_INFO("video_sourceCmd is %d\n", video_source);
+        return 1;
+    }
+
+    cv::VideoCapture cap(video_source);
+    // Check if video device can be opened with the given index，检查是否可以使用给定的索引打开视频设备
+    if (!cap.isOpened())
+    {
+        ROS_INFO("can not opencv video device\n");
+        return 1;
+    }
+    cv::Mat frame;
+    sensor_msgs::ImagePtr msg;
+
+    while (nh.ok())
+    {
+        cap >> frame;
+        // cv::imshow("veiwer", frame);
+        // Check if grabbed frame is actually full with some content，检查抓取的帧是否实际上充满了一些内容
+        if (!frame.empty())
+        {
+            msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
+            pub.publish(msg);
+            //cv::Wait(1);
+        }
+        ros::spinOnce();
+        loop_rate.sleep();
+        // if(cv::waitKey(2) >= 0)
+        // break;
+    }
+}
--- a/mycobot_description/urdf/myarm_c650/base.dae
+++ b/mycobot_description/urdf/myarm_c650/base.dae
--- a/mycobot_description/urdf/myarm_c650/endeffector.dae
+++ b/mycobot_description/urdf/myarm_c650/endeffector.dae
--- a/mycobot_description/urdf/myarm_c650/joint1.dae
+++ b/mycobot_description/urdf/myarm_c650/joint1.dae
--- a/mycobot_description/urdf/myarm_c650/joint2.dae
+++ b/mycobot_description/urdf/myarm_c650/joint2.dae
--- a/mycobot_description/urdf/myarm_c650/joint3.dae
+++ b/mycobot_description/urdf/myarm_c650/joint3.dae
--- a/mycobot_description/urdf/myarm_c650/joint4.dae
+++ b/mycobot_description/urdf/myarm_c650/joint4.dae
--- a/mycobot_description/urdf/myarm_c650/joint5.dae
+++ b/mycobot_description/urdf/myarm_c650/joint5.dae
--- a/mycobot_description/urdf/myarm_c650/myarm_basic_base.dae
+++ b/mycobot_description/urdf/myarm_c650/myarm_basic_base.dae
--- a/mycobot_description/urdf/myarm_c650/myarm_c650.urdf
+++ b/mycobot_description/urdf/myarm_c650/myarm_c650.urdf
@ -0,0 +1,262 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="base">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_basic_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_basic_base.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="link1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.0065 " rpy = " 3.14159 0 0"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.0065 " rpy = " 3.14159 0 0"/>
+    </collision>
+  </link>
+
+  <link name="link2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0.003 0.0 " rpy = " 3.14159 0 -1.5708 "/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0.003 0.0 " rpy = " 3.14159 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="link3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 -0.001 0.004 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 -0.001 0.004" rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link4">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.004 " rpy = " 0 3.14159 0"/>
+    </visual>
+    <collision>
+       <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.004 " rpy = " 0 3.14159 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link5">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0 0.0 0.001 " rpy = " 3.14159 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0 0.0 0.001 " rpy = " 3.14159 0 -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="gripper">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_Base.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.003 " rpy = " 3.14159 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_Base.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.003 " rpy = " 3.14159 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_R.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_R.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_L.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_L.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </collision>
+  </link>
+
+
+  
+
+
+
+  <!-- <link name="endeffector">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/endeffector.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.009 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/endeffector.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.009 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link> -->
+
+
+
+  <joint name="joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.7925" upper = "2.7925" velocity = "0"/>
+    <parent link="base"/>
+    <child link="link1"/>
+    <origin xyz= "0 0 0.070" rpy = "3.14159 0 0"/>  
+  </joint>
+
+
+  <joint name="joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.3962" upper = "1.3962" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <!-- <origin xyz= "0 0  0" rpy = "1.5708 3.14159 0"/>   -->
+    <origin xyz= "0 0 -0.080" rpy = "1.5708 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.3962" upper = "1.32" velocity = "0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz= "0.0565 -0.296 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+ 
+  <joint name="joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.7453" upper = "1.7453" velocity = "0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <origin xyz= "0.081 0 0.004" rpy = "0 -1.5708 0"/>  
+  </joint>
+
+
+  <joint name="joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.3962" upper = "1.3962" velocity = "0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz= "0.0 0.0003 -0.250" rpy = "-1.5708 3.14159 -1.5708"/>  
+  </joint>
+
+ 
+  <joint name="joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.9198" upper = "1.9198" velocity = "0"/>
+    <parent link="link5"/>
+    <child link="gripper"/>
+    <origin xyz= "0 -0.055 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+  <joint name="gripper" type="prismatic">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "0" upper = "0.022" velocity = "0"/>
+    <parent link="gripper"/>
+    <child link="gripper_left"/>
+    <origin xyz= "-0.030 0 -0.066" rpy = "3.14 -1.57 1.57"/>  
+  </joint>
+
+  <joint name="gripper_r" type="prismatic">
+    <axis xyz="0 0 -1"/>
+    <limit effort = "1000.0" lower = "0" upper = "0.022" velocity = "0"/>
+    <parent link="gripper"/>
+    <child link="gripper_right"/>
+    <origin xyz= "-0.030 0 -0.066" rpy = "3.14 -1.57 1.57"/>
+    <!-- 跟随左手指运动 -->
+    <mimic joint="gripper" multiplier="1.0" offset="0" />
+  </joint>
+
+  <!-- <joint name="endeffector_to_endeffector" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.8797" upper = "2.8797" velocity = "0"/>
+    <parent link="endeffector"/>
+    <child link="endeffector"/>
+    <origin xyz= "0 -0.056 0" rpy = "1.5708 0 0"/>  
+  </joint> -->
+
+
+
+</robot>
+
--- a/mycobot_description/urdf/myarm_c650/myarm_c650.urdf.xacro
+++ b/mycobot_description/urdf/myarm_c650/myarm_c650.urdf.xacro
@ -0,0 +1,235 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="ns" value="$(arg ns)/" />
+
+
+  <link name="base">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_basic_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_basic_base.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="link1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.0065 " rpy = " 3.14159 0 0"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.0065 " rpy = " 3.14159 0 0"/>
+    </collision>
+  </link>
+
+  <link name="link2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0.003 0.0 " rpy = " 3.14159 0 -1.5708 "/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0.003 0.0 " rpy = " 3.14159 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="link3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 -0.001 0.004 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 -0.001 0.004" rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link4">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.004 " rpy = " 0 3.14159 0"/>
+    </visual>
+    <collision>
+       <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.004 " rpy = " 0 3.14159 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link5">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0 0.009 0.001 " rpy = " 3.14159 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0 0.009 0.001 " rpy = " 3.14159 0 -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="gripper">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_Base.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.003 " rpy = " 3.14159 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_Base.dae"/>
+      </geometry>
+    <origin xyz = "0 0 0.003 " rpy = " 3.14159 0 0"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_R.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_R.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="gripper_right">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_L.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/myarm_c650/myarm_c_grip_L.dae"/>
+      </geometry>
+    <origin xyz = "0.010 0 0 " rpy = " 1.5708 3.14159 -1.5708"/>
+    </collision>
+  </link>
+
+
+
+
+  <joint name="joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.7925" upper = "2.7925" velocity = "0"/>
+    <parent link="base"/>
+    <child link="link1"/>
+    <origin xyz= "0 0 0.070" rpy = "3.14159 0 0"/>  
+  </joint>
+
+
+  <joint name="joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.3962" upper = "1.3962" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <!-- <origin xyz= "0 0  0" rpy = "1.5708 3.14159 0"/>   -->
+    <origin xyz= "0 0 -0.080" rpy = "1.5708 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.8797" upper = "2.8797" velocity = "0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz= "0.0565 -0.296 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+ 
+  <joint name="joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.7453" upper = "1.3962" velocity = "0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <origin xyz= "0.081 0 0.004" rpy = "0 -1.5708 0"/>  
+  </joint>
+
+
+  <joint name="joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.8797" upper = "2.8797" velocity = "0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz= "0.0 0.0003 -0.2595" rpy = "-1.5708 3.14159 -1.5708"/>  
+  </joint>
+
+ 
+  <joint name="joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.9198" upper = "1.9198" velocity = "0"/>
+    <parent link="link5"/>
+    <child link="gripper"/>
+    <origin xyz= "0 -0.045 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+  <joint name="gripper" type="prismatic">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "0" upper = "0.022" velocity = "0"/>
+    <parent link="gripper"/>
+    <child link="gripper_left"/>
+    <origin xyz= "-0.030 0 -0.066" rpy = "3.14 -1.57 1.57"/>  
+  </joint>
+
+  <joint name="gripper_r" type="prismatic">
+    <axis xyz="0 0 -1"/>
+    <limit effort = "1000.0" lower = "0" upper = "0.022" velocity = "0"/>
+    <parent link="gripper"/>
+    <child link="gripper_right"/>
+    <origin xyz= "-0.030 0 -0.066" rpy = "3.14 -1.57 1.57"/>
+    <!-- 跟随左手指运动 -->
+    <mimic joint="gripper" multiplier="1.0" offset="0" />
+  </joint>
+
+  
+
+
+
+</robot>
+
--- a/mycobot_description/urdf/myarm_c650/myarm_c_grip_Base.dae
+++ b/mycobot_description/urdf/myarm_c650/myarm_c_grip_Base.dae
--- a/mycobot_description/urdf/myarm_c650/myarm_c_grip_L.dae
+++ b/mycobot_description/urdf/myarm_c650/myarm_c_grip_L.dae
--- a/mycobot_description/urdf/myarm_c650/myarm_c_grip_R.dae
+++ b/mycobot_description/urdf/myarm_c650/myarm_c_grip_R.dae
--- a/mycobot_description/urdf/myarm_m/endeffector.dae
+++ b/mycobot_description/urdf/myarm_m/endeffector.dae
--- a/mycobot_description/urdf/myarm_m/finger_holder.dae
+++ b/mycobot_description/urdf/myarm_m/finger_holder.dae
--- a/mycobot_description/urdf/myarm_m/finger_left.dae
+++ b/mycobot_description/urdf/myarm_m/finger_left.dae
--- a/mycobot_description/urdf/myarm_m/finger_right.dae
+++ b/mycobot_description/urdf/myarm_m/finger_right.dae
--- a/mycobot_description/urdf/myarm_m/link1.dae
+++ b/mycobot_description/urdf/myarm_m/link1.dae
--- a/mycobot_description/urdf/myarm_m/link2.dae
+++ b/mycobot_description/urdf/myarm_m/link2.dae
--- a/mycobot_description/urdf/myarm_m/link3.dae
+++ b/mycobot_description/urdf/myarm_m/link3.dae
--- a/mycobot_description/urdf/myarm_m/link4.dae
+++ b/mycobot_description/urdf/myarm_m/link4.dae
--- a/mycobot_description/urdf/myarm_m/link5.dae
+++ b/mycobot_description/urdf/myarm_m/link5.dae
--- a/mycobot_description/urdf/myarm_m/myarm_m.urdf
+++ b/mycobot_description/urdf/myarm_m/myarm_m.urdf
@ -0,0 +1,218 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="myarm_m" >
+	
+<xacro:property name="width" value=".2" />
+<!-- 扭矩未设置 -->
+
+  <!-- base_footprint -->
+
+  <link name="base">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/myarm_m_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/myarm_m_base.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="link1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.1 " rpy = " 0 3.14 0"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.1 " rpy = "0 3.14 0"/>
+    </collision>
+  </link>
+
+  <link name="link2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = "0 3.14159 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = "0 3.14159 0"/>
+    </collision>
+  </link>
+
+
+  <link name="link3">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0" rpy = "0 3.14159 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.0" rpy = "0 3.14159 0"/>
+    </collision>
+  </link>
+  
+
+  <link name="link4">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 3.14159 -1.5708"/>
+    </visual>
+    <collision>
+       <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 3.14159  -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="link5">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link5.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0" rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/link5.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="gripper">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/finger_holder.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/finger_holder.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+    </collision>
+  </link>
+
+  <link name="gripper_left">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/finger_left.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/finger_left.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0" rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="gripper_right">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/finger_right.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/myarm_m/finger_right.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+    </collision>
+  </link>
+
+  <joint name="joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.0368" upper = "2.9147" velocity = "0"/>
+    <parent link="base"/>
+    <child link="link1"/>
+    <origin xyz= "0 0 0" rpy = "0 3.14 0"/>  
+  </joint>
+
+  <joint name="joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.6057" upper = "1.6232" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin xyz= "0 0 -0.175" rpy = "-1.57079 -1.57079 0"/>  
+  </joint>
+
+   <joint name="joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.5882" upper = "1.7977" velocity = "0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz= "-0.303 -0.057 0   " rpy = "0 3.14159 -1.57079"/>  
+  </joint>
+
+
+  <joint name="joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.9671" upper = "2.9671" velocity = "0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <origin xyz= "-0.08 0 0" rpy = "0 1.57079 0"/>  
+  </joint>
+
+ 
+  <joint name="joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-1.6755" upper = "1.5533" velocity = "0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz= "0 0 -0.258" rpy = "0 1.57079 0"/>  
+  </joint>
+
+  <joint name="joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.9671" upper = "2.9671" velocity = "0"/>
+    <parent link="link5"/>
+    <child link="gripper"/>
+    <origin xyz= "0.07 0 0" rpy = "0 -1.57079 0"/>  
+  </joint>
+
+
+  <joint name="gripper" type="prismatic">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "0" upper = "0.0345" velocity = "0"/>
+    <parent link="gripper"/>
+    <child link="gripper_left"/>
+    <origin xyz= "0.008 0 -0.036" rpy = "-1.57 0 -1.57"/>  
+  </joint>
+
+  <joint name="gripper_r" type="prismatic">
+    <axis xyz="0 0 -1"/>
+    <limit effort = "1000.0" lower = "0" upper = "0.06" velocity = "0"/>
+    <parent link="gripper"/>
+    <child link="gripper_right"/>
+    <origin xyz= "-0.008 0 -0.036" rpy = "-1.57 0 -1.57"/>
+    <!-- 跟随左手指运动 -->
+    <mimic joint="gripper" multiplier="1.0" offset="0" />
+  </joint>
+
+</robot>
--- a/mycobot_description/urdf/myarm_m/myarm_m.urdf.xacro
+++ b/mycobot_description/urdf/myarm_m/myarm_m.urdf.xacro
@ -0,0 +1,215 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="myarm_m" >
+	
+<xacro:property name="ns" value="$(arg ns)/" />
+  
+<link name="base">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/myarm_m_base.dae"/>
+    </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+  </visual>
+  <collision>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/myarm_m_base.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+  </collision>
+</link>
+
+<link name="link1">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link1.dae"/>
+    </geometry>
+    <origin xyz = "0.0 0 -0.1 " rpy = " 0 3.14 0"/>
+  </visual>
+  <collision>
+   <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link1.dae"/>
+    </geometry>
+    <origin xyz = "0.0 0 -0.1 " rpy = "0 3.14 0"/>
+  </collision>
+</link>
+
+<link name="link2">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link2.dae"/>
+    </geometry>
+  <origin xyz = "0.0 0 0 " rpy = "0 3.14159 0"/>
+  </visual>
+  <collision>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link2.dae"/>
+    </geometry>
+    <origin xyz = "0.0 0 0 " rpy = "0 3.14159 0"/>
+  </collision>
+</link>
+
+
+<link name="link3">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link3.dae"/>
+    </geometry>
+    <origin xyz = "0.0 0 0" rpy = "0 3.14159 0"/>
+  </visual>
+  <collision>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link3.dae"/>
+    </geometry>
+    <origin xyz = "0.0 0 0.0" rpy = "0 3.14159 0"/>
+  </collision>
+</link>
+
+
+<link name="link4">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link4.dae"/>
+    </geometry>
+  <origin xyz = "0.0 0 0 " rpy = " 0 3.14159 -1.5708"/>
+  </visual>
+  <collision>
+     <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link4.dae"/>
+    </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 3.14159  -1.5708"/>
+  </collision>
+</link>
+
+
+<link name="link5">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link5.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0" rpy = " 0 0 0"/>
+  </visual>
+  <collision>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/link5.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0 " rpy = " 0 0 0"/>
+  </collision>
+</link>
+
+
+<link name="gripper">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/finger_holder.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+  </visual>
+  <collision>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/finger_holder.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+  </collision>
+</link>
+
+<link name="gripper_left">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/finger_left.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+  </visual>
+  <collision>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/finger_left.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0" rpy = " 0 0 0"/>
+  </collision>
+</link>
+
+
+<link name="gripper_right">
+  <visual>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/finger_right.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+  </visual>
+  <collision>
+    <geometry>
+     <mesh filename="package://mycobot_description/urdf/myarm_m/finger_right.dae"/>
+    </geometry>
+  <origin xyz = "0 0.00 0" rpy = " 0 3.14159 -1.57079"/>
+  </collision>
+</link>
+
+<joint name="joint1" type="revolute">
+  <axis xyz="0 0 1"/>
+  <limit effort = "1000.0" lower = "-3.0368" upper = "2.9147" velocity = "0"/>
+  <parent link="base"/>
+  <child link="link1"/>
+  <origin xyz= "0 0 0" rpy = "0 3.14 0"/>  
+</joint>
+
+<joint name="joint2" type="revolute">
+  <axis xyz="0 0 1"/>
+  <limit effort = "1000.0" lower = "-1.6057" upper = "1.6232" velocity = "0"/>
+  <parent link="link1"/>
+  <child link="link2"/>
+  <origin xyz= "0 0 -0.175" rpy = "-1.57079 -1.57079 0"/>  
+</joint>
+
+ <joint name="joint3" type="revolute">
+  <axis xyz=" 0 0 1"/>
+  <limit effort = "1000.0" lower = "-1.5882" upper = "1.7977" velocity = "0"/>
+  <parent link="link2"/>
+  <child link="link3"/>
+  <origin xyz= "-0.303 -0.057 0   " rpy = "0 3.14159 -1.57079"/>  
+</joint>
+
+
+<joint name="joint4" type="revolute">
+  <axis xyz=" 0 0 1"/>
+  <limit effort = "1000.0" lower = "-2.9671" upper = "2.9671" velocity = "0"/>
+  <parent link="link3"/>
+  <child link="link4"/>
+  <origin xyz= "-0.08 0 0" rpy = "0 1.57079 0"/>  
+</joint>
+
+
+<joint name="joint5" type="revolute">
+  <axis xyz="0 0 1"/>
+  <limit effort = "1000.0" lower = "-1.6755" upper = "1.5533" velocity = "0"/>
+  <parent link="link4"/>
+  <child link="link5"/>
+  <origin xyz= "0 0 -0.258" rpy = "0 1.57079 0"/>  
+</joint>
+
+<joint name="joint6" type="revolute">
+  <axis xyz="0 0 1"/>
+  <limit effort = "1000.0" lower = "-2.9671" upper = "2.9671" velocity = "0"/>
+  <parent link="link5"/>
+  <child link="gripper"/>
+  <origin xyz= "0.07 0 0" rpy = "0 -1.57079 0"/>  
+</joint>
+
+
+<joint name="gripper" type="prismatic">
+  <axis xyz="0 0 1"/>
+  <limit effort = "1000.0" lower = "0" upper = "0.0345" velocity = "0"/>
+  <parent link="gripper"/>
+  <child link="gripper_left"/>
+  <origin xyz= "0.008 0 -0.036" rpy = "-1.57 0 -1.57"/>  
+</joint>
+
+<joint name="gripper_r" type="prismatic">
+  <axis xyz="0 0 -1"/>
+  <limit effort = "1000.0" lower = "0" upper = "0.06" velocity = "0"/>
+  <parent link="gripper"/>
+  <child link="gripper_right"/>
+  <origin xyz= "-0.008 0 -0.036" rpy = "-1.57 0 -1.57"/>
+  <!-- 跟随左手指运动 -->
+  <mimic joint="gripper" multiplier="1.0" offset="0" />
+</joint>
+
+</robot>
--- a/mycobot_description/urdf/myarm_m/myarm_m_base.dae
+++ b/mycobot_description/urdf/myarm_m/myarm_m_base.dae
--- a/test.py
+++ b/test.py
@ -1,69 +0,0 @@
-#!/usr/bin/env python
-
-"""
-This package need `pymycobot`.
-This file for test the API if right.
-Just can run in Linux.
-"""
-
-import time, random, subprocess
-from pymycobot.mycobot import MyCobot
-from pymycobot.genre import Angle, Coord
-
-if __name__ == "__main__":
-    sys_ = subprocess.check_output(["uname"], shell=True).decode()
-    if not sys_.startswith("Linux"):
-        print("This script just can run on Linux.")
-        exit(0)
-
-    port = subprocess.check_output(["echo -n /dev/ttyUSB*"], shell=True).decode()
-    mycobot = MyCobot(port)
-
-    print("Start check api\n")
-
-    print("::get_angles()")
-    print("==> degrees: {}\n".format(mycobot.get_angles()))
-    time.sleep(0.5)
-
-    print("::get_radians()")
-    print("==> radians: {}\n".format(mycobot.get_radians()))
-    time.sleep(0.5)
-
-    print("::send_angles()")
-    mycobot.send_angles([0, 0, 0, 0, 0, 0], 80)
-    print("==> set angles [0,0,0,0,0,0], speed 80\n")
-    print("Is moving: {}".format(mycobot.is_moving()))
-    time.sleep(3)
-
-    print("::send_radians")
-    mycobot.send_radians([1, 1, 1, 1, 1, 1], 70)
-    print("==> set raidans [1,1,1,1,1,1], speed 70\n")
-    time.sleep(1.5)
-
-    print("::send_angle()")
-    mycobot.send_angle(Angle.J2.value, 10, 50)
-    print("==> angle: joint2, degree: 10, speed: 50\n")
-    time.sleep(1)
-
-    print("::get_coords()")
-    print("==> coords {}\n".format(mycobot.get_coords()))
-    time.sleep(0.5)
-
-    print("::send_coords()")
-    coord_list = [160, 160, 160, 0, 0, 0]
-    mycobot.send_coords(coord_list, 70, 0)
-    print("==> send coords [160,160,160,0,0,0], speed 70, mode 0\n")
-    time.sleep(3.0)
-
-    print(mycobot.is_in_position(coord_list, 1))
-    time.sleep(1)
-
-    print("::send_coord()")
-    mycobot.send_coord(Coord.X.value, -40, 70)
-    print("==> send coord id: X, coord value: -40, speed: 70\n")
-    time.sleep(2)
-
-    print("::release_all_servos()")
-    mycobot.release_all_servos()
-    print("==> into free moving mode.")
-    print("=== check end <==\n")