修复280AR-mega2560 ros使用

mycobot_280/mycobot_280arduino/config/mycobot_arduino.rviz

@@ -7,10 +7,9 @@ Panels:
         - /Global Options1
         - /Status1
         - /RobotModel1
-        - /RobotModel1/Status1
         - /TF1
       Splitter Ratio: 0.5
-    Tree Height: 645
+    Tree Height: 609
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -19,17 +18,18 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -41,7 +41,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -106,16 +106,40 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
-      Marker Scale: 1
+        joint1:
+          Value: true
+        joint2:
+          Value: true
+        joint3:
+          Value: true
+        joint4:
+          Value: true
+        joint5:
+          Value: true
+        joint6:
+          Value: true
+        joint6_flange:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.30000001192092896
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        joint1:
+          joint2:
+            joint3:
+              joint4:
+                joint5:
+                  joint6:
+                    joint6_flange:
+                      {}
       Update Interval: 0
       Value: true
   Enabled: true
@@ -133,7 +157,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -143,33 +170,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 5.27731943
+      Distance: 1.539350986480713
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
         X: 0
         Y: 0
         Z: 0
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.0500000007
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
-      Pitch: 0.785398006
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6003978252410889
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.785398006
+      Yaw: 5.470398426055908
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 926
+  Height: 906
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000018900000314fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000314000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000314fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000314000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000049b0000031400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000189000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ec000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000494000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -178,6 +205,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1855
-  X: 65
-  Y: 24
+  Width: 1848
+  X: 72
+  Y: 27

mycobot_280/mycobot_280arduino/scripts/follow_display.py

@@ -18,6 +18,7 @@ def talker():
     print("port: {}, baud: {}\n".format(port, baud))
     try:
         mycobot = MyCobot(port, baud)
+        time.sleep(2) # open serial need wait
     except Exception as e:
         print(e)
         print(
@@ -29,7 +30,7 @@ def talker():
         )
         exit(1)
     mycobot.release_all_servos()
-    time.sleep(0.1)
+    time.sleep(2)
     print("Rlease all servos over.\n")
 
     pub = rospy.Publisher("joint_states", JointState, queue_size=10)
@@ -56,44 +57,47 @@ def talker():
     marker_.ns = "my_namespace"
 
     print("publishing ...")
+
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
-
-        angles = mycobot.get_radians()
-        data_list = []
-        for index, value in enumerate(angles):
-            data_list.append(value)
-
-        # rospy.loginfo('{}'.format(data_list))
-        joint_state_send.position = data_list
-
-        pub.publish(joint_state_send)
-
-        coords = mycobot.get_coords()
-
-        # marker
-        marker_.header.stamp = rospy.Time.now()
-        marker_.type = marker_.SPHERE
-        marker_.action = marker_.ADD
-        marker_.scale.x = 0.04
-        marker_.scale.y = 0.04
-        marker_.scale.z = 0.04
-
-        # marker position initial
-        # print(coords)
-        if not coords:
-            coords = [0, 0, 0, 0, 0, 0]
-            rospy.loginfo("error [101]: can not get coord values")
-
-        marker_.pose.position.x = coords[1] / 1000 * -1
-        marker_.pose.position.y = coords[0] / 1000
-        marker_.pose.position.z = coords[2] / 1000
-
-        marker_.color.a = 1.0
-        marker_.color.g = 1.0
-        pub_marker.publish(marker_)
-
-        rate.sleep()
+        try:
+            angles = mycobot.get_radians()
+            data_list = []
+            for index, value in enumerate(angles):
+                data_list.append(value)
+
+            # rospy.loginfo('{}'.format(data_list))
+            joint_state_send.position = data_list
+
+            pub.publish(joint_state_send)
+
+            coords = mycobot.get_coords()
+
+            # marker
+            marker_.header.stamp = rospy.Time.now()
+            marker_.type = marker_.SPHERE
+            marker_.action = marker_.ADD
+            marker_.scale.x = 0.04
+            marker_.scale.y = 0.04
+            marker_.scale.z = 0.04
+
+            # marker position initial
+            # print(coords)
+            if not coords:
+                coords = [0, 0, 0, 0, 0, 0]
+                rospy.loginfo("error [101]: can not get coord values")
+
+            marker_.pose.position.x = coords[1] / 1000 * -1
+            marker_.pose.position.y = coords[0] / 1000
+            marker_.pose.position.z = coords[2] / 1000
+
+            marker_.color.a = 1.0
+            marker_.color.g = 1.0
+            pub_marker.publish(marker_)
+
+            rate.sleep()
+        except Exception as e:
+            pass
 
 
 if __name__ == "__main__":

