update mira ros

Mira/mira/CMakeLists.txt

@@ -21,7 +21,6 @@ catkin_package(
 include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
 
 catkin_install_python(PROGRAMS
-  scripts/follow_display.py
   scripts/slider_control.py
   scripts/teleop_keyboard.py
   scripts/listen_real.py

Mira/mira/config/mira.rviz

@@ -9,7 +9,7 @@ Panels:
         - /TF1
         - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 591
+    Tree Height: 579
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -161,17 +161,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.40479761362075806
+      Pitch: 0.34979769587516785
       Target Frame: <Fixed Frame>
-      Yaw: 2.270383358001709
+      Yaw: 2.305382013320923
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 888
+  Height: 876
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002da000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002cefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ce000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000002ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

Mira/mira/launch/follow.launch

@@ -1,14 +0,0 @@
-<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" />
-
-	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
-
-	<!-- Combinejoin values to TF .将值合并到 TF-->
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
-		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
-	</node>
-	<!-- Show in Rviz ，显示在Rviz-->
-	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
-</launch>

Mira/mira/scripts/follow_display.py

@@ -1,109 +0,0 @@
-#!/usr/bin/env python3
-import time
-
-import rospy
-from sensor_msgs.msg import JointState
-from std_msgs.msg import Header
-from visualization_msgs.msg import Marker
-
-
-from pymycobot.mypalletizer import MyPalletizer
-
-
-def talker():
-    rospy.init_node("display", anonymous=True)
-
-    print("Try connect real mypal...")
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
-    baud = rospy.get_param("~baud", 115200)
-    print("port: {}, baud: {}\n".format(port, baud))
-    try:
-        mira = MyPalletizer(port, baud)
-    except Exception as e:
-        print(e)
-        print(
-            """\
-            \rTry connect mypalletizer failed!
-            \rPlease check wether connected with MyPalletizer.
-            \rPlease chckt wether the port or baud is right.
-        """
-        )
-        exit(1)
-    mira.release_all_servos()
-    time.sleep(0.1)
-    print("Rlease all servos over.\n")
-
-    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
-    pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
-    rate = rospy.Rate(30)  # 30hz
-
-    # pub joint state. 发布关节状态
-    joint_state_send = JointState()
-    joint_state_send.header = Header()
-
-    joint_state_send.name = [
-
-        "joint1_to_base",
-        "joint2_to_joint1",
-        "joint3_to_joint2",
-        # "joint4_to_joint3",
-        # "joint5_to_joint4",
-    ]
-    joint_state_send.velocity = [0]
-    joint_state_send.effort = []
-
-    marker_ = Marker()
-    marker_.header.frame_id = "/base"
-    marker_.ns = "my_namespace"
-
-    print("publishing ...")
-    while not rospy.is_shutdown():
-        joint_state_send.header.stamp = rospy.Time.now()
-
-        angles = mira.get_radians()
-        data_list = []
-        for index, value in enumerate(angles):
-            data_list.append(value)
-
-        # rospy.loginfo('{}'.format(data_list))
-        joint_state_send.position = data_list
-        
-        # data_list.insert(3,0.0)
-        print(joint_state_send.position)
-
-        pub.publish(joint_state_send)
-
-        coords = mira.get_coords()
-
-        # marker
-        marker_.header.stamp = rospy.Time.now()
-        marker_.type = marker_.SPHERE
-        marker_.action = marker_.ADD
-        marker_.scale.x = 0.04
-        marker_.scale.y = 0.04
-        marker_.scale.z = 0.04
-
-        # marker position initial. 标记初始位置
-        # print(coords)
-        if not coords:
-            # coords = [0, 0, 0, 0, 0 ]
-            coords = [0, 0, 0, 0]
-
-            # rospy.loginfo("error [101]: can not get coord values")
-
-        marker_.pose.position.x = coords[1] / 1000 * -1
-        marker_.pose.position.y = coords[0] / 1000
-        marker_.pose.position.z = coords[2] / 1000
-
-        marker_.color.a = 1.0
-        marker_.color.g = 1.0
-        pub_marker.publish(marker_)
-
-        rate.sleep()
-
-
-if __name__ == "__main__":
-    try:
-        talker()
-    except rospy.ROSInterruptException:
-        pass

Mira/mira/scripts/listen_real.py

@@ -6,9 +6,9 @@ import math
 import rospy
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Header
-# from mycobot_communication.srv import GetAngles
 from mira_communication.srv import GetAngles
 # from pymycobot.mypalletizer import MyPalletizer
+from pymycobot.mira import Mira
 
 
 def talker():
@@ -28,7 +28,7 @@ def talker():
         # "joint4_to_joint3",
         # "joint5_to_joint4",
     ]
-    joint_state_send.velocity = [0]
+    joint_state_send.velocity = [0.0]
     joint_state_send.effort = []
     
     # waiting util server `get_joint_angles` enable.
@@ -40,6 +40,7 @@ def talker():
     while not rospy.is_shutdown():
         # get real angles from server.
         #  从服务获取真实的角度
+
         res = func()
         if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
             continue
@@ -52,7 +53,7 @@ def talker():
 
         ]
         rospy.loginfo("res: {}".format(radians_list))
