From 9019f6362a3db4233f02d7fe27bebb83547d1984 Mon Sep 17 00:00:00 2001 From: weijian Date: Wed, 31 Aug 2022 14:01:29 +0800 Subject: [PATCH] update mira ros --- Mira/mira/CMakeLists.txt | 1 - Mira/mira/config/mira.rviz | 10 +- Mira/mira/launch/follow.launch | 14 --- Mira/mira/scripts/follow_display.py | 109 ------------------ Mira/mira/scripts/listen_real.py | 9 +- Mira/mira/scripts/simple_gui.py | 71 +++++++----- Mira/mira/scripts/slider_control.py | 21 ++-- .../scripts/mira_services.py | 99 ++++++++-------- .../mira_communication/scripts/mira_topics.py | 0 .../scripts/mira_topics_seeed.py | 0 Mira/mira_communication/srv/SetCoords.srv | 2 +- 11 files changed, 116 insertions(+), 220 deletions(-) delete mode 100644 Mira/mira/launch/follow.launch delete mode 100644 Mira/mira/scripts/follow_display.py mode change 100644 => 100755 Mira/mira/scripts/listen_real.py mode change 100644 => 100755 Mira/mira/scripts/simple_gui.py mode change 100644 => 100755 Mira/mira/scripts/slider_control.py mode change 100644 => 100755 Mira/mira_communication/scripts/mira_services.py mode change 100644 => 100755 Mira/mira_communication/scripts/mira_topics.py mode change 100644 => 100755 Mira/mira_communication/scripts/mira_topics_seeed.py diff --git a/Mira/mira/CMakeLists.txt b/Mira/mira/CMakeLists.txt index 0ea2acf..c3a95b8 100644 --- a/Mira/mira/CMakeLists.txt +++ b/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 diff --git a/Mira/mira/config/mira.rviz b/Mira/mira/config/mira.rviz index 6d8bf8b..4b0cfd1 100644 --- a/Mira/mira/config/mira.rviz +++ b/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: - 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: diff --git a/Mira/mira/launch/follow.launch b/Mira/mira/launch/follow.launch deleted file mode 100644 index 85249ad..0000000 --- a/Mira/mira/launch/follow.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - ["joint_states"] - - - - diff --git a/Mira/mira/scripts/follow_display.py b/Mira/mira/scripts/follow_display.py deleted file mode 100644 index c69828d..0000000 --- a/Mira/mira/scripts/follow_display.py +++ /dev/null @@ -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 diff --git a/Mira/mira/scripts/listen_real.py b/Mira/mira/scripts/listen_real.py old mode 100644 new mode 100755 index 0bb3ad6..025c100 --- a/Mira/mira/scripts/listen_real.py +++ b/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,9 +28,9 @@ 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. rospy.loginfo("wait service") rospy.wait_for_service("get_joint_angles") @@ -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 diff --git a/Mira/mira/scripts/simple_gui.py b/Mira/mira/scripts/simple_gui.py old mode 100644 new mode 100755 index ba3d5be..fcf0155 --- a/Mira/mira/scripts/simple_gui.py +++ b/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.res_angles = [0, 0, 0, self.speed,] + self.record_coords = [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( - "switch_gripper_status", GripperStatus - ) + # self.switch_gripper = rospy.ServiceProxy( + # "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_c = [self.x, self.y, self.z,] + self.all_j = [self.J_1, self.J_2, self.J_3] + 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() - if self.angles.joint_1 > 1: + self.angles = self.get_angles_info() + # if self.angles.joint_1 > 1: + if self.angles != []: break time.sleep(0.1) # print(self.angles.joint_1) - self.record_coords = [ - round(self.res.x, 2), - round(self.res.y, 2), - round(self.res.z, 2), - self.speed, - ] - 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('angles:',self.res_angles) + 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.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('angles:',self.res_angles) # def send_input(self,dates): def show_j_date(self, date, way=""): diff --git a/Mira/mira/scripts/slider_control.py b/Mira/mira/scripts/slider_control.py old mode 100644 new mode 100755 index ddd5201..6660b73 --- a/Mira/mira/scripts/slider_control.py +++ b/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 - -mc = None +ma = 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) - - # print(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 + 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退出，直到该节点停止 diff --git a/Mira/mira_communication/scripts/mira_services.py b/Mira/mira_communication/scripts/mira_services.py old mode 100644 new mode 100755 index 08d4558..6ab7e30 --- a/Mira/mira_communication/scripts/mira_services.py +++ b/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.mypalletizer import MyPalletizer +from pymycobot.mira import Mira -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 - - if mc: - mc.send_angles(angles, sp) + print('mira_services:',angles) + if ma: + ma.set_angles(angles[0], angles[1], angles[2], sp) return SetAnglesResponse(True) def get_angles(req): - if mc: - angles = mc.get_angles() - return GetAnglesResponse(*angles) + if ma: + angles = ma.get_angles_info() + # 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 - - if mc: - mc.send_coords(coords, sp, mod) + # mod = req.model + print('mira_services:',coords) + if ma: + ma.set_coords(coords[0], coords[1], coords[2], sp) return SetCoordsResponse(True) def get_coords(req): - if mc: - coords = mc.get_coords() - return GetCoordsResponse(*coords) + if ma: + # coords = ma.get_coords_info() + 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): - if mc: - if req.Status: - mc.set_basic_output(req.Pin1, 0) - mc.set_basic_output(req.Pin2, 0) - else: - mc.set_basic_output(req.Pin1, 1) - mc.set_basic_output(req.Pin2, 1) +# def toggle_pump(req): +# if ma: +# if req.Status: +# ma.set_basic_output(req.Pin1, 0) +# ma.set_basic_output(req.Pin2, 0) +# else: +# ma.set_basic_output(req.Pin1, 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: - cn = mc.is_controller_connected() - if cn == 1: - connect_status = True - time.sleep(0.1) - si = mc.is_all_servo_enable() - if si == 1: - servo_infomation = "all connected" + if ma: + # cn = ma.is_controller_connected() + # if cn == 1: + connect_status = True + # time.sleep(0.1) + # si = ma.is_all_servo_enable() + # if si == 1: + servo_infomation = "all connected" print( robot_msg % (connect_status, servo_infomation, servo_temperature, atom_version) ) + time.sleep(2) if __name__ == "__main__": diff --git a/Mira/mira_communication/scripts/mira_topics.py b/Mira/mira_communication/scripts/mira_topics.py old mode 100644 new mode 100755 diff --git a/Mira/mira_communication/scripts/mira_topics_seeed.py b/Mira/mira_communication/scripts/mira_topics_seeed.py old mode 100644 new mode 100755 diff --git a/Mira/mira_communication/srv/SetCoords.srv b/Mira/mira_communication/srv/SetCoords.srv index 8052408..7ae32a3 100644 --- a/Mira/mira_communication/srv/SetCoords.srv +++ b/Mira/mira_communication/srv/SetCoords.srv @@ -6,7 +6,7 @@ float32 z # float32 rz int8 speed -int8 model +# int8 model ---