From c1e97e3fb16da8293b9f475cce28df81e1793386 Mon Sep 17 00:00:00 2001
From: weijian <weijian.wang@elephantrobotics.com>
Date: Wed, 31 Aug 2022 13:56:49 +0800
Subject: [PATCH] update mira ros1 moveit

---
 Mira/mira_moveit/launch/moveit.rviz   | 15 +++++-----
 Mira/mira_moveit/scripts/sync_plan.py | 40 +++++++++++++--------------
 2 files changed, 27 insertions(+), 28 deletions(-)

diff --git a/Mira/mira_moveit/launch/moveit.rviz b/Mira/mira_moveit/launch/moveit.rviz
index efe5c75..11eca8c 100644
--- a/Mira/mira_moveit/launch/moveit.rviz
+++ b/Mira/mira_moveit/launch/moveit.rviz
@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 84
+    Help Height: 70
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -10,9 +10,8 @@ Panels:
         - /MotionPlanning1/Planning Request1
         - /MotionPlanning1/Planning Metrics1
         - /MotionPlanning1/Planned Path1
-        - /MotionPlanning1/Planned Path1/Links1
       Splitter Ratio: 0.7425600290298462
-    Tree Height: 184
+    Tree Height: 186
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -49,7 +48,7 @@ Visualization Manager:
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
       Move Group Namespace: ""
-      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_Approximate_IK: true
       MoveIt_Allow_External_Program: false
       MoveIt_Allow_Replanning: false
       MoveIt_Allow_Sensor_Positioning: false
@@ -199,14 +198,14 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.14020325243473053
+      Pitch: 0.08979679644107819
       Target Frame: link1
-      Yaw: 2.3235831260681152
+      Yaw: 2.2435832023620605
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 888
+  Height: 876
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -215,7 +214,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000031efc0200000007fb000000100044006900730070006c006100790073010000003d00000149000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000018c000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001d3000001880000017d00ffffff0000053f0000031e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f300000312fc0200000007fb000000100044006900730070006c006100790073010000003d0000013d000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000180000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c7000001880000017d00ffffff0000053f0000031200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1848
diff --git a/Mira/mira_moveit/scripts/sync_plan.py b/Mira/mira_moveit/scripts/sync_plan.py
index 6b5b018..92cfe53 100644
--- a/Mira/mira_moveit/scripts/sync_plan.py
+++ b/Mira/mira_moveit/scripts/sync_plan.py
@@ -1,39 +1,39 @@
 #!/usr/bin/env python3
-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
 
 
-mc = None
+ma = None
 
 
 def callback(data):
-    rospy.loginfo(rospy.get_caller_id() + "%s", data)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+    # print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        # if index != 2:
-        #     value *= -1
-        data_list.append(value)
-    # print("data_list:",data_list)
-    mc.send_radians(data_list, 80)
+        data_list.append(round(value,3))
+    
+    print('data_list:',data_list)
+    ma.set_radians(data_list[0],data_list[1],data_list[2], 50)
+    # time.sleep(0.5)
 
 
 def listener():
-    global mc
-    rospy.init_node("mypal_reciver", anonymous=True)
-
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
-    baud = rospy.get_param("~baud", 115200)
-    print(port, baud)
-    mc = MyPalletizer(port, baud)
+    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)
+    ma = Mira(port, baud)
+    ma.power_on()
+    ma.go_zero()
 
-    # spin() simply keeps python from exiting until this node is stopped 
-    # spin()只是阻止python退出，直到该节点停止
+    # spin() simply keeps python from exiting until this node is stopped
+    # spin() 只是阻止python退出，直到该节点停止
+    print("spin ...")
     rospy.spin()