-
+        print('res:',res)
         # publish angles. 发布角度
         joint_state_send.header.stamp = rospy.Time.now()
         joint_state_send.position = radians_list

Mira/mira/scripts/simple_gui.py

@@ -12,7 +12,7 @@ class Window:
         self.win = handle
         self.win.resizable(0, 0)  # fixed window size 固定窗口大小 
 
-        self.model = 0
+        # self.model = 0
         self.speed = rospy.get_param("~speed", 50)
 
         # set default speed. 设置默认速度 
@@ -22,8 +22,8 @@ class Window:
         self.connect_ser()
 
         # Get robotic arm data. 获取机械臂数据
-        self.record_coords = [0, 0, 0, self.speed,]
+        self.record_coords = [0, 0, 0, self.speed]
-        self.res_angles = [0, 0, 0, self.speed,]
+        self.res_angles = [0, 0, 0, self.speed]
         self.get_date()
 
         # get screen width and height. 获取屏幕宽度和高度
@@ -33,7 +33,7 @@ class Window:
         # 计算 Tk 根窗口的 x 和 y 坐标
         x = (self.ws / 2) - 190
         y = (self.hs / 2) - 250
-        self.win.geometry("430x350+{}+{}".format(x, y))
+        self.win.geometry("430x350+{}+{}".format(int(x), int(y)))
         # layout. 布局
         self.set_layout()
         # input. 输入部分
@@ -66,15 +66,15 @@ class Window:
         rospy.wait_for_service("set_joint_angles")
         rospy.wait_for_service("get_joint_coords")
         rospy.wait_for_service("set_joint_coords")
-        rospy.wait_for_service("switch_gripper_status")
+        # rospy.wait_for_service("switch_gripper_status")
         try:
-            self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
+            self.get_coords_info = rospy.ServiceProxy("get_joint_coords", GetCoords)
             self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
-            self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
+            self.get_angles_info = rospy.ServiceProxy("get_joint_angles", GetAngles)
             self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
-            self.switch_gripper = rospy.ServiceProxy(
+            # self.switch_gripper = rospy.ServiceProxy(
-                "switch_gripper_status", GripperStatus
+            #     "switch_gripper_status", GripperStatus
-            )
+            # )
         except:
             print("start error ...")
             exit(1)
@@ -143,8 +143,8 @@ class Window:
 
 
         # All input boxes, used to get the input data. 所有输入框，用于拿输入的数据
-        self.all_j = [self.J_1, self.J_2, self.J_3,]
+        self.all_j = [self.J_1, self.J_2, self.J_3]
-        self.all_c = [self.x, self.y, self.z,]
+        self.all_c = [self.x, self.y, self.z]
 
 
 
@@ -285,10 +285,10 @@ class Window:
             int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
         )
         c_value.append(self.speed)
-        c_value.append(self.model)
+        # c_value.append(self.model)
         # print(c_value)
         try:
-            self.set_coords(*c_value)
+            self.set_coords(c_value[0], c_value[1], c_value[2], self.speed)
         except ServiceException:
             pass
         self.show_j_date(c_value[:-2], "coord")
@@ -306,7 +306,7 @@ class Window:
         j_value.append(self.speed)
 
         try:
