Merge branch 'noetic' of github.com:elephantrobotics/mycobot_ros into new_mycobot_ai

mecharm/mecharm/scripts/simple_gui.py

@@ -33,7 +33,7 @@ class Window:
         # 计算 Tk 根窗口的 x 和 y 坐标
         x = (self.ws / 2) - 190
         y = (self.hs / 2) - 250
-        self.win.geometry("430x400+{}+{}".format(x, y))
+        self.win.geometry("430x400+{}+{}".format(int(x), int(y)))
         # layout,布局
         self.set_layout()
         # input section,输入部分

mecharm/mecharm/scripts/teleop_keyboard.py

@@ -84,7 +84,7 @@ def teleop_keyboard():
 
 
     init_pose = [0, 0, 0, 0, 0, 0, speed]
-    home_pose = [0, 8, -127, 40, 0, 0, speed]
+    home_pose = [0, 30, 30, 0, 30, 0, speed]
 
     # rsp = set_angles(*init_pose)
 

mecharm/mecharm_communication/scripts/mecharm_services.py

@@ -1,17 +1,55 @@
-#!/usr/bin/env python2
- # -*- coding: UTF-8 -*-  
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
 import time
 import rospy
+import os
+import fcntl
 from mycobot_communication.srv import *
 
 from pymycobot.mycobot import MyCobot
 
 mc = None
 
+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+    open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+    fd = os.open(lock_file, open_mode)
+
+    pid = os.getpid()
+    lock_file_fd = None
+    
+    timeout = 50.0
+    start_time = current_time = time.time()
+    while current_time < start_time + timeout:
+        try:
+            # The LOCK_EX means that only one process can hold the lock
+            # The LOCK_NB means that the fcntl.flock() is not blocking
+            # and we are able to implement termination of while loop,
+            # when timeout is reached.
+            fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+        except (IOError, OSError):
+            pass
+        else:
+            lock_file_fd = fd
+            break
+        # print('pid waiting for lock:%d'% pid)
+        time.sleep(1.0)
+        current_time = time.time()
+    if lock_file_fd is None:
+        os.close(fd)
+    return lock_file_fd
+
+
+def release(lock_file_fd):
+    # Do not remove the lockfile:
+    fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+    os.close(lock_file_fd)
+    return None
+
 
 def create_handle():
     global mc
-    rospy.init_node("mecharm_services")
+    rospy.init_node("mycobot_services")
     rospy.loginfo("start ...")
     port = rospy.get_param("~port")
     baud = rospy.get_param("~baud")
@@ -43,7 +81,9 @@ def set_angles(req):
     sp = req.speed
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         mc.send_angles(angles, sp)
+        release(lock)
 
     return SetAnglesResponse(True)
 
@@ -51,7 +91,9 @@ def set_angles(req):
 def get_angles(req):
     """get angles,获取角度"""
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         angles = mc.get_angles()
+        release(lock)
         return GetAnglesResponse(*angles)
 
 
@@ -68,14 +110,18 @@ def set_coords(req):
     mod = req.model
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         mc.send_coords(coords, sp, mod)
+        release(lock)
 
     return SetCoordsResponse(True)
 
 
 def get_coords(req):
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         coords = mc.get_coords()
+        release(lock)
         return GetCoordsResponse(*coords)
 
 
@@ -83,23 +129,26 @@ def switch_status(req):
     """Gripper switch status"""
     """夹爪开关状态"""
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         if req.Status:
             mc.set_gripper_state(0, 80)
         else:
             mc.set_gripper_state(1, 80)
+        release(lock)
 
     return GripperStatusResponse(True)
 
 
 def toggle_pump(req):
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         if req.Status:
             mc.set_basic_output(req.Pin1, 0)
             mc.set_basic_output(req.Pin2, 0)
         else:
             mc.set_basic_output(req.Pin1, 1)
             mc.set_basic_output(req.Pin2, 1)
-
+        release(lock)
     return PumpStatusResponse(True)
 
 
