Three models can run in rviz have been updated

mycobot_ai/launch/Readme.txt

@@ -0,0 +1,36 @@
+Tips：
+1、请使用与机械臂相同型号的文件名文件。
+
+2、请先执行以下操作:
+1）打开一个新的终端
+2）输入命令
+chmod +x /home/h/catkin_mycobot/src/mycobot_ros/mycobot_communication/scripts/xxx.py
+								(此处为各个新增文件的文件名）
+3、jetson nano的文件还没有使用机械臂进行过测试，可能存在问题。
+
+4、数莓派版本的使用:
+1）打开VScode,新建一个文件，复制以下内容（请确保电脑与数莓派机械臂已经连接）并运行
+
+from pymycobot import MyCobotSocket
+
+mc = MyCobotSocket("192.168.10.10","9000")
+mc.connect()
+
+如果缺少包或版本还未更新请自己安装更新最新版本
+
+2）
+打开“网络与internet”设置
+更改适配器选项
+右键打开数莓派的以太网属性
+打开 “internet协议版本4” 的属性
+选择 “使用下面的IP地址”
+IP地址为 ：  192.168.10.100 （最后一位非10都可）
+子网掩码为： 255.255.255.0
+确认
+
+
+
+
+
+
+

mycobot_ai/launch/jsnn.launch

@@ -0,0 +1,27 @@
+<launch>
+	<arg name="port" default="/dev/ttyTHS1" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/jetsonNano/mycobot_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<!-- <arg name="gui" default="false" /> -->
+
+	<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="state_publisher" >
+		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+	</node>
+
+	<!-- mycobot-topics -->
+    <include file="$(find mycobot_communication)/launch/communication_jsnn.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" />
+
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_ai/launch/pi.launch

@@ -0,0 +1,28 @@
+<launch>
+	<arg name="port" default="192.168.10.10" />
+	<arg name="baud" default="9000" />
+
+    <!-- urdf文件，模型文件的路径 -->
+	<arg name="model" default="$(find mycobot_description)/urdf/jetsonNano/mycobot_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<!-- <arg name="gui" default="false" /> -->
+
+	<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="state_publisher" >
+		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+	</node>
+
+	<!-- mycobot-topics -->
+    <include file="$(find mycobot_communication)/launch/communication_topic_pi.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" />
+
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_ai/launch/seeed.launch

@@ -0,0 +1,27 @@
+<launch>
+<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/seeed/mycobot_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<!-- <arg name="gui" default="false" /> -->
+
+	<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="state_publisher" >
+		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+	</node>
+
+	<!-- mycobot-topics -->
+    <include file="$(find mycobot_communication)/launch/communication_seeed.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" />
+
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_communication/launch/communication_jsnn.launch

@@ -0,0 +1,10 @@
+<launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<!-- Open communication service -->
+	<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_jsnn.py" output="screen">
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+    </node>
+</launch>

mycobot_communication/launch/communication_seeed.launch

@@ -0,0 +1,10 @@
+<launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<!-- Open communication service -->
+	<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_seeed.py" output="screen">
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+    </node>
+</launch>

mycobot_communication/launch/communication_topic_pi.launch

@@ -0,0 +1,10 @@
+<launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<!-- Open communication service -->
+	<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_pi.py" output="screen">
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+    	</node>
+</launch>

mycobot_communication/scripts/mycobot_topics_jsnn.py

@@ -0,0 +1,212 @@
+#!/usr/bin/env python2
+import time
+import os
+import sys
+import signal
+import threading
+
+import rospy
+
+from mycobot_communication.msg import (
+    MycobotAngles,
+    MycobotCoords,
+    MycobotSetAngles,
+    MycobotSetCoords,
+    MycobotGripperStatus,
+    MycobotPumpStatus,
+)
+
+
+# from pymycobot import MyCobot
+
+from pymycobot import MyCobotSocket  # pi
+
+
+class Watcher:
+    """this class solves two problems with multithreaded
+    programs in Python, (1) a signal might be delivered
+    to any thread (which is just a malfeature) and (2) if
+    the thread that gets the signal is waiting, the signal
+    is ignored (which is a bug).
+
+    The watcher is a concurrent process (not thread) that
+    waits for a signal and the process that contains the
+    threads.  See Appendix A of The Little Book of Semaphores.
+    http://greenteapress.com/semaphores/
+
+    I have only tested this on Linux.  I would expect it to
+    work on the Macintosh and not work on Windows.
+    """
+
+    def __init__(self):
+        """Creates a child thread, which returns.  The parent
+        thread waits for a KeyboardInterrupt and then kills
+        the child thread.
+        """
+        self.child = os.fork()
+        if self.child == 0:
+            return
+        else:
+            self.watch()
+
+    def watch(self):
+        try:
+            os.wait()
+        except KeyboardInterrupt:
+            # I put the capital B in KeyBoardInterrupt so I can
+            # tell when the Watcher gets the SIGINT
+            print("KeyBoardInterrupt")
+            self.kill()
+        sys.exit()
+
+    def kill(self):
+        try:
+            os.kill(self.child, signal.SIGKILL)
+        except OSError:
+            pass
+
+
+class MycobotTopics(object):
+    def __init__(self):
+        super(MycobotTopics, self).__init__()
+
+        rospy.init_node("mycobot_topics")
+        rospy.loginfo("start ...")
+        # problem
+        port = rospy.get_param("~port", "/dev/ttyUSB0")
+        baud = rospy.get_param("~baud", 115200)
+        rospy.loginfo("%s,%s" % (port, baud))
+        # self.mc = MyCobot(port,baud)
+        self.mc = MyCobotSocket(port, baud) # port
+        self.mc.connect()   
+
+
+        self.lock = threading.Lock()
+
+    def start(self):
+        pa = threading.Thread(target=self.pub_real_angles)
+        pb = threading.Thread(target=self.pub_real_coords)
+        sa = threading.Thread(target=self.sub_set_angles)
+        sb = threading.Thread(target=self.sub_set_coords)
+        sg = threading.Thread(target=self.sub_gripper_status)
+        sp = threading.Thread(target=self.sub_pump_status)
+
+        pa.setDaemon(True)
+        pa.start()
+        pb.setDaemon(True)
+        pb.start()
+        sa.setDaemon(True)
+        sa.start()
+        sb.setDaemon(True)
+        sb.start()
+        sg.setDaemon(True)
+        sg.start()
+        sp.setDaemon(True)
+        sp.start()
+
+        pa.join()
+        pb.join()
+        sa.join()
+        sb.join()
+        sg.join()
+        sp.join()
+
+    def pub_real_angles(self):
+        pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
+        ma = MycobotAngles()
+        while not rospy.is_shutdown():
+            self.lock.acquire()
+            angles = self.mc.get_angles()
+            self.lock.release()
+            if angles:
+                ma.joint_1 = angles[0]
+                ma.joint_2 = angles[1]
+                ma.joint_3 = angles[2]
+                ma.joint_4 = angles[3]
+                ma.joint_5 = angles[4]
+                ma.joint_6 = angles[5]
+                pub.publish(ma)
+            time.sleep(0.25)
+
+    def pub_real_coords(self):
+        pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
+        ma = MycobotCoords()
+
+        while not rospy.is_shutdown():
+            self.lock.acquire()
+            coords = self.mc.get_coords()
+            self.lock.release()
+            if coords:
+                ma.x = coords[0]
+                ma.y = coords[1]
+                ma.z = coords[2]
+                ma.rx = coords[3]
+                ma.ry = coords[4]
+                ma.rz = coords[5]
+                pub.publish(ma)
+            time.sleep(0.25)
+
+    def sub_set_angles(self):
+        def callback(data):
+            angles = [
+                data.joint_1,
+                data.joint_2,
+                data.joint_3,
+                data.joint_4,
+                data.joint_5,
+                data.joint_6,
+            ]
+            sp = int(data.speed)
+            self.mc.send_angles(angles, sp)
+
+        sub = rospy.Subscriber(
+            "mycobot/angles_goal", MycobotSetAngles, callback=callback
+        )
+        rospy.spin()
+
+    def sub_set_coords(self):
+        def callback(data):
+            angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
+            sp = int(data.speed)
+            model = int(data.model)
+            self.mc.send_coords(angles, sp, model)
+
+        sub = rospy.Subscriber(
+            "mycobot/coords_goal", MycobotSetCoords, callback=callback
+        )
+        rospy.spin()
+
+    def sub_gripper_status(self):
+        def callback(data):
+            if data.Status:
+                self.mc.set_gripper_state(0, 80)
+            else:
+                self.mc.set_gripper_state(1, 80)
+
+        sub = rospy.Subscriber(
+            "mycobot/gripper_status", MycobotGripperStatus, callback=callback
+        )
+        rospy.spin()
+
+    def sub_pump_status(self):
+        def callback(data):
+            if data.Status:
+                self.mc.set_basic_output(data.Pin1, 0)
+                self.mc.set_basic_output(data.Pin2, 0)
+            else:
+                self.mc.set_basic_output(data.Pin1, 1)
+                self.mc.set_basic_output(data.Pin2, 1)
+
+        sub = rospy.Subscriber(
+            "mycobot/pump_status", MycobotPumpStatus, callback=callback
+        )
+        rospy.spin()
+
+if __name__ == "__main__":
+    Watcher()
+    mc_topics = MycobotTopics()
+    mc_topics.start()
+    # while True:
+    #     mc_topics.pub_real_coords()
+    # mc_topics.sub_set_angles()
+    pass

mycobot_communication/scripts/mycobot_topics_pi.py

@@ -0,0 +1,208 @@
+#!/usr/bin/env python2
+import time
+import os
+import sys
+import signal
+import threading
+
+import rospy
+
+from mycobot_communication.msg import (
+    MycobotAngles,
+    MycobotCoords,
+    MycobotSetAngles,
+    MycobotSetCoords,
+    MycobotGripperStatus,
+    MycobotPumpStatus,
+)
+
+
+
+from pymycobot import MyCobotSocket
+
+
+class Watcher:
+    """this class solves two problems with multithreaded
+    programs in Python, (1) a signal might be delivered
+    to any thread (which is just a malfeature) and (2) if
+    the thread that gets the signal is waiting, the signal
+    is ignored (which is a bug).
+
+    The watcher is a concurrent process (not thread) that
+    waits for a signal and the process that contains the
+    threads.  See Appendix A of The Little Book of Semaphores.
+    http://greenteapress.com/semaphores/
+
+    I have only tested this on Linux.  I would expect it to
+    work on the Macintosh and not work on Windows.
+    """
+
+    def __init__(self):
+        """Creates a child thread, which returns.  The parent
+        thread waits for a KeyboardInterrupt and then kills
+        the child thread.
+        """
+        self.child = os.fork()
+        if self.child == 0:
+            return
+        else:
+            self.watch()
+
+    def watch(self):
+        try:
+            os.wait()
+        except KeyboardInterrupt:
+            # I put the capital B in KeyBoardInterrupt so I can
+            # tell when the Watcher gets the SIGINT
+            print("KeyBoardInterrupt")
+            self.kill()
+        sys.exit()
+
+    def kill(self):
+        try:
+            os.kill(self.child, signal.SIGKILL)
+        except OSError:
+            pass
+
+
+class MycobotTopics(object):
+    def __init__(self):
+        super(MycobotTopics, self).__init__()
+
+        rospy.init_node("mycobot_topics_pi")
+        rospy.loginfo("start ...")
+        # problem
+        port = rospy.get_param("~port", "/dev/ttyUSB0")
+        baud = rospy.get_param("~baud", 115200)
+        rospy.loginfo("%s,%s" % (port, baud))
+        self.mc = MyCobotSocket(port, baud) # port
+        self.mc.connect()   #pi
+        self.lock = threading.Lock()
+
+    def start(self):
+        pa = threading.Thread(target=self.pub_real_angles)
+        pb = threading.Thread(target=self.pub_real_coords)
+        sa = threading.Thread(target=self.sub_set_angles)
+        sb = threading.Thread(target=self.sub_set_coords)
+        sg = threading.Thread(target=self.sub_gripper_status)
+        sp = threading.Thread(target=self.sub_pump_status)
+
+        pa.setDaemon(True)
+        pa.start()
+        pb.setDaemon(True)
+        pb.start()
+        sa.setDaemon(True)
+        sa.start()
+        sb.setDaemon(True)
+        sb.start()
+        sg.setDaemon(True)
+        sg.start()
+        sp.setDaemon(True)
+        sp.start()
+
+        pa.join()
+        pb.join()
+        sa.join()
+        sb.join()
+        sg.join()
+        sp.join()
+
+    def pub_real_angles(self):
+        pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
+        ma = MycobotAngles()
+        while not rospy.is_shutdown():
+            self.lock.acquire()
+            angles = self.mc.get_angles()
+            self.lock.release()
+            if angles:
+                ma.joint_1 = angles[0]
+                ma.joint_2 = angles[1]
+                ma.joint_3 = angles[2]
+                ma.joint_4 = angles[3]
+                ma.joint_5 = angles[4]
+                ma.joint_6 = angles[5]
+                pub.publish(ma)
+            time.sleep(0.25)
+
+    def pub_real_coords(self):
+        pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
+        ma = MycobotCoords()
+
+        while not rospy.is_shutdown():
+            self.lock.acquire()
+            coords = self.mc.get_coords()
+            self.lock.release()
+            if coords:
+                ma.x = coords[0]
+                ma.y = coords[1]
+                ma.z = coords[2]
+                ma.rx = coords[3]
+                ma.ry = coords[4]
+                ma.rz = coords[5]
+                pub.publish(ma)
+            time.sleep(0.25)
+
+    def sub_set_angles(self):
+        def callback(data):
+            angles = [
+                data.joint_1,
+                data.joint_2,
+                data.joint_3,
+                data.joint_4,
+                data.joint_5,
+                data.joint_6,
+            ]
+            sp = int(data.speed)
+            self.mc.send_angles(angles, sp)
+
+        sub = rospy.Subscriber(
+            "mycobot/angles_goal", MycobotSetAngles, callback=callback
+        )
+        rospy.spin()
+
+    def sub_set_coords(self):
+        def callback(data):
+            angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
+            sp = int(data.speed)
+            model = int(data.model)
+            self.mc.send_coords(angles, sp, model)
+
+        sub = rospy.Subscriber(
+            "mycobot/coords_goal", MycobotSetCoords, callback=callback
+        )
+        rospy.spin()
+
+    def sub_gripper_status(self):
+        def callback(data):
+            if data.Status:
+                self.mc.set_gripper_state(0, 80)
+            else:
+                self.mc.set_gripper_state(1, 80)
+
+        sub = rospy.Subscriber(
+            "mycobot/gripper_status", MycobotGripperStatus, callback=callback
+        )
+        rospy.spin()
+
+    def sub_pump_status(self):
+        def callback(data):
+            if data.Status:
+                self.mc.set_basic_output(data.Pin1, 0)
+                self.mc.set_basic_output(data.Pin2, 0)
+            else:
+                self.mc.set_basic_output(data.Pin1, 1)
+                self.mc.set_basic_output(data.Pin2, 1)
+
+        sub = rospy.Subscriber(
+            "mycobot/pump_status", MycobotPumpStatus, callback=callback
+        )
+        rospy.spin()
+
+if __name__ == "__main__":
+    Watcher()
+    mc_topics = MycobotTopics()
+    mc_topics.start()
+    # while True:
+    #     mc_topics.pub_real_coords()
+    # mc_topics.sub_set_angles()
+    pass

mycobot_communication/scripts/mycobot_topics_seeed.py

@@ -0,0 +1,206 @@
+#!/usr/bin/env python2
+import time
+import os
+import sys
+import signal
+import threading
+
+import rospy
+
+from mycobot_communication.msg import (
+    MycobotAngles,
+    MycobotCoords,
+    MycobotSetAngles,
+    MycobotSetCoords,
+    MycobotGripperStatus,
+    MycobotPumpStatus,
+)
+
+
+from pymycobot import MyCobot
+
+
+class Watcher:
+    """this class solves two problems with multithreaded
+    programs in Python, (1) a signal might be delivered
+    to any thread (which is just a malfeature) and (2) if
+    the thread that gets the signal is waiting, the signal
+    is ignored (which is a bug).
+
+    The watcher is a concurrent process (not thread) that
+    waits for a signal and the process that contains the
+    threads.  See Appendix A of The Little Book of Semaphores.
+    http://greenteapress.com/semaphores/
+
+    I have only tested this on Linux.  I would expect it to
+    work on the Macintosh and not work on Windows.
+    """
+
+    def __init__(self):
+        """Creates a child thread, which returns.  The parent
+        thread waits for a KeyboardInterrupt and then kills
+        the child thread.
+        """
+        self.child = os.fork()
+        if self.child == 0:
+            return
+        else:
+            self.watch()
+
+    def watch(self):
+        try:
+            os.wait()
+        except KeyboardInterrupt:
+            # I put the capital B in KeyBoardInterrupt so I can
+            # tell when the Watcher gets the SIGINT
+            print("KeyBoardInterrupt")
+            self.kill()
+        sys.exit()
+
+    def kill(self):
+        try:
+            os.kill(self.child, signal.SIGKILL)
+        except OSError:
+            pass
+
+
+class MycobotTopics(object):
+    def __init__(self):
+        super(MycobotTopics, self).__init__()
+
+        rospy.init_node("mycobot_topics")
+        rospy.loginfo("start ...")
+        # problem
+        port = rospy.get_param("~port", "/dev/ttyUSB0")
+        baud = rospy.get_param("~baud", 115200)
+        rospy.loginfo("%s,%s" % (port, baud))
+        self.mc = MyCobot(port,baud)
+        self.lock = threading.Lock()
+
+    def start(self):
+        pa = threading.Thread(target=self.pub_real_angles)
+        pb = threading.Thread(target=self.pub_real_coords)
+        sa = threading.Thread(target=self.sub_set_angles)
+        sb = threading.Thread(target=self.sub_set_coords)
+        sg = threading.Thread(target=self.sub_gripper_status)
+        sp = threading.Thread(target=self.sub_pump_status)
+
+        pa.setDaemon(True)
+        pa.start()
+        pb.setDaemon(True)
+        pb.start()
+        sa.setDaemon(True)
+        sa.start()
+        sb.setDaemon(True)
+        sb.start()
+        sg.setDaemon(True)
+        sg.start()
+        sp.setDaemon(True)
+        sp.start()
+
+        pa.join()
+        pb.join()
+        sa.join()
+        sb.join()
+        sg.join()
+        sp.join()
+
+    def pub_real_angles(self):
+        pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
+        ma = MycobotAngles()
+        while not rospy.is_shutdown():
+            self.lock.acquire()
+            angles = self.mc.get_angles()
+            self.lock.release()
+            if angles:
+                ma.joint_1 = angles[0]
+                ma.joint_2 = angles[1]
+                ma.joint_3 = angles[2]
+                ma.joint_4 = angles[3]
+                ma.joint_5 = angles[4]
+                ma.joint_6 = angles[5]
+                pub.publish(ma)
+            time.sleep(0.25)
+
+    def pub_real_coords(self):
+        pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
+        ma = MycobotCoords()
+
+        while not rospy.is_shutdown():
+            self.lock.acquire()
+            coords = self.mc.get_coords()
+            self.lock.release()
+            if coords:
+                ma.x = coords[0]
+                ma.y = coords[1]
+                ma.z = coords[2]
+                ma.rx = coords[3]
+                ma.ry = coords[4]
+                ma.rz = coords[5]
+                pub.publish(ma)
+            time.sleep(0.25)
+
+    def sub_set_angles(self):
+        def callback(data):
+            angles = [
+                data.joint_1,
+                data.joint_2,
+                data.joint_3,
+                data.joint_4,
+                data.joint_5,
+                data.joint_6,
+            ]
+            sp = int(data.speed)
+            self.mc.send_angles(angles, sp)
+
+        sub = rospy.Subscriber(
+            "mycobot/angles_goal", MycobotSetAngles, callback=callback
+        )
+        rospy.spin()
+
+    def sub_set_coords(self):
+        def callback(data):
+            angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
+            sp = int(data.speed)
+            model = int(data.model)
+            self.mc.send_coords(angles, sp, model)
+
+        sub = rospy.Subscriber(
+            "mycobot/coords_goal", MycobotSetCoords, callback=callback
+        )
+        rospy.spin()
+
+    def sub_gripper_status(self):
+        def callback(data):
+            if data.Status:
+                self.mc.set_gripper_state(0, 80)
+            else:
+                self.mc.set_gripper_state(1, 80)
+
+        sub = rospy.Subscriber(
+            "mycobot/gripper_status", MycobotGripperStatus, callback=callback
+        )
+        rospy.spin()
+
+    def sub_pump_status(self):
+        def callback(data):
+            if data.Status:
+                self.mc.set_basic_output(data.Pin1, 0)
+                self.mc.set_basic_output(data.Pin2, 0)
+            else:
+                self.mc.set_basic_output(data.Pin1, 1)
+                self.mc.set_basic_output(data.Pin2, 1)
+
+        sub = rospy.Subscriber(
+            "mycobot/pump_status", MycobotPumpStatus, callback=callback
+        )
+        rospy.spin()
+
+if __name__ == "__main__":
+    Watcher()
+    mc_topics = MycobotTopics()
+    mc_topics.start()
+    # while True:
+    #     mc_topics.pub_real_coords()
+    # mc_topics.sub_set_angles()
+    pass

mycobot_description/urdf/jetsonNano/mycobot_urdf.urdf

@@ -0,0 +1,142 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/jetsonNano/joint1_jet.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/jetsonNano/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/jetsonNano/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/jetsonNano/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+  </link>
+
+
+
+
+<link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/jetsonNano/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </visual>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/jetsonNano/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/jetsonNano/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+  </link>
+
+
+
+
+
+    <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.15756" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0 -0.001" rpy = "0 1.5708 -1.5708"/>  
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+
+
+</robot>
+

mycobot_description/urdf/jetsonNano/nano.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+	<arg name="firefighter" />
+	<arg name="gui" default="False" />
+	<param name="robot_description" textfile="$(arg model)" />
+	<param name="use_gui" value="$(arg gui)"/>
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
+</launch>

mycobot_description/urdf/mycobot/noenv.urdf

@@ -0,0 +1,249 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="mycobot_ai_no" >
+
+  <xacro:property name="width" value=".2" />
+
+
+  <link name="joint1">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="10"/>
+      <inertia
+        ixx="0.0" ixy="0.0" ixz="0.0"
+        iyy="0.5" iyz="0.0"
+        izz="0.0"/>
+  </inertial>
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0.0 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="0.0" ixy="0.0" ixz="0.0"
+        iyy="0.5" iyz="0.0"
+        izz="0.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.061 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.061 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint3">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="0.0" ixy="0.0" ixz="0.0"
+        iyy="0.5" iyz="0.0"
+        izz="0.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.00 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+<link name="joint4">
+  <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="0.0" ixy="0.0" ixz="0.0"
+        iyy="0.5" iyz="0.0"
+        izz="0.0"/>
+  </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "-0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint5">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="0.0" ixy="0.0" ixz="0.0"
+        iyy="0.5" iyz="0.0"
+        izz="0.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.stl"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.02956 " rpy = " 0 -1.5708 1.5708"/>
+    </visual>
+    <collision>
+       <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.stl"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.02956 " rpy = " 0 -1.5708 1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="0.0" ixy="0.0" ixz="0.0"
+        iyy="0.5" iyz="0.0"
+        izz="0.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "-0.00 0.0 -0.044 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "-0.00 0.0 -0.044 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint6_flange">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia
+        ixx="0.0" ixy="0.0" ixz="0.0"
+        iyy="0.0" iyz="0.0"
+        izz="0.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "-0.00 0.00 -0.014 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "-0.00 0.00 -0.014 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.13156" rpy = "0 0 0"/>
+  </joint>
+
+    <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0 -0.001" rpy = "0 1.5708 -1.5708"/>
+  </joint>
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>
+  </joint>
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06062" rpy = "0 0 -1.5708"/>
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0486 -0.007" rpy = "-1.5708 0 0"/>
+  </joint>
+
+
+
+
+
+
+
+
+<link name="world"/>
+
+<joint name="fixed" type="fixed">
+  <parent link="world"/>
+  <child link="joint1"/>
+</joint>
+
+
+
+
+
+
+</robot>
+

mycobot_description/urdf/mycobot/withenv.urdf

@@ -0,0 +1,263 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="mycobot_ai_with" >
+
+  <xacro:property name="width" value=".2" />
+
+  <link name="env">
+  <inertial>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="10"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+  </inertial>
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/suit_env.dae"/>
+      </geometry>
+      <origin xyz = "0 0 0.0" rpy = "1.5708 0 -1.5708"/>
+    </visual>
+  </link>
+
+  <link name="joint1">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+  </inertial>
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.02 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint1.dae"/>
+        </geometry>
+        <origin xyz = "0.0 0 0.02 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+     <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+  <link name="joint3">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+    <origin xyz = "-0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint3.dae"/>
+      </geometry>
+      <origin xyz = "-0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+<link name="joint4">
+  <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+  </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "-0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint4.dae"/>
+      </geometry>
+      <origin xyz = "-0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint5">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.stl"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.57080 "/>
+    </visual>
+    <collision>
+       <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint5.stl"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.57080"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.2"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0.00 0.00 -0.048 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0.00 0.00 -0.048 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint6_flange">
+    <inertial>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "-0.00 -0.00 -0.026 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot/joint7.dae"/>
+      </geometry>
+    <origin xyz = "-0.00 -0.00 -0.026 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.15156" rpy = "0 0 0"/>
+  </joint>
+
+    <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0 0" rpy = "0 1.5708 -1.5708"/>
+  </joint>
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0.02   " rpy = "0 0 0"/>
+  </joint>
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0.00 -0.07718 -0.004" rpy = "1.5708 -1.5708 0"/>
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0.0 0.06 -0.01" rpy = "-1.5708 0 0"/>
+  </joint>
+
+  <joint name="vision_joint" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="env"/>
+    <child link="joint1"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>
+  </joint>
+
+
+
+<link name="world"/>
+<joint name="fixed" type="fixed">
+  <parent link="world"/>
+  <child link="env"/>
+</joint>
+
+</robot>
+

mycobot_description/urdf/mycobot_pi/mycobot_urdf.urdf

@@ -0,0 +1,189 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+      <collision>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+      <collision>
+        <geometry>
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+
+
+<link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+      <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+
+
+
+    <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.13956" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0 -0.001" rpy = "0 1.5708 -1.5708"/>  
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+
+
+</robot>
+

mycobot_description/urdf/mycobot_pi/pi.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+
+
+	<!-- urdf文件，模型文件的路径-->
+	<arg name="firefighter" default="$(find mycobot_description)/urdf/mycobot_pi/mycobot_urdf.urdf"/>
+	
+	
+	
+	<arg name="gui" default="False" />
+	<param name="robot_description" textfile="$(arg model)" />
+	<param name="use_gui" value="$(arg gui)"/>
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+	
+	
+	
+	
+	<!-- Show in Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
+</launch>

mycobot_description/urdf/seeed/mycobot_urdf.urdf

@@ -0,0 +1,142 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/seeed/joint1_seeed.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/seeed/joint2.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+       
+       <mesh filename="package://mycobot_description/urdf/seeed/joint3.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/seeed/joint4.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+  </link>
+
+
+
+
+<link name="joint5">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/seeed/joint5.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.03356 " rpy = " 0 -1.5708 1.5708"/>
+    </visual>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/seeed/joint6.dae"/>
+      </geometry>
+    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/seeed/joint7.dae"/>
+      </geometry>
+    <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+  </link>
+
+
+
+
+
+    <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.15756" rpy = "0 0 0"/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0 -0.001" rpy = "0 1.5708 -1.5708"/>  
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>  
+  </joint>
+
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>  
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>  
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>  
+  </joint>
+
+
+
+</robot>
+

mycobot_description/urdf/seeed/seeed.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+	<arg name="firefighter" />
+	<arg name="gui" default="False" />
+	<param name="robot_description" textfile="$(arg model)" />
+	<param name="use_gui" value="$(arg gui)"/>
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
+</launch>