-            self.set_angles(*j_value)
+            self.set_angles(j_value[0], j_value[1], j_value[2], self.speed)
         except ServiceException:
             pass
         self.show_j_date(j_value[:-1])
@@ -316,34 +316,41 @@ class Window:
         # Get the data of robotic arm for display. 拿机械臂的数据，用于展示
         t = time.time()
         while time.time() - t < 2:
-            self.res = self.get_coords()
+            self.res = self.get_coords_info()
 
             print(self.res)
             
-            if self.res.x > 1:
+            # if self.res.x > 1:
+            if self.res!= []:
                 break
             time.sleep(0.1)
 
         t = time.time()
         while time.time() - t < 2:
-            self.angles = self.get_angles()
+            self.angles = self.get_angles_info()
-            if self.angles.joint_1 > 1:
+            # if self.angles.joint_1 > 1:
+            if self.angles != []:
                 break
             time.sleep(0.1)
         # print(self.angles.joint_1)
+            if self.res and self.angles != None:
+                print('---------------->',self.res)
                 self.record_coords = [
                     round(self.res.x, 2),
                     round(self.res.y, 2),
                     round(self.res.z, 2),
-            self.speed,
+                    self.speed
                 ]
+                # self.record_coords[0] = self.res[:3]
+                # self.res_angles[0] = self.angles[:3]
                 self.res_angles = [
                     round(self.angles.joint_1, 2),
                     round(self.angles.joint_2, 2),
                     round(self.angles.joint_3, 2),
+    
                 ]
-        # print('coord:',self.record_coords)
+                print('coord:',self.record_coords)
-        # print('angles:',self.res_angles)
+                print('angles:',self.res_angles)
 
     # def send_input(self,dates):
     def show_j_date(self, date, way=""):

Mira/mira/scripts/slider_control.py

