diff --git a/Mira/mira/scripts/listen_real.py b/Mira/mira/scripts/listen_real.py index 170e457..688641b 100755 --- a/Mira/mira/scripts/listen_real.py +++ b/Mira/mira/scripts/listen_real.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# license removed for brevity +# -*- coding: utf-8 -*- import time import math @@ -8,8 +8,6 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header from mira_communication.srv import GetAngles -from pymycobot.mira import Mira - def talker(): rospy.loginfo("start ...") @@ -25,35 +23,39 @@ def talker(): "joint1_to_base", "joint2_to_joint1", "joint3_to_joint2", - # "joint4_to_joint3", - # "joint5_to_joint4", ] - joint_state_send.velocity = [0.0] + joint_state_send.velocity = [0] joint_state_send.effort = [] # waiting util server `get_joint_angles` enable. rospy.loginfo("wait service") rospy.wait_for_service("get_joint_angles") - func = rospy.ServiceProxy("get_joint_angles", GetAngles) - + # func = rospy.ServiceProxy("get_joint_angles", GetAngles) + while True: + try: + func = rospy.ServiceProxy("get_joint_angles", GetAngles) + break + except rospy.ServiceException as e: + print('service_error:', e) + + rospy.loginfo("start loop ...") while not rospy.is_shutdown(): # get real angles from server. # 从服务获取真实的角度 - + res = func() - if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: + + if (res == None) or (res.joint_1 == res.joint_2 == res.joint_3 == 0.0): continue + radians_list = [ - res.joint_1 * (math.pi / 180), - res.joint_2 * (math.pi / 180), - res.joint_3 * (math.pi / 180), - 0, - # res.joint_4 * (math.pi / 180), - + round(res.joint_1 * (math.pi / 180), 2), + round(res.joint_2 * (math.pi / 180), 2), + round(res.joint_3 * (math.pi / 180), 2), ] 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 index a7dcfe4..12ca53c 100755 --- a/Mira/mira/scripts/simple_gui.py +++ b/Mira/mira/scripts/simple_gui.py @@ -11,7 +11,7 @@ class Window: def __init__(self, handle): self.win = handle self.win.resizable(0, 0) # fixed window size 固定窗口大小 - + # self.model = 0 self.speed = rospy.get_param("~speed", 50) @@ -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.0, 0.0, self.speed] + self.res_angles = [0.0, 0.0, 0.0, self.speed] self.get_date() # get screen width and height. 获取屏幕宽度和高度 @@ -52,12 +52,12 @@ class Window: ) # Gripper Switch. 夹爪开关按钮 - # tk.Button(self.frmLB, text="Gripper Open", command=self.gripper_open, width=10).grid( - # row=1, column=0, sticky="w", padx=3, pady=50 - # ) - # tk.Button(self.frmLB, text="Gripper Close", command=self.gripper_close, width=10).grid( - # row=1, column=1, sticky="w", padx=3, pady=2 - # ) + tk.Button(self.frmLB, text="Gripper Open", command=self.gripper_open, width=10).grid( + row=1, column=0, sticky="w", padx=3, pady=50 + ) + tk.Button(self.frmLB, text="Gripper Close", command=self.gripper_close, width=10).grid( + row=1, column=1, sticky="w", padx=3, pady=2 + ) def connect_ser(self): rospy.init_node("simple_gui", anonymous=True, disable_signals=True) @@ -72,9 +72,9 @@ class Window: self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords) 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) @@ -97,12 +97,10 @@ class Window: tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) tk.Label(self.frmLT, text="Joint 3 ").grid(row=2) - tk.Label(self.frmRT, text=" x ").grid(row=0) tk.Label(self.frmRT, text=" y ").grid(row=1) tk.Label(self.frmRT, text=" z ").grid(row=2) - # Set the default value of the input box. # 设置输入框的默认值 self.j1_default = tk.StringVar() @@ -112,8 +110,6 @@ class Window: self.j3_default = tk.StringVar() self.j3_default.set(self.res_angles[2]) - - self.x_default = tk.StringVar() self.x_default.set(self.record_coords[0]) self.y_default = tk.StringVar() @@ -121,7 +117,6 @@ class Window: self.z_default = tk.StringVar() self.z_default.set(self.record_coords[2]) - # joint input box. 输入框 self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default) self.J_1.grid(row=0, column=1, pady=3) @@ -129,8 +124,6 @@ class Window: self.J_2.grid(row=1, column=1, pady=3) self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default) self.J_3.grid(row=2, column=1, pady=3) - - # coord input box. 输入框 self.x = tk.Entry(self.frmRT, textvariable=self.x_default) @@ -139,15 +132,11 @@ class Window: self.y.grid(row=1, column=1, pady=3) self.z = tk.Entry(self.frmRT, textvariable=self.z_default) self.z.grid(row=2, column=1, pady=3) - - # 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] - - # speed input box. 速度输入框 tk.Label( self.frmLB, @@ -169,7 +158,6 @@ class Window: self.cont_3 = tk.StringVar(self.frmLC) self.cont_3.set(str(self.res_angles[2]) + "°") - self.cont_all = [ self.cont_1, self.cont_2, @@ -221,7 +209,6 @@ class Window: self.coord_z = tk.StringVar() self.coord_z.set(str(self.record_coords[2])) - self.coord_all = [ self.coord_x, self.coord_y, @@ -261,6 +248,20 @@ class Window: tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid( row=i, column=5 ) + + def gripper_open(self): + try: + self.switch_gripper(True) + except ServiceException: + # Probably because the method has no return value, the service throws an unhandled error + # 可能由于该方法没有返回值,服务抛出无法处理的错误 + pass + + def gripper_close(self): + try: + self.switch_gripper(False) + except ServiceException: + pass def get_coord_input(self): # Get the data input by coord and send it to the robotic arm @@ -302,7 +303,7 @@ class Window: def get_date(self): # Get the data of robotic arm for display. 拿机械臂的数据,用于展示 t = time.time() - while time.time() - t < 2: + while time.time() - t < 3: self.res = self.get_coords_info() print(self.res) @@ -313,31 +314,26 @@ class Window: time.sleep(0.1) t = time.time() - while time.time() - t < 2: + while time.time() - t < 3: 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) - 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), + if self.res and self.angles != None: + self.record_coords = [ + round(self.res.x, 2), + round(self.res.y, 2), + round(self.res.z, 2), + self.speed, + ] - ] - # print('coord:',self.record_coords) - # print('angles:',self.res_angles) + self.res_angles = [ + round(self.angles.joint_1, 2), + round(self.angles.joint_2, 2), + round(self.angles.joint_3, 2), + ] # def send_input(self,dates): def show_j_date(self, date, way=""): @@ -361,10 +357,9 @@ class Window: else: raise - def main(): window = tk.Tk() - window.title("mycobot ros GUI") + window.title("Mira ros GUI") Window(window).run() diff --git a/Mira/mira_communication/launch/communication_topic.launch b/Mira/mira_communication/launch/communication_topic.launch index 8e6b076..f5cdad9 100644 --- a/Mira/mira_communication/launch/communication_topic.launch +++ b/Mira/mira_communication/launch/communication_topic.launch @@ -4,7 +4,7 @@ - + diff --git a/Mira/mira_communication/msg/MiraAngles.msg b/Mira/mira_communication/msg/MiraAngles.msg index 0fccc03..d8cf687 100644 --- a/Mira/mira_communication/msg/MiraAngles.msg +++ b/Mira/mira_communication/msg/MiraAngles.msg @@ -1,7 +1,3 @@ -# float32 base float32 joint_1 float32 joint_2 float32 joint_3 -# float32 joint_4 -# float32 joint_5 -# float32 joint_6 diff --git a/Mira/mira_communication/msg/MiraCoords.msg b/Mira/mira_communication/msg/MiraCoords.msg index 2d11f66..0fb5429 100644 --- a/Mira/mira_communication/msg/MiraCoords.msg +++ b/Mira/mira_communication/msg/MiraCoords.msg @@ -1,6 +1,3 @@ float32 x float32 y -float32 z -# float32 rx -# float32 ry -# float32 rz \ No newline at end of file +float32 z \ No newline at end of file diff --git a/Mira/mira_communication/msg/MiraPumpStatus.msg b/Mira/mira_communication/msg/MiraPumpStatus.msg index f716601..2ac4e02 100644 --- a/Mira/mira_communication/msg/MiraPumpStatus.msg +++ b/Mira/mira_communication/msg/MiraPumpStatus.msg @@ -1,3 +1,2 @@ bool Status -# int8 Pin1 -# int8 Pin2 + diff --git a/Mira/mira_communication/msg/MiraSetAngles.msg b/Mira/mira_communication/msg/MiraSetAngles.msg index cab304e..e8c86f1 100644 --- a/Mira/mira_communication/msg/MiraSetAngles.msg +++ b/Mira/mira_communication/msg/MiraSetAngles.msg @@ -1,9 +1,5 @@ -# float32 base float32 joint_1 float32 joint_2 float32 joint_3 -# float32 joint_4 -# float32 joint_5 -# float32 joint_6 int8 speed diff --git a/Mira/mira_communication/msg/MiraSetCoords.msg b/Mira/mira_communication/msg/MiraSetCoords.msg index 2aac661..3df33ec 100644 --- a/Mira/mira_communication/msg/MiraSetCoords.msg +++ b/Mira/mira_communication/msg/MiraSetCoords.msg @@ -1,9 +1,6 @@ float32 x float32 y float32 z -# float32 rx -# float32 ry -# float32 rz int8 speed -# int8 model + diff --git a/Mira/mira_communication/scripts/mira_services.py b/Mira/mira_communication/scripts/mira_services.py index 7a2214f..23598f5 100755 --- a/Mira/mira_communication/scripts/mira_services.py +++ b/Mira/mira_communication/scripts/mira_services.py @@ -2,9 +2,7 @@ # -*- coding: utf-8 -*- import time import rospy -# from mycobot_communication.srv import * from mira_communication.srv import * - from pymycobot.mira import Mira ma = None @@ -23,8 +21,7 @@ def create_handle(): ma.power_on() # calibrate the zero position of the robot arm ma.go_zero() - - time.sleep(5) + # time.sleep(3) def create_services(): @@ -33,19 +30,19 @@ 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() def set_angles(req): + """set angles""" angles = [ req.joint_1, req.joint_2, req.joint_3, ] sp = req.speed - print('mira_services:',angles) if ma: ma.set_angles(angles, sp) @@ -53,12 +50,16 @@ def set_angles(req): def get_angles(req): - if ma: - angles = ma.get_angles_info() - # angles = [0.0, 0.0, 0.0, 0.0] - if angles != None: - # print('angles:',angles) - return GetAnglesResponse(*angles) + count = 0 + while count < 10: + if ma: + angles = ma.get_angles_info() + if angles != None: + return GetAnglesResponse(*angles) + count += 1 + continue + else: + return GetAnglesResponse(0.0, 0.0, 0.0) def set_coords(req): @@ -68,8 +69,6 @@ def set_coords(req): req.z, ] sp = req.speed - # mod = req.model - print('mira_services:',coords) if ma: ma.set_coords(coords, sp) @@ -77,12 +76,17 @@ def set_coords(req): def get_coords(req): - if ma: - # coords = ma.get_coords_info() - coords = [0.0, 0.0, 0.0] - if coords != None: - # print('coords:',coords) - return GetCoordsResponse(*coords) + count = 0 + while count < 10: + if ma: + coords = ma.get_coords_info() + # coords = [176.0, 0.0, 120.0] + if coords != None: + return GetCoordsResponse(*coords) + count += 1 + continue + else: + return GetCoordsResponse(176.0, 0.0, 120.0) def switch_status(req): @@ -96,16 +100,14 @@ def switch_status(req): return GripperStatusResponse(True) -# 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) +def toggle_pump(req): + if ma: + if req.Status: + ma.set_gpio_state(1) + else: + ma.set_gpio_state(0) -# return PumpStatusResponse(True) + return PumpStatusResponse(True) robot_msg = """ @@ -116,13 +118,16 @@ Joint Limit: joint 2: 0 ~ 90 joint 3: 0 ~ 75 +Coords Limit: + x: 0 ~ 270 + y: 0 ~ 270 + z: 0 ~ 125 + Connect Status: %s Servo Infomation: %s Servo Temperature: %s - -Atom Version: %s """ @@ -130,22 +135,15 @@ def output_robot_message(): connect_status = False servo_infomation = "unknown" servo_temperature = "unknown" - atom_version = "unknown" 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) + servo_temperature) ) - time.sleep(2) if __name__ == "__main__": diff --git a/Mira/mira_communication/scripts/mira_topics.py b/Mira/mira_communication/scripts/mira_topics.py index 8a6b6f7..3147f7d 100755 --- a/Mira/mira_communication/scripts/mira_topics.py +++ b/Mira/mira_communication/scripts/mira_topics.py @@ -74,6 +74,7 @@ class MiraTopics(object): baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) self.mc = Mira(port, baud) + self.mc.go_zero() self.lock = threading.Lock() def start(self): @@ -112,7 +113,7 @@ class MiraTopics(object): ma = MiraAngles() while not rospy.is_shutdown(): self.lock.acquire() - angles = self.mc.get_angles() + angles = self.mc.get_angles_info() self.lock.release() if angles: ma.joint_1 = angles[0] @@ -133,7 +134,7 @@ class MiraTopics(object): while not rospy.is_shutdown(): self.lock.acquire() - coords = self.mc.get_coords() + coords = self.mc.get_coords_info() self.lock.release() if coords: ma.x = coords[0] @@ -196,11 +197,9 @@ class MiraTopics(object): def sub_pump_status(self): def callback(data): if data.Status: - self.mc.set_basic_output(data.Pin1, 0) - self.mc.set_basic_output(data.Pin2, 0) + self.mc.set_gpio_state(0) else: - self.mc.set_basic_output(data.Pin1, 1) - self.mc.set_basic_output(data.Pin2, 1) + self.mc.set_gpio_state(1) sub = rospy.Subscriber( "Mypal/pump_status", MiraPumpStatus, callback=callback diff --git a/Mira/mira_communication/scripts/mira_topics_seeed.py b/Mira/mira_communication/scripts/mira_topics_seeed.py index fa40b10..09a7607 100755 --- a/Mira/mira_communication/scripts/mira_topics_seeed.py +++ b/Mira/mira_communication/scripts/mira_topics_seeed.py @@ -8,7 +8,7 @@ import threading import rospy -from mira_communication import ( +from mira_communication.msg import ( MiraAngles, MiraCoords, MiraSetAngles, @@ -16,7 +16,7 @@ from mira_communication import ( MiraGripperStatus, MiraPumpStatus, ) -from pymycobot.mypalletizer import Mypalletizer +from pymycobot.mira import Mira class Watcher: @@ -73,7 +73,8 @@ 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.mc.go_zero() self.lock = threading.Lock() def start(self): diff --git a/Mira/mira_communication/srv/GetAngles.srv b/Mira/mira_communication/srv/GetAngles.srv index 135c6d1..89a607d 100644 --- a/Mira/mira_communication/srv/GetAngles.srv +++ b/Mira/mira_communication/srv/GetAngles.srv @@ -1,11 +1,9 @@ --- -# float32 base + float32 joint_1 float32 joint_2 float32 joint_3 -# float32 joint_4 -# float32 joint_5 -# float32 joint_6 + diff --git a/Mira/mira_communication/srv/GetCoords.srv b/Mira/mira_communication/srv/GetCoords.srv index c1121c2..f0e6e5e 100644 --- a/Mira/mira_communication/srv/GetCoords.srv +++ b/Mira/mira_communication/srv/GetCoords.srv @@ -4,6 +4,3 @@ float32 x float32 y float32 z -# float32 rx -# float32 ry -# float32 rz \ No newline at end of file diff --git a/Mira/mira_communication/srv/PumpStatus.srv b/Mira/mira_communication/srv/PumpStatus.srv index 8bd4b53..3887d75 100644 --- a/Mira/mira_communication/srv/PumpStatus.srv +++ b/Mira/mira_communication/srv/PumpStatus.srv @@ -1,6 +1,4 @@ bool Status -int8 Pin1 -int8 Pin2 --- diff --git a/Mira/mira_communication/srv/SetAngles.srv b/Mira/mira_communication/srv/SetAngles.srv index 11bb277..39c7074 100644 --- a/Mira/mira_communication/srv/SetAngles.srv +++ b/Mira/mira_communication/srv/SetAngles.srv @@ -1,12 +1,6 @@ -# float32 base float32 joint_1 float32 joint_2 float32 joint_3 -# float32 joint_4 -# float32 joint_5 -# float32 joint_6 - - int8 speed diff --git a/Mira/mira_communication/srv/SetCoords.srv b/Mira/mira_communication/srv/SetCoords.srv index 7ae32a3..3bb59ea 100644 --- a/Mira/mira_communication/srv/SetCoords.srv +++ b/Mira/mira_communication/srv/SetCoords.srv @@ -1,12 +1,8 @@ float32 x float32 y float32 z -# float32 rx -# float32 ry -# float32 rz int8 speed -# int8 model ---