@@ -107,12 +156,12 @@ robot_msg = """
 MyCobot Status
 --------------------------------
 Joint Limit:
-    joint 1: -160 ~ +160
-    joint 2: -90 ~ +90
+    joint 1: -160 ~ +170
+    joint 2: -85 ~ +90
     joint 3: -180 ~ +45
     joint 4: -160 ~ +160
     joint 5: -100 ~ +100
-    joint 6: -180 ~ +180
+    joint 6: -∞ ~ +∞
 
 Connect Status: %s
 
@@ -131,11 +180,15 @@ def output_robot_message():
     atom_version = "unknown"
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         cn = mc.is_controller_connected()
+        release(lock)
         if cn == 1:
             connect_status = True
         time.sleep(0.1)
+        lock = acquire("/tmp/mycobot_lock")
         si = mc.is_all_servo_enable()
+        release(lock)
         if si == 1:
             servo_infomation = "all connected"
 

mecharm/mecharm_pi/scripts/simple_gui.py

@@ -1,6 +1,7 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
-import Tkinter as tk
+# import Tkinter as tk # python2
+import tkinter as tk
 from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
 import rospy
 import time
@@ -33,7 +34,7 @@ class Window:
         # 计算 Tk 根窗口的 x 和 y 坐标
         x = (self.ws / 2) - 190
         y = (self.hs / 2) - 250
-        self.win.geometry("430x400+{}+{}".format(x, y))
+        self.win.geometry("430x400+{}+{}".format(int(x), int(y)))
         # layout,布局
         self.set_layout()
         # input section,输入部分

mecharm/mecharm_pi/scripts/teleop_keyboard.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding:utf-8 -*-
 from __future__ import print_function
 from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
@@ -84,7 +84,7 @@ def teleop_keyboard():
 
 
     init_pose = [0, 0, 0, 0, 0, 0, speed]
-    home_pose = [0, 8, -127, 40, 0, 0, speed]
+    home_pose = [0, 30, 30, 0, 30, 0, speed]
 
     # rsp = set_angles(*init_pose)
 

mycobot_280/mycobot_280/config/mycobot.rviz

@@ -8,6 +8,7 @@ Panels:
         - /Status1
         - /RobotModel1
         - /TF1
+        - /Marker1
       Splitter Ratio: 0.5
     Tree Height: 607
   - Class: rviz/Selection
@@ -147,6 +148,14 @@ Visualization Manager:
                       {}
       Update Interval: 0
       Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /cube
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

mycobot_280/mycobot_280pi/scripts/simple_gui.py

@@ -1,6 +1,7 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
-import Tkinter as tk
+# import Tkinter as tk
+import tkinter as tk
 from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
 import rospy
 import time
@@ -33,7 +34,7 @@ class Window:
         # 计算 Tk 根窗口的 x 和 y 坐标
         x = (self.ws / 2) - 190
         y = (self.hs / 2) - 250
-        self.win.geometry("430x400+{}+{}".format(x, y))
+        self.win.geometry("430x400+{}+{}".format(int(x), int(y)))
         # layout,布局
         self.set_layout()
         # input section,输入部分

mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # encoding=utf-8
 from __future__ import print_function
 from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus

mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py

@@ -40,8 +40,8 @@ class Object_detect(Movement):
         self.move_coords = [
             [92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
             [165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
-            [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
-            [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket         
+            [84.3, 123.8, 205.0, 153.45, -3.67, 142.01], # above the blue bucket
+            [-15, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket         
         ]
 
         # which robot: USB* is m5; ACM* is wio; AMA* is raspi
@@ -50,7 +50,7 @@ class Object_detect(Movement):
         self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
         self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
         self.raspi = False
-        if "dev" in self.robot_m5:
+        if "dev" in self.robot_m5 or "dev" in self.robot_wio:
             self.Pin = [2, 5]
             # self.Pin = [5]
         elif "dev" in self.robot_wio:
@@ -173,7 +173,7 @@ class Object_detect(Movement):
 
         
         # open pump
-        if "dev" in self.robot_m5:
+        if "dev" in self.robot_m5 or "dev" in self.robot_wio:
             self.pump_on()
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             self.gpio_status(True)
@@ -188,7 +188,7 @@ class Object_detect(Movement):
         time.sleep(0.5)
         
         # print(tmp)
-        self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
+        self.mc.send_angles([tmp[0], 17.22, -45, tmp[3], 97, tmp[5]],30)
         time.sleep(3)
 
 
@@ -200,7 +200,7 @@ class Object_detect(Movement):
         time.sleep(3)
        
         # close pump
-        if "dev" in self.robot_m5:
+        if "dev" in self.robot_m5 or "dev" in self.robot_wio:
            self.pump_off()
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             self.gpio_status(False)
@@ -231,7 +231,9 @@ class Object_detect(Movement):
     # init 270
     def run(self):
         if "dev" in self.robot_m5:
-            self.mc = MyCobot(self.robot_m5, 115200) 
+            self.mc = MyCobot(self.robot_m5, 115200)
+        elif "dev" in self.robot_wio:
+            self.mc = MyCobot(self.robot_wio, 115200) 
         elif "dev" in self.robot_raspi:
             self.mc = MyCobot(self.robot_raspi, 1000000)
         if not self.raspi:

mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py

@@ -46,8 +46,8 @@ class Object_detect(Movement):
         self.move_coords = [
             [92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
             [165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
-            [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
-            [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket  
+            [84.3, 123.8, 205.0, 153.45, -3.67, 142.01], # above the blue bucket
+            [-15, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket  
         ]
 
         # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed       
@@ -56,7 +56,7 @@ class Object_detect(Movement):
         self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
         self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
         self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
-        if "dev" in self.robot_m5:
+        if "dev" in self.robot_m5 or "dev" in self.robot_wio:
             self.Pin = [2, 5]
         elif "dev" in self.robot_wio:
             self.Pin = [20, 21]
@@ -158,21 +158,20 @@ class Object_detect(Movement):
     def move(self, x, y, color):
         # send Angle to move 270
         self.mc.send_angles(self.move_angles[0], 30)
-        time.sleep(7)
+        time.sleep(4)
 
         print("x %s ,y %s" % (x,y))
         # send coordinates to move 270 根据不同底板机械臂，调整吸泵高度
         self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
-        time.sleep(7)
-        print("ntm")
+        time.sleep(3)
 
         self.mc.send_coords([x, y, 95, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15       m5
         # self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15     pi
         # self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
-        time.sleep(6)
+        time.sleep(3)
 
         # open pump
-        if "dev" in self.robot_m5:
+        if "dev" in self.robot_m5 or "dev" in self.robot_wio:
            self.pump_on()
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             self.gpio_status(True)
@@ -187,25 +186,25 @@ class Object_detect(Movement):
         time.sleep(0.5)
         
         # print(tmp)
-        self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
-        time.sleep(6)
+        self.mc.send_angles([tmp[0], 17.22, -45, tmp[3], 97, tmp[5]],30)
+        time.sleep(3)
         
 
         self.mc.send_coords(self.move_coords[color], 30, 1)
         self.pub_marker(self.move_coords[color][0] / 1000.0,
                         self.move_coords[color][1] / 1000.0,
                         self.move_coords[color][2] / 1000.0)
-        time.sleep(6)
+        time.sleep(3)
 
         # close pump
-        if "dev" in self.robot_m5:
+        if "dev" in self.robot_m5 or "dev" in self.robot_wio:
            self.pump_off()
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             self.gpio_status(False)
         time.sleep(6)
 
         self.mc.send_angles(self.move_angles[1], 30)
-        time.sleep(6)
+        time.sleep(2)
 
     # decide whether grab cube
     def decide_move(self, x, y, color):

mycobot_ai/ai_mycobot_280/scripts/ai_windows.py

@@ -0,0 +1,191 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+
+from tkinter import ttk
+from tkinter import *
+import os,sys
+import time
+import subprocess
+
+import threading
+from multiprocessing import Process
+
+
+class Application(object):
+    def __init__(self):
+        self.win = Tk()
+        # 窗口置顶
+        self.win.wm_attributes('-topmost', 1)
+        self.ros = False
+        # 运行的文件
+        self.run_py = ""
+        # 判断通信口并给权限
+        try:
+            self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+            self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+            self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+            self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+            if "dev" in self.robot_wio:
+                self.set_file(self.robot_wio)
+            elif "dev" in self.robot_m5:
+               self.set_file(self.robot_m5)
+            elif "dev" in self.robot_raspi:
+                self.change_file(self.robot_raspi)
+            elif "dev" in self.robot_jes:
+                self.change_file(self.robot_jes)
+        except Exception as e:
+            pass
+
+        # 设置标题
+        self.win.title("aikit启动工具")
+        self.win.geometry(
+            "800x600+100+100")  # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
+        # 打开ros按钮
+        self.btn = Button(self.win, text="open ROS", font=("Helvetica","13"), command=self.open_ros)
+        self.btn.grid(row=0)
+
+        self.chanse_code = Label(self.win, text="选择程序:", font=("Helvetica","13"), width=10)
+        self.chanse_code.grid(row=1)
+
+        self.myComboList = [u"颜色识别", u"物体识别", u"二维码识别"]
+        self.myCombox = ttk.Combobox(self.win, font=("Helvetica","13"), values=self.myComboList)
+        self.myCombox.grid(row=1, column=1)
+
+        self.add_btn = Button(self.win, text="添加新的物体图像", font=("Helvetica","13"), command=self.add_img)
+        self.add_btn.grid(row=1, column=2)
+
+        self.tips = "1、首先打开ros,大概需要等待5s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、根据终端提示，输入数字(1~4)保存到相对应图像的文件夹，按下Enter键即可保存至对应文件夹。"
+
+        self.btn = Button(self.win, text="运行", font=("Helvetica","13"), command=self.start_run)
+        self.btn.grid(row=5)
+
+        self.close = Button(self.win, text="close", font=("Helvetica","13"), command=self.close_py)
+        self.close.grid(row=5, column=1)
+
+        self.t2 = None
+        self.log_data = Text(self.win, width=74, height=20, font=("Helvetica","13"))
+        self.log_data.grid(row=16, column=0, columnspan=10)
+        self.log_data.insert(END, self.tips)
+
+        # self.open_ros()
+        self.win.protocol('WM_DELETE_WINDOW', self.close_rviz)
+
+    def close_rviz(self):
+        os.system(
+            "ps -ef | grep -E mycobot.rviz | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
+        sys.exit(0)
+        
+    def set_file(self,port):
+        self.command = '<arg name="port" default="{}" />'.format(
+            port)
+        # 根据通信口修改ros启动文件
+        os.system(
+            "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/launch/vision_wio.launch"
+            .format(self.command))
+
+    def change_file(self, port):
+        command1 = '<arg name="port" default="{}" />'.format(port)
+        command2 = '<arg name="baud" default="{}" />'.format(1000000)
+        # 根据通信口修改ros启动文件
+        os.system(
+            "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command1))
+        os.system(
+            "sed -i '3c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command2))
+
+    def start_run(self):
+        try:
+            print(u"开始运行")
+            one = self.myCombox.get()
+            if one == u"颜色识别":
+                self.run_py = "combine_detect_obj_color.py"
+                t2 = threading.Thread(target=self.open_py1)
+                t2.setDaemon(True)
+                t2.start()
+            elif one == u"物体识别":
+                self.run_py = "combine_detect_obj_img_folder_opt.py"
+                t3 = threading.Thread(target=self.open_py)
+                t3.setDaemon(True)
+                t3.start()
+            elif one == u"二维码识别":
+                self.run_py = "detect_encode.py"
+                t3 = threading.Thread(target=self.open_py2)
+                t3.setDaemon(True)
+                t3.start()
+        except Exception as e:
+            self.tips = str(e)
+            self.log_data.insert(END, self.tips)
+
+    def open_py(self):
+        os.system(
+            "cd /home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280 && python scripts/combine_detect_obj_img_folder_opt.py"
+        )
+
+    def open_py1(self):
+        os.system(
+            "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_color.py"
+        )
+
+    def open_py2(self):
+        os.system(
+            "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/scripts/detect_encode.py"
+        )
+
+    def add_img(self):
+        os.system(
+            "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/scripts/add_img.py"
+        )
+
+    def open_ros(self):
+        if self.ros:
+            print("ros is opened")
+            return
+        # t1 = threading.Thread(target=self.ross)
+        # t1.setDaemon(True)
+        # t1.start()
+        self.ross()
+        self.ros = True
+
+    def ross(self):
+        # os.system(
+        #     "roslaunch ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/launch/vision_wio.launch"
+        # )
+        p = subprocess.Popen(["roslaunch", "/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/launch/vision_wio.launch"],shell=False, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
+
+    def close_py(self):
+        t1 = threading.Thread(target=self.close_p)
+        t1.setDaemon(True)
+        t1.start()
+
+    def close_p(self):
+        # 关闭ai程序
+        os.system("ps -ef | grep -E " + self.run_py +
+                  " | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
+
+    def get_current_time(self):
+        # 日志时间
+        """Get current time with format."""
+        current_time = time.strftime("%Y-%m-%d %H:%M:%S",
+                                     time.localtime(time.time()))
+        return current_time
+
+    def write_log_to_Text(self, logmsg):
+        # 设置日志函数
+        global LOG_NUM
+        current_time = self.get_current_time()
+        logmsg_in = str(current_time) + " " + str(logmsg) + "\n"  # 换行
+
+        if LOG_NUM <= 18:
+            self.log_data_Text.insert(END, logmsg_in)
+            LOG_NUM += len(logmsg_in.split("\n"))
+            # print(LOG_NUM)
+        else:
+            self.log_data_Text.insert(END, logmsg_in)
+            self.log_data_Text.yview("end")
+
+    def run(self):
+        self.win.mainloop()
+
+
+if __name__ == "__main__":
+    mc = Application()
+    mc.run()

mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_color.py

@@ -51,8 +51,8 @@ class Object_detect(Movement):
             # self.Pin = [20, 21]
             self.Pin = [2, 5]
 
-            for i in self.move_coords:
-                i[2] -= 20
+            # for i in self.move_coords:
+            #     i[2] -= 20
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             import RPi.GPIO as GPIO
             GPIO.setwarnings(False)

mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py

@@ -61,8 +61,8 @@ class Object_detect(Movement):
             self.Pin = [2, 5]
         elif "dev" in self.robot_wio:
             self.Pin = [2, 5]
-            for i in self.move_coords:
-                i[2] -= 20
+            # for i in self.move_coords:
+            #     i[2] -= 20
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             import RPi.GPIO as GPIO
             GPIO.setwarnings(False)

mycobot_ai/ai_ultraarm/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(ai_mira)
+project(ai_ultraarm)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -8,7 +8,7 @@ project(ai_mira)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-  mira
+  ultraarm
 )
 
 ## System dependencies are found with CMake's conventions

mycobot_ai/ai_ultraarm/launch/vision_m5.launch

@@ -2,8 +2,8 @@
 	<arg name="port" default="/dev/ttyUSB0" />
 	<arg name="baud" default="115200" />
 
-	<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260_aikit.urdf"/>
-	<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
+	<arg name="model" default="$(find mycobot_description)/urdf/ultraArm_urdf/ultraArm_aikit.urdf"/>
+	<arg name="rvizconfig" default="$(find ultraarm)/config/ultraArm.rviz" />
 	<!-- <arg name="gui" default="false" /> -->
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
@@ -14,13 +14,13 @@
 	</node>
 
 	<!-- mypalletizer-topics -->
-    <include file="$(find mira_communication)/launch/communication_topic.launch">
+    <include file="$(find ultraarm_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="mira_260" type="listen_real_of_topic.py" />
+	<node name="real_listener" pkg="ultraarm" type="listen_real_of_topic.py" />
 
 	<!-- Show in Rviz -->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

mycobot_ai/ai_ultraarm/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>ai_mira</name>
+  <name>ai_ultraarm</name>
   <version>0.0.0</version>
-  <description>The ai_mira package</description>
+  <description>The ai_ultraarm package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -49,9 +49,9 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>mira</build_depend>
-  <build_export_depend>mira</build_export_depend>
-  <exec_depend>mira</exec_depend>
+  <build_depend>ultraarm</build_depend>
+  <build_export_depend>ultraarm</build_export_depend>
+  <exec_depend>ultraarm</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

mycobot_ai/ai_ultraarm/scripts/add_img.py

@@ -49,8 +49,8 @@ def take_photo():
 
 def cut_photo():
     
-    path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/'    # pi
-    path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/'         # m5
+    path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_ultraarm/'    # pi
+    path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_ultraarm/'         # m5
 
     if os.path.exists(path1):
         path = path1

mycobot_ai/ai_ultraarm/scripts/combine_detect_obj_color.py

@@ -9,7 +9,7 @@ import json
 import os,sys
 import rospy
 from visualization_msgs.msg import Marker   
-from pymycobot.mira import Mira
+from pymycobot.ultraArm import ultraArm
 from moving_utils import Movement
 
 IS_CV_4 = cv2.__version__[0] == '4'
@@ -25,8 +25,8 @@ class Object_detect(Movement):
         # get path of file
         dir_path = os.path.dirname(__file__)
 
-        # declare mira
-        self.mc = None
+        # declare ultraArm
+        self.ua = None
 
         # 移动角度
         self.move_angles = [
@@ -63,11 +63,11 @@ class Object_detect(Movement):
             "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
             "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
         }
-        # use to calculate coord between cube and mira
+        # use to calculate coord between cube and ultraArm
         self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
-        # The coordinates of the grab center point relative to the mira
+        # The coordinates of the grab center point relative to the ultraArm
         self.camera_x, self.camera_y = camera_x, camera_y
-        # The coordinates of the cube relative to the mira
+        # The coordinates of the cube relative to the ultraArm
         self.c_x, self.c_y = 0, 0
         # The ratio of pixels to actual values
         self.ratio = 0
@@ -121,35 +121,35 @@ class Object_detect(Movement):
     
     # 开启吸泵 m5
     def pump_on(self):
-        self.mc.set_gpio_state(0)
+        self.ua.set_gpio_state(0)
 
     # 停止吸泵 m5
     def pump_off(self):
-        self.mc.set_gpio_state(1)
+        self.ua.set_gpio_state(1)
 
     # Grasping motion
     def move(self, x, y, color):
-        # send Angle to move mira
+        # send Angle to move ultraArm
         print('color-->', color)
-        self.mc.set_angles(self.move_angles[0], 20)
+        self.ua.set_angles(self.move_angles[0], 20)
         time.sleep(3)
 
-        # send coordinates to move mira
-        self.mc.set_coords([x, -y, 58.84], 20)
+        # send coordinates to move ultraArm
+        self.ua.set_coords([x, -y, 58.84], 20)
         time.sleep(1.5)
-        self.mc.set_coords([x, -y, 21.8], 20)
+        self.ua.set_coords([x, -y, 21.8], 20)
         time.sleep(2)
 
         # open pump
         self.pump_on()
         time.sleep(1.5)
 
-        self.mc.set_angle(2, 0, 30)
+        self.ua.set_angle(2, 0, 30)
         time.sleep(0.3)
-        self.mc.set_angle(3, 0, 30)
+        self.ua.set_angle(3, 0, 30)
         time.sleep(1)
 
-        self.mc.set_coords(self.move_coords[color], 30)
+        self.ua.set_coords(self.move_coords[color], 30)
         self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
                         [1]/1000.0, self.move_coords[color][2]/1000.0)
         time.sleep(7)
@@ -166,7 +166,7 @@ class Object_detect(Movement):
             self.pub_marker(
                 self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
 
-        self.mc.set_angles(self.move_angles[1], 20)
+        self.ua.set_angles(self.move_angles[1], 20)
         time.sleep(1.5)
 
     # decide whether grab cube
@@ -181,11 +181,11 @@ class Object_detect(Movement):
             # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
             self.move(x, y, color)
 
-    # init mira
+    # init ultraArm
     def run(self):
-        self.mc = Mira(self.robot_m5, 115200)
-        self.mc.go_zero()
-        self.mc.set_angles([19.48, 0.0, 0.0], 40)
+        self.ua = ultraArm(self.robot_m5, 115200)
+        self.ua.go_zero()
+        self.ua.set_angles([19.48, 0.0, 0.0], 40)
         time.sleep(3)
 
     # draw aruco
@@ -239,13 +239,13 @@ class Object_detect(Movement):
         self.y2 = int(y2)
         print(self.x1, self.y1, self.x2, self.y2)
 
-    # set parameters to calculate the coords between cube and mira
+    # set parameters to calculate the coords between cube and ultraArm
     def set_params(self, c_x, c_y, ratio):
         self.c_x = c_x
         self.c_y = c_y
         self.ratio = 220.0/ratio
 
-    # calculate the coords between cube and mira
+    # calculate the coords between cube and ultraArm
     def get_position(self, x, y):
         return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
 
@@ -319,7 +319,7 @@ class Object_detect(Movement):
                     cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
                     # calculate the rectangle center
                     x, y = (x*2+w)/2, (y*2+h)/2
-                    # calculate the real coordinates of mira relative to the target
+                    # calculate the real coordinates of ultraArm relative to the target
                     if mycolor == "red":
                         self.color = 0
                     elif mycolor == "green":
@@ -343,7 +343,7 @@ if __name__ == "__main__":
         cap.open()
     # init a class of Object_detect
     detect = Object_detect()
-    # init mira
+    # init ultraArm
     detect.run()
 
     _init_ = 20  
@@ -386,7 +386,7 @@ if __name__ == "__main__":
             init_num += 1
             continue
 
-        # calculate params of the coords between cube and mira
+        # calculate params of the coords between cube and ultraArm
         if nparams < 10:
             if detect.get_calculate_params(frame) is None:
                 cv2.imshow("figure", frame)
@@ -403,7 +403,7 @@ if __name__ == "__main__":
                 continue
         elif nparams == 10:
             nparams += 1
-            # calculate and set params of calculating real coord between cube and mira
+            # calculate and set params of calculating real coord between cube and ultraArm
             detect.set_params(
                 (detect.sum_x1+detect.sum_x2)/20.0,
                 (detect.sum_y1+detect.sum_y2)/20.0,
@@ -420,7 +420,7 @@ if __name__ == "__main__":
             continue
         else:
             x, y = detect_result
-            # calculate real coord between cube and mira
+            # calculate real coord between cube and ultraArm
             real_x, real_y = detect.get_position(x, y)
             if num == 20:
                 detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)

mycobot_ai/ai_ultraarm/scripts/combine_detect_obj_img_folder_opt.py

@@ -20,7 +20,7 @@ from threading import Thread
 import tkFileDialog as filedialog
 import Tkinter as tk
 from moving_utils import Movement
-from pymycobot.mira import Mira
+from pymycobot.ultraArm import ultraArm
 
 IS_CV_4 = cv2.__version__[0] == '4'
 __version__ = "1.0"  # Adaptive seeed
@@ -35,7 +35,7 @@ class Object_detect(Movement):
         dir_path = os.path.dirname(__file__)
 
         # declare mypal260
-        self.mc = None
+        self.ua = None
         # 移动角度
         self.move_angles = [
             [0.0, 0.0, 0.0],  # init the point
@@ -116,34 +116,34 @@ class Object_detect(Movement):
     
     # 开启吸泵 m5
     def pump_on(self):
-        self.mc.set_gpio_state(0)
+        self.ua.set_gpio_state(0)
 
     # 停止吸泵 m5
     def pump_off(self):
-        self.mc.set_gpio_state(1)
+        self.ua.set_gpio_state(1)
 
     # Grasping motion
     def move(self, x, y, color):
         # send Angle to move mypal260
-        self.mc.set_angles(self.move_angles[0], 20)
+        self.ua.set_angles(self.move_angles[0], 20)
         time.sleep(3)
         
         # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度
-        self.mc.set_coords([x, -y, 58.84], 20)
+        self.ua.set_coords([x, -y, 58.84], 20)
         time.sleep(1.5)
-        self.mc.set_coords([x, -y, 21.8], 20)
+        self.ua.set_coords([x, -y, 21.8], 20)
         time.sleep(2)
 
         # open pump
         self.pump_on()
         time.sleep(1.5)
 
-        self.mc.set_angle(2, 0, 30)
+        self.ua.set_angle(2, 0, 30)
         time.sleep(0.3)
-        self.mc.set_angle(3, 0, 30)
+        self.ua.set_angle(3, 0, 30)
         time.sleep(2)
 
-        self.mc.set_coords(self.move_coords[color], 30)
+        self.ua.set_coords(self.move_coords[color], 30)
         self.pub_marker(self.move_coords[color][0] / 1000.0,
                         self.move_coords[color][1] / 1000.0,
                         self.move_coords[color][2] / 1000.0)
@@ -153,7 +153,7 @@ class Object_detect(Movement):
         self.pump_off()
         time.sleep(6)
 
-        self.mc.set_angles(self.move_angles[1], 20)
+        self.ua.set_angles(self.move_angles[1], 20)
         time.sleep(1.5)
 
     # decide whether grab cube
@@ -170,9 +170,9 @@ class Object_detect(Movement):
 
     # init mypal260
     def run(self):
-        self.mc = Mira(self.robot_m5, 115200) 
-        self.mc.go_zero()
-        self.mc.set_angles([19.48, 0.0, 0.0], 40)
+        self.ua = ultraArm(self.robot_m5, 115200) 
+        self.ua.go_zero()
+        self.ua.set_angles([19.48, 0.0, 0.0], 40)
         time.sleep(3)
 
     # draw aruco
@@ -358,8 +358,8 @@ class Object_detect(Movement):
 # The path to save the image folder
 def parse_folder(folder):
     restore = []
-    path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' + folder
-    path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' + folder
+    path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_ultraarm/' + folder
+    path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_ultraarm/' + folder
 
     if os.path.exists(path1):
         path = path1

mycobot_communication/scripts/mycobot_services.py

@@ -1,13 +1,51 @@
 #!/usr/bin/env python2
-# -*- coding: utf-8 -*
+# -*- coding: utf-8 -*-
 import time
 import rospy
+import os
+import fcntl
 from mycobot_communication.srv import *
 
 from pymycobot.mycobot import MyCobot
 
 mc = None
 
+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+    open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+    fd = os.open(lock_file, open_mode)
+
+    pid = os.getpid()
+    lock_file_fd = None
+    
+    timeout = 50.0
+    start_time = current_time = time.time()
+    while current_time < start_time + timeout:
+        try:
+            # The LOCK_EX means that only one process can hold the lock
+            # The LOCK_NB means that the fcntl.flock() is not blocking
+            # and we are able to implement termination of while loop,
+            # when timeout is reached.
+            fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+        except (IOError, OSError):
+            pass
+        else:
+            lock_file_fd = fd
+            break
+        # print('pid waiting for lock:%d'% pid)
+        time.sleep(1.0)
+        current_time = time.time()
+    if lock_file_fd is None:
+        os.close(fd)
+    return lock_file_fd
+
+
+def release(lock_file_fd):
+    # Do not remove the lockfile:
+    fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+    os.close(lock_file_fd)
+    return None
+
 
 def create_handle():
     global mc
@@ -41,9 +79,11 @@ def set_angles(req):
         req.joint_6,
     ]
     sp = req.speed
-    print('angles1:',angles)
+
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         mc.send_angles(angles, sp)
+        release(lock)
 
     return SetAnglesResponse(True)
 
@@ -51,8 +91,9 @@ def set_angles(req):
 def get_angles(req):
     """get angles,获取角度"""
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         angles = mc.get_angles()
-        print('angles2:',angles)
+        release(lock)
         return GetAnglesResponse(*angles)
 
 
@@ -69,15 +110,18 @@ def set_coords(req):
     mod = req.model
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         mc.send_coords(coords, sp, mod)
+        release(lock)
 
     return SetCoordsResponse(True)
 
 
 def get_coords(req):
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         coords = mc.get_coords()
-        print('coords:',coords)
+        release(lock)
         return GetCoordsResponse(*coords)
 
 
@@ -85,23 +129,26 @@ def switch_status(req):
     """Gripper switch status"""
     """夹爪开关状态"""
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         if req.Status:
             mc.set_gripper_state(0, 80)
         else:
             mc.set_gripper_state(1, 80)
+        release(lock)
 
     return GripperStatusResponse(True)
 
 
 def toggle_pump(req):
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         if req.Status:
             mc.set_basic_output(req.Pin1, 0)
             mc.set_basic_output(req.Pin2, 0)
         else:
             mc.set_basic_output(req.Pin1, 1)
             mc.set_basic_output(req.Pin2, 1)
-
+        release(lock)
     return PumpStatusResponse(True)
 
 
@@ -133,11 +180,15 @@ def output_robot_message():
     atom_version = "unknown"
 
     if mc:
+        lock = acquire("/tmp/mycobot_lock")
         cn = mc.is_controller_connected()
+        release(lock)
         if cn == 1:
             connect_status = True
         time.sleep(0.1)
+        lock = acquire("/tmp/mycobot_lock")
         si = mc.is_all_servo_enable()
+        release(lock)
         if si == 1:
             servo_infomation = "all connected"
 

mycobot_description/urdf/ultraArm_urdf/ultraArm.urdf

@@ -8,14 +8,14 @@
     <visual>
       <geometry>
 	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-base.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-base.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/mira_urdf/Mira-base.dae"/>
+        <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-base.dae"/>
       </geometry>
       <origin xyz = " 0 0 0.0 " rpy = " 0 0 0"/>
     </collision>
@@ -25,13 +25,13 @@
   <link name="link1">
     <visual>
       <geometry>
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-1.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-1.dae"/>
       </geometry>
       <origin xyz = "0 0 0.122 " rpy = " 3.1415926 0 0"/>
     </visual>
     <collision>
       <geometry>
-        <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-1.dae"/>
+        <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-1.dae"/>
       </geometry>
       <origin xyz = "0 0  0.122 " rpy = " 3.1415926 0 0"/>
     </collision>
@@ -40,13 +40,13 @@
   <link name="link2">
     <visual>
       <geometry>    
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-2.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-2.dae"/>
       </geometry>
       <origin xyz = "-0.03  0.112 0.0  " rpy = " 1.5708 0 0"/>
     </visual>
     <collision>
       <geometry>    
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-2.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-2.dae"/>
       </geometry>
       <origin xyz = "-0.03  0.112 0.0  " rpy = " 1.5708 0 0"/>
     </collision>     
@@ -57,14 +57,14 @@
     <visual>
       <geometry>
        <!--- 0.0 0 -0.04 -->
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-3.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-3.dae"/>
       </geometry>
       <origin xyz = "-0.03 0.23  0.0 " rpy = "1.5708 0 0"/>
     </visual>
     <collision>
       <geometry>
        <!--- 0.0 0 -0.04 -->
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-3.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-3.dae"/>
       </geometry>
       <origin xyz = "-0.03 0.23  0.0 " rpy = "1.5708 0 0"/>
     </collision>

mycobot_description/urdf/ultraArm_urdf/ultraArm_aiki.urdf

@@ -6,7 +6,7 @@
    <link name="env">
     <visual>
       <geometry>
-        <mesh filename="package://mycobot_description/urdf/mira_urdf/suit_env.dae" scale="1.2 1.2 1.2"/>
+        <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/suit_env.dae" scale="1.2 1.2 1.2"/>
       </geometry>
       <origin xyz = "0 0 -0.02 " rpy = "1.5708 0 -1.5708"/>
     </visual>
@@ -16,14 +16,14 @@
     <visual>
       <geometry>
 	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-base.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-base.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/mira_urdf/Mira-base.dae"/>
+        <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-base.dae"/>
       </geometry>
       <origin xyz = " 0 0 0.0 " rpy = " 0 0 0"/>
     </collision>
@@ -33,13 +33,13 @@
   <link name="link1">
     <visual>
       <geometry>
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-1.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-1.dae"/>
       </geometry>
       <origin xyz = "0 0 0.122 " rpy = " 3.1415926 0 0"/>
     </visual>
     <collision>
       <geometry>
-        <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-1.dae"/>
+        <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-1.dae"/>
       </geometry>
       <origin xyz = "0 0  0.122 " rpy = " 3.1415926 0 0"/>
     </collision>
@@ -48,13 +48,13 @@
   <link name="link2">
     <visual>
       <geometry>    
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-2.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-2.dae"/>
       </geometry>
       <origin xyz = "-0.03  0.112 0.0  " rpy = " 1.5708 0 0"/>
     </visual>
     <collision>
       <geometry>    
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-2.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-2.dae"/>
       </geometry>
       <origin xyz = "-0.03  0.112 0.0  " rpy = " 1.5708 0 0"/>
     </collision>     
@@ -65,14 +65,14 @@
     <visual>
       <geometry>
        <!--- 0.0 0 -0.04 -->
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-3.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-3.dae"/>
       </geometry>
       <origin xyz = "-0.03 0.23  0.0 " rpy = "1.5708 0 0"/>
     </visual>
     <collision>
       <geometry>
        <!--- 0.0 0 -0.04 -->
-       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-3.dae"/>
+       <mesh filename="package://mycobot_description/urdf/ultraArm_urdf/ultraArm-3.dae"/>
       </geometry>
       <origin xyz = "-0.03 0.23  0.0 " rpy = "1.5708 0 0"/>
     </collision>

ultraArm/ultraarm/CMakeLists.txt

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

ultraArm/ultraarm/launch/simple_gui.launch

@@ -3,8 +3,8 @@
 	<arg name="port" default="/dev/ttyUSB0" />
 	<arg name="baud" default="115200" />
 	<!-- Load file model ，加载文件模型-->
-	<arg name="model" default="$(find mycobot_description)/urdf/mira_urdf/mira.urdf"/>
-	<arg name="rvizconfig" default="$(find mira)/config/mira.rviz" />
+	<arg name="model" default="$(find mycobot_description)/urdf/ultraArm_urdf/ultraArm.urdf"/>
+	<arg name="rvizconfig" default="$(find ultraarm)/config/ultraArm.rviz" />
 	<arg name="gui" default="false" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
@@ -14,11 +14,11 @@
 	<!-- Show in Rviz -->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 	<!-- mycobot-topics, mycobot-话题-->
-    <include file="$(find mira_communication)/launch/communication_service.launch">
+    <include file="$(find ultraarm_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="mira" type="listen_real.py" />
-	<node name="simple_gui" pkg="mira" type="simple_gui.py" />
+	<node name="real_listener" pkg="ultraarm" type="listen_real.py" />
+	<node name="simple_gui" pkg="ultraarm" type="simple_gui.py" />
 </launch>

ultraArm/ultraarm/launch/slider_control.launch

@@ -3,8 +3,8 @@
 	<arg name="baud" default="115200" /> -->
 	
 	<!-- Load file model ，加载文件模型-->
-	<arg name="model" default="$(find mycobot_description)/urdf/mira_urdf/mira.urdf"/>
-	<arg name="rvizconfig" default="$(find mira)/config/mira.rviz" />
+	<arg name="model" default="$(find mycobot_description)/urdf/ultraArm_urdf/ultraArm.urdf"/>
+	<arg name="rvizconfig" default="$(find ultraarm)/config/ultraArm.rviz" />
 	<arg name="gui" default="true" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />

ultraArm/ultraarm/launch/teleop_keyboard.launch

@@ -6,8 +6,8 @@
 	<!-- Load file model ，加载文件模型-->
 	<!-- <arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" /> -->
-	<arg name="model" default="$(find mycobot_description)/urdf/mira_urdf/mira.urdf"/>
-	<arg name="rvizconfig" default="$(find mira)/config/mira.rviz" />
+	<arg name="model" default="$(find mycobot_description)/urdf/ultraArm_urdf/ultraArm.urdf"/>
+	<arg name="rvizconfig" default="$(find ultraarm)/config/ultraArm.rviz" />
 	<arg name="gui" default="false" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
@@ -18,11 +18,11 @@
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
 	<!-- mycobot-topics，mycobot-话题 -->
-    <include file="$(find mira_communication)/launch/communication_service.launch">
+    <include file="$(find ultraarm_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="mira" type="listen_real.py" />
+	<node name="real_listener" pkg="ultraarm" type="listen_real.py" />
 </launch>

ultraArm/ultraarm/launch/test.launch

@@ -1,7 +1,7 @@
 <launch>
 	<!-- Load file model，加载文件模型 -->
-	<arg name="model" default="$(find mycobot_description)/urdf/mira_urdf/mira.urdf"/>
-	<arg name="rvizconfig" default="$(find mira)/config/mira.rviz" />
+	<arg name="model" default="$(find mycobot_description)/urdf/ultraArm_urdf/ultraArm.urdf"/>
+	<arg name="rvizconfig" default="$(find ultraarm)/config/ultraArm.rviz" />
 	<arg name="gui" default="true" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />

ultraArm/ultraarm/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>mira</name>
+  <name>ultraarm</name>
   <version>0.3.0</version>
-  <description>The mira package</description>
+  <description>The ultraarm package</description>
 
   <author email="lijun.zhang@elephantrobotics.com">ZhangLijun</author>
   <maintainer email="lijun.zhang@elephantrobotics.com">ZhangLijun</maintainer>
@@ -18,9 +18,9 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>mycobot_description</build_depend>
-  <build_depend>mira_communication</build_depend>
+  <build_depend>ultraarm_communication</build_depend>
 
-  <build_export_depend>mira_communication</build_export_depend>
+  <build_export_depend>ultraarm_communication</build_export_depend>
   <build_export_depend>mycobot_description</build_export_depend>
 
   <exec_depend>roscpp</exec_depend>
@@ -36,7 +36,7 @@
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>python-tk</exec_depend>
   <exec_depend>mycobot_description</exec_depend>
-  <exec_depend>mira_communication</exec_depend>
+  <exec_depend>ultraarm_communication</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

ultraArm/ultraarm/scripts/listen_real.py

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

ultraArm/ultraarm/scripts/listen_real_of_topic.py

@@ -1,11 +1,11 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 import math
 
 import rospy
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Header
 # from mycobot_communication.msg import MycobotAngles
-from mira_communication.msg import MiraAngles
+from ultraarm_communication.msg import ultraArmAngles
  
 
 class Listener(object):
@@ -17,7 +17,7 @@ class Listener(object):
         # init publisher. 初始化发布者
         self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
         # init subscriber. 初始化订阅者
-        self.sub = rospy.Subscriber("mypal/angles_real", MiraAngles, self.callback)
+        self.sub = rospy.Subscriber("mypal/angles_real", ultraArmAngles, self.callback)
         rospy.spin()
 
     def callback(self, data):