mycobot_280/mycobot_280arduino/scripts/simple_gui.py

@@ -385,6 +385,7 @@ def get_coord_input(self):
         # print(c_value)
         try:
             self.set_coords(*c_value)
+            time.sleep(2)
         except ServiceException:
             pass
         self.show_j_date(c_value[:-2], "coord")
@@ -402,6 +403,7 @@ def get_joint_input(self):
 
         try:
             self.set_angles(*j_value)
+            time.sleep(2)
         except ServiceException:
             pass
         self.show_j_date(j_value[:-1])

mycobot_280/mycobot_280arduino/scripts/slider_control.py

@@ -20,15 +20,14 @@
 
 
 def callback(data):
-    # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
-    data_list = []
-    for index, value in enumerate(data.position):
-        radians_to_angles = round(math.degrees(value), 2)
-        data_list.append(radians_to_angles)
-
-    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
-    mc.send_angles(data_list, 25)
+    global latest_data
+    latest_data = [round(math.degrees(value), 2) for value in data.position]
+    rospy.loginfo(f"Joint angles: {latest_data}")
 
+def control_loop(event):
+    if latest_data:
+        rospy.loginfo(f"Sending angles: {latest_data}")
+        mc.send_angles(latest_data, 25)
 
 def listener():
     global mc
@@ -39,14 +38,14 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot(port, baud)
-    time.sleep(0.05)
-    # mc.set_fresh_mode(1)
-    time.sleep(0.05)
+    time.sleep(2) # open port,need wait
+
+    # 启动定时器，每0.5秒执行一次控制循环
+    rospy.Timer(rospy.Duration(0.5), control_loop)
 
-    # spin() simply keeps python from exiting until this node is stopped
-    print("spin ...")
     rospy.spin()
 
 
+
 if __name__ == "__main__":
     listener()

mycobot_280/mycobot_280arduino/scripts/teleop_keyboard.py

@@ -84,9 +84,10 @@ def teleop_keyboard():
     home_pose = [0, 8, -127, 40, 0, 0, speed]
 
     # rsp = set_angles(*init_pose)
-
     while True:
         res = get_coords()
+        time.sleep(1)
+        print('res:', res)
         if res.x > 1:
             break
         time.sleep(0.1)

mycobot_280/mycobot_280arduino/scripts/test.py

@@ -0,0 +1,40 @@
+import time
+from pymycobot.mycobot import MyCobot
+
+
+from pymycobot import *
+import time
+import datetime
+m = MyCobot('/dev/ttyUSB0', 115200)
+time.sleep(2)
+delay_time = 0.1
+run_delay_time = 1
+angles = [0,0,0,0,0,0]
+coords = [0,0,0,0,0,0]
+angle_loss_count = 0
+coord_loss_count = 0
+total_count = 0
+send_angles = [[0,0,0,0,0,0], [0,0,0,0,20,20]]
+sp = 50
+# m = MyCobot("com64", 115200)
+
+# time.sleep(2) #open port,need wait
+# print(m.get_radians())
+while 1:
+    for i in range(len(send_angles)) :
+        angles = m.get_angles()
+        if (angles is None):
+            angle_loss_count = angle_loss_count + 1
+            #angles = m.get_angles()
+        time.sleep(delay_time)
+        coords = m.get_coords()
+        if (coords is None):
+            coord_loss_count = coord_loss_count + 1
+            #coords = m.get_coords()
+        time.sleep(delay_time)
+        m.send_angles(send_angles[i], sp)
+        time.sleep(run_delay_time)
+        total_count = total_count + 1
+        now = datetime.datetime.now()
+        print(now, "angles, coords ", angles, coords, angle_loss_count, coord_loss_count, total_count)
+        time.sleep(delay_time)