@@ -9,14 +9,15 @@ Passable parameters:
     baud: serial prot baudrate. Defaults is 115200.
 """
 
+import time
 import rospy
 from sensor_msgs.msg import JointState
 
 # from pymycobot.mycobot import MyCobot
-from pymycobot.mypalletizer import MyPalletizer
+from pymycobot.mira import Mira
+import math
 
-
+ma = None
-mc = None
 
 
 def callback(data):
@@ -24,22 +25,24 @@ def callback(data):
     print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
+        data_list.append(round(value,3))
   
-    # print(data_list)
+    print('data_list:',data_list)
-    mc.send_radians(data_list, 80)
+    ma.set_radians(data_list[0],data_list[1],data_list[2], 50)
     # time.sleep(0.5)
 
 
 def listener():
-    global mc
+    global ma
     rospy.init_node("control_slider", anonymous=True)
 
     rospy.Subscriber("joint_states", JointState, callback)
     port = rospy.get_param("~port", "/dev/ttyUSB0") # Select connected device. 选择连接设备
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
-    mc = MyPalletizer(port, baud)
+    ma = Mira(port, baud)
+    ma.power_on()
+    ma.go_zero()
 
     # spin() simply keeps python from exiting until this node is stopped
     # spin() 只是阻止python退出，直到该节点停止

Mira/mira_communication/scripts/mira_services.py

@@ -5,20 +5,26 @@ import rospy
 # from mycobot_communication.srv import *
 from mira_communication.srv import *
 
-# from pymycobot.mycobot import MyCobot
+from pymycobot.mira import Mira
-from pymycobot.mypalletizer import MyPalletizer
 
-mc = None
+ma = None
 
 
 def create_handle():
-    global mc
+    global ma
     rospy.init_node("mira_services")
     rospy.loginfo("start ...")
     port = rospy.get_param("~port")
     baud = rospy.get_param("~baud")
     rospy.loginfo("%s,%s" % (port, baud))
-    mc = MyPalletizer(port, baud)
+    ma = Mira(port, baud)
+    
+    # Power on the robotic arm
+    ma.power_on()
+    # calibrate the zero position of the robot arm
+    ma.go_zero()
+    
+    time.sleep(5)
 
 
 def create_services():
@@ -27,7 +33,7 @@ def create_services():
     rospy.Service("set_joint_coords", SetCoords, set_coords)
     rospy.Service("get_joint_coords", GetCoords, get_coords)
     rospy.Service("switch_gripper_status", GripperStatus, switch_status)
-    rospy.Service("switch_pump_status", PumpStatus, toggle_pump)
+    # rospy.Service("switch_pump_status", PumpStatus, toggle_pump)
     rospy.loginfo("ready")
     rospy.spin()
 
@@ -37,22 +43,23 @@ def set_angles(req):
         req.joint_1,
         req.joint_2,
         req.joint_3,
-        # req.joint_4,
-        # req.joint_5,
-        # req.joint_6,
     ]
     sp = req.speed
-
+    print('mira_services:',angles)
-    if mc:
+    if ma:
-        mc.send_angles(angles, sp)
+        ma.set_angles(angles[0], angles[1], angles[2], sp)
 
     return SetAnglesResponse(True)
 
 
 def get_angles(req):
-    if mc:
+    if ma:
-        angles = mc.get_angles()
+        angles = ma.get_angles_info()
-        return GetAnglesResponse(*angles)
+        # angles = [0.0, 0.0, 0.0, 0.0]
+        if angles != None:
+            angles_data = angles[:3]
+            # print('angles:',angles)
+            return GetAnglesResponse(*angles_data)
 
 
 def set_coords(req):
@@ -60,46 +67,47 @@ def set_coords(req):
         req.x,
         req.y,
         req.z,
-        # req.rx,
-        # req.ry,
-        # req.rz,
     ]
     sp = req.speed
-    mod = req.model
+    # mod = req.model
-
+    print('mira_services:',coords)
-    if mc:
+    if ma:
-        mc.send_coords(coords, sp, mod)
+        ma.set_coords(coords[0], coords[1], coords[2], sp)
 
     return SetCoordsResponse(True)
 
 
 def get_coords(req):
-    if mc:
+    if ma:
-        coords = mc.get_coords()
+        # coords = ma.get_coords_info()
-        return GetCoordsResponse(*coords)
+        coords = [0.0, 0.0, 0.0, 0.0]
+        if coords != None:
+            coords_data = coords[:3]
+            # print('coords:',coords)
+            return GetCoordsResponse(*coords_data)
 
 
 def switch_status(req):
     """Gripper switch,夹爪开关"""
-    if mc:
+    if ma:
         if req.Status:
-            mc.set_gripper_state(0, 80)
+            ma.set_gripper_state(0, 50)
         else:
-            mc.set_gripper_state(1, 80)
+            ma.set_gripper_state(1, 50)
 
     return GripperStatusResponse(True)
 
 
-def toggle_pump(req):
+# def toggle_pump(req):
-    if mc:
+#     if ma:
-        if req.Status:
+#         if req.Status:
-            mc.set_basic_output(req.Pin1, 0)
+#             ma.set_basic_output(req.Pin1, 0)
-            mc.set_basic_output(req.Pin2, 0)
+#             ma.set_basic_output(req.Pin2, 0)
-        else:
+#         else:
-            mc.set_basic_output(req.Pin1, 1)
+#             ma.set_basic_output(req.Pin1, 1)
-            mc.set_basic_output(req.Pin2, 1)
+#             ma.set_basic_output(req.Pin2, 1)
 
-    return PumpStatusResponse(True)
+#     return PumpStatusResponse(True)
 
 
 robot_msg = """
@@ -126,19 +134,20 @@ def output_robot_message():
     servo_temperature = "unknown"
     atom_version = "unknown"
 
-    if mc:
+    if ma:
-        cn = mc.is_controller_connected()
+        # cn = ma.is_controller_connected()
-        if cn == 1:
+        # if cn == 1:
         connect_status = True
-        time.sleep(0.1)
+        # time.sleep(0.1)
-        si = mc.is_all_servo_enable()
+        # si = ma.is_all_servo_enable()
-        if si == 1:
+        # if si == 1:
         servo_infomation = "all connected"
 
     print(
         robot_msg % (connect_status, servo_infomation,
                      servo_temperature, atom_version)
     )
+    time.sleep(2)
 
 
 if __name__ == "__main__":

Mira/mira_communication/srv/SetCoords.srv

@@ -6,7 +6,7 @@ float32 z
 # float32 rz
 
 int8 speed
-int8 model
+# int8 model
 
 ---