From 2a76f925df4bda6302e1b9713f8a393360820ef9 Mon Sep 17 00:00:00 2001
From: wangWking <842749351@qq.com>
Date: Wed, 27 Apr 2022 12:01:49 +0800
Subject: [PATCH] fix bug

---
 mecharm/mecharm_pi/scripts/follow_display.py  |  2 +-
 mecharm/mecharm_pi/scripts/listen_real.py     |  4 +--
 .../scripts/listen_real_of_topic.py           |  2 +-
 mecharm/mecharm_pi/scripts/teleop_keyboard.py | 29 ++++++++++++++-----
 4 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/mecharm/mecharm_pi/scripts/follow_display.py b/mecharm/mecharm_pi/scripts/follow_display.py
index eed0c18..c36e0f7 100644
--- a/mecharm/mecharm_pi/scripts/follow_display.py
+++ b/mecharm/mecharm_pi/scripts/follow_display.py
@@ -70,7 +70,7 @@ def talker():
         # rospy.loginfo('{}'.format(data_list))
         joint_state_send.position = data_list
         
-        data_list.insert(3,0.0)
+        # data_list.insert(3,0.0)
         print(joint_state_send.position)
 
         pub.publish(joint_state_send)
diff --git a/mecharm/mecharm_pi/scripts/listen_real.py b/mecharm/mecharm_pi/scripts/listen_real.py
index 617935e..9eb2919 100644
--- a/mecharm/mecharm_pi/scripts/listen_real.py
+++ b/mecharm/mecharm_pi/scripts/listen_real.py
@@ -7,9 +7,7 @@ import math
 import rospy
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Header
-# from mycobot_communication.srv import GetAngles
-from mypalletizer_communication.srv import GetAngles
-# from pymycobot.mypalletizer import MyPalletizer
+from mycobot_communication.srv import GetAngles
 
 
 def talker():
diff --git a/mecharm/mecharm_pi/scripts/listen_real_of_topic.py b/mecharm/mecharm_pi/scripts/listen_real_of_topic.py
index f316507..6ca18bc 100644
--- a/mecharm/mecharm_pi/scripts/listen_real_of_topic.py
+++ b/mecharm/mecharm_pi/scripts/listen_real_of_topic.py
@@ -6,7 +6,7 @@ import rospy
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Header
 from mycobot_communication.msg import MycobotAngles
-from mypalletizer_communication.msg import MypalAngles
+
  
 
 class Listener(object):
diff --git a/mecharm/mecharm_pi/scripts/teleop_keyboard.py b/mecharm/mecharm_pi/scripts/teleop_keyboard.py
index d48c91a..c2e03d4 100644
--- a/mecharm/mecharm_pi/scripts/teleop_keyboard.py
+++ b/mecharm/mecharm_pi/scripts/teleop_keyboard.py
@@ -15,14 +15,15 @@ import roslib
 msg = """\
 MyCobot Teleop Keyboard Controller
 ---------------------------
-Movimg options(control coordinations [x,y,z,rx]):
+Movimg options(control coordinations [x,y,z,rx,ry,rz]):
               w(x+)
 
     a(y-)     s(x-)     d(y+)
 
-        z(z-)       x(z+)
+    z(z-) x(z+)
 
-        u(rx+)      i(rx-)
+u(rx+)   i(ry+)   o(rz+)
+j(rx-)   k(ry-)   l(rz-)
    
 
 Gripper control:
@@ -82,8 +83,8 @@ def teleop_keyboard():
         exit(1)
 
 
-    init_pose = [0, 0,0, 0, speed]
-    home_pose = [0,30, 30, 0,speed]
+    init_pose = [0, 0, 0, 0, 0, 0, speed]
+    home_pose = [0, 8, -127, 40, 0, 0, speed]
 
     # rsp = set_angles(*init_pose)
 
@@ -93,7 +94,7 @@ def teleop_keyboard():
             break
         time.sleep(0.1)
 
-    record_coords = [res.x, res.y, res.z, res.rx, speed, model]
+    record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
 
     print(record_coords)
 
@@ -129,9 +130,21 @@ def teleop_keyboard():
                 elif key in ["u", "U"]:
                     record_coords[3] += change_angle
                     set_coords(*record_coords)
-                elif key in ["i", "I"]:
+                elif key in ["j", "J"]:
                     record_coords[3] -= change_angle
                     set_coords(*record_coords)
+                elif key in ["i", "I"]:
+                    record_coords[4] += change_angle
+                    set_coords(*record_coords)
+                elif key in ["k", "K"]:
+                    record_coords[4] -= change_angle
+                    set_coords(*record_coords)
+                elif key in ["o", "O"]:
+                    record_coords[5] += change_angle
+                    set_coords(*record_coords)
+                elif key in ["l", "L"]:
+                    record_coords[5] -= change_angle
+                    set_coords(*record_coords)
                 elif key in ["g", "G"]:
                     switch_gripper(True)
                 elif key in ["h", "H"]:
@@ -146,7 +159,7 @@ def teleop_keyboard():
                     home_pose[1] = rep.joint_2
                     home_pose[2] = rep.joint_3
                     home_pose[3] = rep.joint_4
-                    # home_pose[4] = rep.joint_5
+                    home_pose[4] = rep.joint_5
                 else:
                     continue