From dc39836ce5e9251d1718662a5a4badaabe2174e7 Mon Sep 17 00:00:00 2001 From: weijian Date: Fri, 16 Sep 2022 14:26:42 +0800 Subject: [PATCH] fix and update --- Mira/mira/scripts/listen_real.py | 2 +- Mira/mira/scripts/listen_real_of_topic.py | 4 ++-- Mira/mira/scripts/simple_gui.py | 4 ++-- Mira/mira/scripts/slider_control.py | 5 ++-- Mira/mira/scripts/teleop_keyboard.py | 7 +++--- .../mira_communication/msg/MiraPumpStatus.msg | 4 ++-- Mira/mira_communication/msg/MiraSetCoords.msg | 2 +- .../scripts/mira_services.py | 8 +++---- .../mira_communication/scripts/mira_topics.py | 23 ++++++++++--------- Mira/mira_moveit/scripts/sync_plan.py | 2 +- 10 files changed, 30 insertions(+), 31 deletions(-) diff --git a/Mira/mira/scripts/listen_real.py b/Mira/mira/scripts/listen_real.py index 025c100..170e457 100755 --- a/Mira/mira/scripts/listen_real.py +++ b/Mira/mira/scripts/listen_real.py @@ -7,7 +7,7 @@ import rospy from sensor_msgs.msg import JointState from std_msgs.msg import Header from mira_communication.srv import GetAngles -# from pymycobot.mypalletizer import MyPalletizer + from pymycobot.mira import Mira diff --git a/Mira/mira/scripts/listen_real_of_topic.py b/Mira/mira/scripts/listen_real_of_topic.py index 3ecc02d..411acbb 100755 --- a/Mira/mira/scripts/listen_real_of_topic.py +++ b/Mira/mira/scripts/listen_real_of_topic.py @@ -35,12 +35,12 @@ class Listener(object): "joint1_to_base", "joint2_to_joint1", "joint3_to_joint2", - "joint4_to_joint3", + # "joint4_to_joint3", # "joint5_to_joint4", # "joint6_to_joint5", # "joint6output_to_joint6", ] - joint_state_send.velocity = [0] + joint_state_send.velocity = [0.0] joint_state_send.effort = [] joint_state_send.header.stamp = rospy.Time.now() diff --git a/Mira/mira/scripts/simple_gui.py b/Mira/mira/scripts/simple_gui.py index c063270..ddfe909 100755 --- a/Mira/mira/scripts/simple_gui.py +++ b/Mira/mira/scripts/simple_gui.py @@ -276,7 +276,7 @@ class Window: # c_value.append(self.model) # print(c_value) try: - self.set_coords(c_value[0], c_value[1], c_value[2], self.speed) + self.set_coords(c_value, self.speed) except ServiceException: pass self.show_j_date(c_value[:-2], "coord") @@ -294,7 +294,7 @@ class Window: j_value.append(self.speed) try: - self.set_angles(j_value[0], j_value[1], j_value[2], self.speed) + self.set_angles(j_value, self.speed) except ServiceException: pass self.show_j_date(j_value[:-1]) diff --git a/Mira/mira/scripts/slider_control.py b/Mira/mira/scripts/slider_control.py index 5c4d471..4a86c89 100755 --- a/Mira/mira/scripts/slider_control.py +++ b/Mira/mira/scripts/slider_control.py @@ -13,7 +13,6 @@ import time import rospy from sensor_msgs.msg import JointState -# from pymycobot.mycobot import MyCobot from pymycobot.mira import Mira import math @@ -28,7 +27,7 @@ def callback(data): data_list.append(round(value,3)) print('data_list:',data_list) - ma.set_radians(data_list[0],data_list[1],data_list[2], 80) + ma.set_radians(data_list, 80) # time.sleep(0.5) @@ -43,7 +42,7 @@ def listener(): ma = Mira(port, baud) ma.power_on() ma.go_zero() - + # spin() simply keeps python from exiting until this node is stopped # spin() 只是阻止python退出，直到该节点停止 print("spin ...") diff --git a/Mira/mira/scripts/teleop_keyboard.py b/Mira/mira/scripts/teleop_keyboard.py index b1f6be5..ab5ef0b 100755 --- a/Mira/mira/scripts/teleop_keyboard.py +++ b/Mira/mira/scripts/teleop_keyboard.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 from __future__ import print_function -# from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus from mira_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import sys @@ -92,7 +91,7 @@ def teleop_keyboard(): break time.sleep(0.1) - record_coords = [res.x, res.y, res.z, speed, model] + record_coords = [res.x, res.y, res.z, speed] print(record_coords) @@ -131,10 +130,10 @@ def teleop_keyboard(): switch_gripper(False) elif key == "1": rsp = set_angles(*init_pose) - record_coords = [res.x, res.y, res.z, speed, model] + record_coords = [res.x, res.y, res.z, speed] elif key in "2": rsp = set_angles(*home_pose) - record_coords = [res.x, res.y, res.z, speed, model] + record_coords = [res.x, res.y, res.z, speed] elif key in "3": rep = get_angles() home_pose[0] = rep.joint_1 diff --git a/Mira/mira_communication/msg/MiraPumpStatus.msg b/Mira/mira_communication/msg/MiraPumpStatus.msg index 2373241..f716601 100644 --- a/Mira/mira_communication/msg/MiraPumpStatus.msg +++ b/Mira/mira_communication/msg/MiraPumpStatus.msg @@ -1,3 +1,3 @@ bool Status -int8 Pin1 -int8 Pin2 +# int8 Pin1 +# int8 Pin2 diff --git a/Mira/mira_communication/msg/MiraSetCoords.msg b/Mira/mira_communication/msg/MiraSetCoords.msg index 6e66271..2aac661 100644 --- a/Mira/mira_communication/msg/MiraSetCoords.msg +++ b/Mira/mira_communication/msg/MiraSetCoords.msg @@ -6,4 +6,4 @@ float32 z # float32 rz int8 speed -int8 model +# int8 model diff --git a/Mira/mira_communication/scripts/mira_services.py b/Mira/mira_communication/scripts/mira_services.py index 26b9a61..7a2214f 100755 --- a/Mira/mira_communication/scripts/mira_services.py +++ b/Mira/mira_communication/scripts/mira_services.py @@ -47,7 +47,7 @@ def set_angles(req): sp = req.speed print('mira_services:',angles) if ma: - ma.set_angles(angles[0], angles[1], angles[2], sp) + ma.set_angles(angles, sp) return SetAnglesResponse(True) @@ -71,7 +71,7 @@ def set_coords(req): # mod = req.model print('mira_services:',coords) if ma: - ma.set_coords(coords[0], coords[1], coords[2], sp) + ma.set_coords(coords, sp) return SetCoordsResponse(True) @@ -89,9 +89,9 @@ def switch_status(req): """Gripper switch,夹爪开关""" if ma: if req.Status: - ma.set_gripper_state(0, 50) + ma.set_gripper_state(0) else: - ma.set_gripper_state(1, 50) + ma.set_gripper_state(1) return GripperStatusResponse(True) diff --git a/Mira/mira_communication/scripts/mira_topics.py b/Mira/mira_communication/scripts/mira_topics.py index ebcffe4..8a6b6f7 100755 --- a/Mira/mira_communication/scripts/mira_topics.py +++ b/Mira/mira_communication/scripts/mira_topics.py @@ -8,8 +8,9 @@ import threading import rospy from pymycobot.mypalletizer import MyPalletizer +from pymycobot.mira import Mira -from mira_communication import( +from mira_communication.msg import( MiraAngles, MiraCoords, MiraSetAngles, @@ -72,7 +73,7 @@ class MiraTopics(object): port = rospy.get_param("~port", "/dev/ttyUSB0") baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) - self.mc = MyPalletizer(port, baud) + self.mc = Mira(port, baud) self.lock = threading.Lock() def start(self): @@ -117,7 +118,7 @@ class MiraTopics(object): ma.joint_1 = angles[0] ma.joint_2 = angles[1] ma.joint_3 = angles[2] - ma.joint_4 = angles[3] + # ma.joint_4 = angles[3] # ma.joint_5 = angles[4] # ma.joint_6 = angles[5] pub.publish(ma) @@ -138,7 +139,7 @@ class MiraTopics(object): ma.x = coords[0] ma.y = coords[1] ma.z = coords[2] - ma.rx = coords[3] + # ma.rx = coords[3] # ma.ry = coords[4] # ma.rz = coords[5] pub.publish(ma) @@ -152,12 +153,12 @@ class MiraTopics(object): data.joint_1, data.joint_2, data.joint_3, - data.joint_4, + # data.joint_4, # data.joint_5, # data.joint_6, ] sp = int(data.speed) - self.mc.send_angles(angles, sp) + self.mc.set_angles(angles, sp) sub = rospy.Subscriber( "Mypal/angles_goal", MiraSetAngles, callback=callback @@ -167,11 +168,11 @@ class MiraTopics(object): def sub_set_coords(self): def callback(data): # angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] - angles = [data.x, data.y, data.z, data.rx] + angles = [data.x, data.y, data.z] sp = int(data.speed) - model = int(data.model) - self.mc.send_coords(angles, sp, model) + # model = int(data.model) + self.mc.set_coords(angles, sp) sub = rospy.Subscriber( "Mypal/coords_goal", MiraSetCoords, callback=callback @@ -183,9 +184,9 @@ class MiraTopics(object): """订阅夹爪状态""" def callback(data): if data.Status: - self.mc.set_gripper_state(0, 80) + self.mc.set_gripper_state(0) else: - self.mc.set_gripper_state(1, 80) + self.mc.set_gripper_state(1) sub = rospy.Subscriber( "Mypal/gripper_status", MiraGripperStatus, callback=callback diff --git a/Mira/mira_moveit/scripts/sync_plan.py b/Mira/mira_moveit/scripts/sync_plan.py index 92cfe53..cc502ef 100644 --- a/Mira/mira_moveit/scripts/sync_plan.py +++ b/Mira/mira_moveit/scripts/sync_plan.py @@ -15,7 +15,7 @@ def callback(data): data_list.append(round(value,3)) print('data_list:',data_list) - ma.set_radians(data_list[0],data_list[1],data_list[2], 50) + ma.set_radians(data_list, 50) # time.sleep(0.5)