diff --git a/mypalletizer_260/launch/detect_marker.launch b/mypalletizer_260/launch/detect_marker.launch index dec163d..e8c6998 100644 --- a/mypalletizer_260/launch/detect_marker.launch +++ b/mypalletizer_260/launch/detect_marker.launch @@ -1,4 +1,5 @@ + @@ -10,6 +11,7 @@ + diff --git a/mypalletizer_260/launch/detect_marker_with_topic.launch b/mypalletizer_260/launch/detect_marker_with_topic.launch index 3acedc6..75c61b8 100644 --- a/mypalletizer_260/launch/detect_marker_with_topic.launch +++ b/mypalletizer_260/launch/detect_marker_with_topic.launch @@ -1,7 +1,9 @@ + - + + @@ -10,17 +12,17 @@ - + - + - + diff --git a/mypalletizer_260/launch/follow.launch b/mypalletizer_260/launch/follow.launch index 56fdccc..56b0dc3 100644 --- a/mypalletizer_260/launch/follow.launch +++ b/mypalletizer_260/launch/follow.launch @@ -1,11 +1,14 @@ + + ["joint_states"] + diff --git a/mypalletizer_260/launch/simple_gui.launch b/mypalletizer_260/launch/simple_gui.launch index b9750ac..e369c2e 100644 --- a/mypalletizer_260/launch/simple_gui.launch +++ b/mypalletizer_260/launch/simple_gui.launch @@ -1,22 +1,24 @@ + - + - + - + + diff --git a/mypalletizer_260/launch/slider_control.launch b/mypalletizer_260/launch/slider_control.launch index 180916d..9e23a16 100644 --- a/mypalletizer_260/launch/slider_control.launch +++ b/mypalletizer_260/launch/slider_control.launch @@ -1,14 +1,15 @@ - + + - + @@ -19,6 +20,6 @@ --> - + diff --git a/mypalletizer_260/launch/teleop_keyboard.launch b/mypalletizer_260/launch/teleop_keyboard.launch index df04a44..802e6b6 100644 --- a/mypalletizer_260/launch/teleop_keyboard.launch +++ b/mypalletizer_260/launch/teleop_keyboard.launch @@ -1,7 +1,9 @@ + + @@ -10,14 +12,17 @@ - + + + + diff --git a/mypalletizer_260/launch/test.launch b/mypalletizer_260/launch/test.launch index 3f79219..1c46c5a 100644 --- a/mypalletizer_260/launch/test.launch +++ b/mypalletizer_260/launch/test.launch @@ -1,16 +1,17 @@ + - + - + diff --git a/mypalletizer_260/scripts/detect_marker.py b/mypalletizer_260/scripts/detect_marker.py index 04ca483..cae6db2 100755 --- a/mypalletizer_260/scripts/detect_marker.py +++ b/mypalletizer_260/scripts/detect_marker.py @@ -27,7 +27,7 @@ class ImageConverter: ) self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() self.camera_matrix = None - # subscriber, listen wether has img come in. + # subscriber, listen wether has img come in. 订阅者，监听是否有img进来 self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) def callback(self, data): @@ -37,7 +37,7 @@ class ImageConverter: pose to transforming. """ try: - # trans `rgb` to `gbr` for opencv. + # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) @@ -45,7 +45,7 @@ class ImageConverter: focal_length = size[1] center = [size[1] / 2, size[0] / 2] if self.camera_matrix is None: - # calc the camera matrix, if don't have. + # calc the camera matrix, if don't have. 如果没有就计算相机矩阵 self.camera_matrix = np.array( [ [focal_length, 0, center[0]], @@ -55,7 +55,7 @@ class ImageConverter: dtype=np.float32, ) gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. + # detect aruco marker. 检测 aruco 标记。 ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) corners, ids = ret[0], ret[1] # process marker data. @@ -63,10 +63,10 @@ class ImageConverter: if ids is not None: # print('corners:', corners, 'ids:', ids) - # detect marker pose. + # detect marker pose. 检测标记姿势 # argument: - # marker corners - # marker size (meter) + # marker corners, 标记角 + # marker size (meter), 标记尺寸（米） ret = cv.aruco.estimatePoseSingleMarkers( corners, 0.05, self.camera_matrix, self.dist_coeffs ) @@ -75,7 +75,7 @@ class ImageConverter: print("rvec:", rvec, "tvec:", tvec) - # just select first one detected marker. + # just select first one detected marker. 只需选择第一个检测到的标记 for i in range(rvec.shape[0]): cv.aruco.drawDetectedMarkers(cv_image, corners) cv.aruco.drawAxis( @@ -90,14 +90,15 @@ class ImageConverter: xyz = tvec[0, 0, :] xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - # get quaternion for ros. + # get quaternion for ros. 为ros获取四元数 euler = rvec[0, 0, :] tf_change = tf.transformations.quaternion_from_euler( euler[0], euler[1], euler[2] ) print("tf_change:", tf_change) - # trans pose according [joint1] + # trans pose according [joint1] + # 根据 [joint1] 变换姿势 self.br.sendTransform( xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" ) diff --git a/mypalletizer_260/scripts/follow_and_pump.py b/mypalletizer_260/scripts/follow_and_pump.py index f26b669..f62112b 100755 --- a/mypalletizer_260/scripts/follow_and_pump.py +++ b/mypalletizer_260/scripts/follow_and_pump.py @@ -5,18 +5,21 @@ from visualization_msgs.msg import Marker import time import os +# Type of message to communicate with mycobot # 与 mycobot 通信的消息类型 from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus rospy.init_node("gipper_subscriber", anonymous=True) -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 +# Control the topic of mycobot, followed by angle, coordinates, gripper +# 控制mycobot的话题，依次是角度、坐标、夹爪 angle_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=5) coord_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=5) -# Judging equipment: ttyUSB* is M5；ttyACM* is wio +# Judging device: ttyUSB* is M5；ttyACM* is wio +# 判断设备：ttyUSB*为M5；ttyACM*为wio robot = os.popen("ls /dev/ttyUSB*").readline() if "dev" in robot: @@ -27,27 +30,30 @@ else: pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=5) -# 实例化消息对象 +# Instantiate the message object. 实例化消息对象 angles = MycobotSetAngles() coords = MycobotSetCoords() pump = MycobotPumpStatus() -# 与 mycobot 真实位置的偏差值 +# Deviation value from the real position of mycobot +# 与mycobot真实位置的偏差值 x_offset = -20 y_offset = 20 z_offset = 110 -# 通过该变量限制，抓取行为只做一次 +# With this variable limit, the fetching behavior is only done once +# 使用此变量限制，抓取行为仅执行一次 flag = False -# 为了后面比较二维码是否移动 +# In order to compare whether the QR code moves later +# 为了比较二维码后面是否移动 temp_x = temp_y = temp_z = 0.0 temp_time = time.time() def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """发布坐标""" + """Post coordinates 发布坐标""" coords.x = x coords.y = y coords.z = z @@ -61,7 +67,7 @@ def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): def pub_angles(a, b, c, d, e, f, sp): - """发布角度""" + """Publishing angle. 发布角度""" angles.joint_1 = float(a) angles.joint_2 = float(b) angles.joint_3 = float(c) @@ -73,7 +79,7 @@ def pub_angles(a, b, c, d, e, f, sp): def pub_pump(flag, Pin): - """发布夹爪状态""" + """Publish gripper status. 发布夹爪状态""" pump.Status = flag pump.Pin1 = Pin[0] pump.Pin2 = Pin[1] @@ -81,7 +87,7 @@ def pub_pump(flag, Pin): def target_is_moving(x, y, z): - """判断目标是否移动""" + """Determine whether the target moves. 判断目标是否移动""" count = 0 for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): print(o, n) @@ -94,18 +100,19 @@ def target_is_moving(x, y, z): def grippercallback(data): - """回调函数""" + """callback function. 回调函数""" global flag, temp_x, temp_y, temp_z # rospy.loginfo('gripper_subscriber get date :%s', data) if flag: return - # 解析出坐标值 + # Parse out the coordinate value. 解析出坐标值 # pump length: 88mm x = float(format(data.pose.position.x * 1000, ".2f")) y = float(format(data.pose.position.y * 1000, ".2f")) z = float(format(data.pose.position.z * 1000, ".2f")) + # When the running time is less than 30s, or the target position is still changing, perform tracking behavior # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 if ( time.time() - temp_time < 30 @@ -120,7 +127,7 @@ def grippercallback(data): temp_x, temp_y, temp_z = x, y, z return - else: # 表示目标处于静止状态，可以尝试抓取 + else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取 print(x, y, z) @@ -172,13 +179,13 @@ def main(): for _ in range(10): # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) + # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) time.sleep(0.5) pub_pump(False, Pin) # time.sleep(2.5) - # mark 信息的订阅者 + # subscribers to mark information, mark信息的订阅者 rospy.Subscriber("visualization_marker", Marker, grippercallback, queue_size=1) diff --git a/mypalletizer_260/scripts/follow_display.py b/mypalletizer_260/scripts/follow_display.py index 19cd207..74524c3 100755 --- a/mypalletizer_260/scripts/follow_display.py +++ b/mypalletizer_260/scripts/follow_display.py @@ -37,7 +37,7 @@ def talker(): pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) rate = rospy.Rate(30) # 30hz - # pub joint state + # pub joint state. 发布关节状态 joint_state_send = JointState() joint_state_send.header = Header() @@ -83,7 +83,7 @@ def talker(): marker_.scale.y = 0.04 marker_.scale.z = 0.04 - # marker position initial + # marker position initial. 标记初始位置 # print(coords) if not coords: # coords = [0, 0, 0, 0, 0 ] diff --git a/mypalletizer_260/scripts/following_marker.py b/mypalletizer_260/scripts/following_marker.py index 4afe3f9..144a52d 100755 --- a/mypalletizer_260/scripts/following_marker.py +++ b/mypalletizer_260/scripts/following_marker.py @@ -33,7 +33,7 @@ def talker(): print(type(trans), trans) print(type(rot), rot) - # marker + # marker 标记 marker_.header.stamp = now marker_.type = marker_.CUBE marker_.action = marker_.ADD @@ -41,7 +41,7 @@ def talker(): marker_.scale.y = 0.04 marker_.scale.z = 0.04 - # marker position initial + # marker position initial. 标记初始位置 marker_.pose.position.x = trans[0] marker_.pose.position.y = trans[1] marker_.pose.position.z = trans[2] diff --git a/mypalletizer_260/scripts/listen_real.py b/mypalletizer_260/scripts/listen_real.py index 1d853f0..4b706bc 100755 --- a/mypalletizer_260/scripts/listen_real.py +++ b/mypalletizer_260/scripts/listen_real.py @@ -17,7 +17,7 @@ def talker(): pub = rospy.Publisher("joint_states", JointState, queue_size=10) rate = rospy.Rate(30) # 30hz - # pub joint state + # pub joint state 发布关节状态 joint_state_send = JointState() joint_state_send.header = Header() @@ -38,7 +38,7 @@ def talker(): rospy.loginfo("start loop ...") while not rospy.is_shutdown(): - # get real angles from server. + # get real angles from server. 从服务获取真实的角度 res = func() if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: continue @@ -52,7 +52,7 @@ def talker(): ] rospy.loginfo("res: {}".format(radians_list)) - # publish angles. + # publish angles. 发布角度 joint_state_send.header.stamp = rospy.Time.now() joint_state_send.position = radians_list pub.publish(joint_state_send) diff --git a/mypalletizer_260/scripts/listen_real_of_topic.py b/mypalletizer_260/scripts/listen_real_of_topic.py index e6770b8..22f95f1 100755 --- a/mypalletizer_260/scripts/listen_real_of_topic.py +++ b/mypalletizer_260/scripts/listen_real_of_topic.py @@ -6,7 +6,7 @@ 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): def __init__(self): @@ -14,9 +14,9 @@ class Listener(object): rospy.loginfo("start ...") rospy.init_node("real_listener_1", anonymous=True) - # init publisher. + # init publisher. 初始化发布者 self.pub = rospy.Publisher("joint_states", JointState, queue_size=10) - # init subscriber. + # init subscriber. 初始化订阅者 self.sub = rospy.Subscriber("mypal/angles_real", MypalAngles, self.callback) rospy.spin() @@ -26,7 +26,7 @@ class Listener(object): Args: data (MycobotAngles): callback argument. """ - # ini publisher object. + # ini publisher object. 初始化发布者对象 joint_state_send = JointState() joint_state_send.header = Header() @@ -43,7 +43,7 @@ class Listener(object): joint_state_send.effort = [] joint_state_send.header.stamp = rospy.Time.now() - # process callback data. + # process callback data. 处理回调数据 radians_list = [ data.joint_1 * (math.pi / 180), data.joint_2 * (math.pi / 180), diff --git a/mypalletizer_260/scripts/simple_gui.py b/mypalletizer_260/scripts/simple_gui.py index d9aa6e4..a758aaa 100755 --- a/mypalletizer_260/scripts/simple_gui.py +++ b/mypalletizer_260/scripts/simple_gui.py @@ -10,47 +10,48 @@ from rospy import ServiceException class Window: def __init__(self, handle): self.win = handle - self.win.resizable(0, 0) # 固定窗口大小 fixed window size + self.win.resizable(0, 0) # fixed window size 固定窗口大小 self.model = 0 self.speed = rospy.get_param("~speed", 50) - # 设置默认速度 set default speed + # set default speed. 设置默认速度 self.speed_d = tk.StringVar() self.speed_d.set(str(self.speed)) # print(self.speed) self.connect_ser() - # # 获取机械臂数据 get data + # Get robotic arm data. 获取机械臂数据 self.record_coords = [0, 0, 0, 0,self.speed, self.model] self.res_angles = [0, 0, 0, 0, self.speed, self.model] self.get_date() - # get screen width and height - self.ws = self.win.winfo_screenwidth() # width of the screen - self.hs = self.win.winfo_screenheight() # height of the screen + # get screen width and height. 获取屏幕宽度和高度 + self.ws = self.win.winfo_screenwidth() + self.hs = self.win.winfo_screenheight() # calculate x and y coordinates for the Tk root window + # 计算 Tk 根窗口的 x 和 y 坐标 x = (self.ws / 2) - 190 y = (self.hs / 2) - 250 self.win.geometry("430x350+{}+{}".format(x, y)) - # 布局 layout + # layout. 布局 self.set_layout() - # 输入部分 input + # input. 输入部分 self.need_input() - # 展示部分 show info + # show info. 展示部分 self.show_init() - # joint 设置按钮 Settings button + # Set joint button. 设置关节按钮 tk.Button(self.frmLT, text="Set", width=5, command=self.get_joint_input).grid( row=6, column=1, sticky="w", padx=3, pady=2 ) - # coordination 设置按钮 Settings button + # Set Coordinate button. 设置坐标按钮 tk.Button(self.frmRT, text="Set", width=5, command=self.get_coord_input).grid( row=6, column=1, sticky="w", padx=3, pady=2 ) - # 夹爪开关按钮 Gripper Switch + # 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 ) @@ -91,7 +92,7 @@ class Window: self.frmRT.grid(row=0, column=1, padx=2, pady=3) def need_input(self): - # 输入提示 input hint + # input hint. 输入提示 tk.Label(self.frmLT, text="Joint 1 ").grid(row=0) tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) tk.Label(self.frmLT, text="Joint 3 ").grid(row=2) @@ -104,7 +105,7 @@ class Window: tk.Label(self.frmRT, text=" rx ").grid(row=3) - # 设置输入框的默认值 Set the default value of the input box + # Set the default value of the input box. 设置输入框的默认值 self.j1_default = tk.StringVar() self.j1_default.set(self.res_angles[0]) self.j2_default = tk.StringVar() @@ -125,7 +126,7 @@ class Window: self.rx_default.set(self.record_coords[3]) - # joint input box 输入框 + # joint input box. 输入框 self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default) self.J_1.grid(row=0, column=1, pady=3) self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default) @@ -136,7 +137,7 @@ class Window: self.J_4.grid(row=3, column=1, pady=3) - # coord input box 输入框 + # coord input box. 输入框 self.x = tk.Entry(self.frmRT, textvariable=self.x_default) self.x.grid(row=0, column=1, pady=3, padx=0) self.y = tk.Entry(self.frmRT, textvariable=self.y_default) @@ -147,13 +148,13 @@ class Window: self.rx.grid(row=3, column=1, pady=3) - # 所有输入框，用于拿输入的数据 all input box + # All input boxes, used to get the input data. 所有输入框，用于拿输入的数据 self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4] self.all_c = [self.x, self.y, self.z, self.rx] - # 速度输入框 speed input box + # speed input box. 速度输入框 tk.Label( self.frmLB, text="speed", @@ -162,7 +163,7 @@ class Window: self.get_speed.grid(row=0, column=1) def show_init(self): - # 显示 display + # display. 显示 tk.Label(self.frmLC, text="Joint 1 ").grid(row=0) tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) tk.Label(self.frmLC, text="Joint 3 ").grid(row=2) @@ -228,9 +229,9 @@ class Window: ] - # 显示 + # display. 显示 tk.Label(self.frmLC, text=" x ").grid(row=0, column=3) - tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) # 第二行 + tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) # the second row. 第二行 tk.Label(self.frmLC, text=" z ").grid(row=2, column=3) tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3) @@ -286,7 +287,7 @@ class Window: bg="white", ).grid(row=3, column=4, padx=5, pady=5) - # mm 单位展示 show label"mm" + # show label"mm" .mm 单位展示 self.unit = tk.StringVar() self.unit.set("mm") for i in range(4): @@ -307,6 +308,7 @@ class Window: pass def get_coord_input(self): + # Get the data input by coord and send it to the robotic arm # 获取 coord 输入的数据，发送给机械臂 c_value = [] for i in self.all_c: @@ -325,7 +327,8 @@ class Window: self.show_j_date(c_value[:-2], "coord") def get_joint_input(self): - # 获取joint输入的数据，发送给机械臂 Get the data of the joint and send it to the robotic arm + # Get the data of the joint and send it to the robotic arm + # 获取joint输入的数据，发送给机械臂 j_value = [] for i in self.all_j: # print(type(i.get())) @@ -343,7 +346,7 @@ class Window: # return j_value,c_value,speed def get_date(self): - # 拿机械臂的数据，用于展示 Get the data of robotic arm for display + # Get the data of robotic arm for display. 拿机械臂的数据，用于展示 t = time.time() while time.time() - t < 2: self.res = self.get_coords() @@ -380,7 +383,7 @@ class Window: # def send_input(self,dates): def show_j_date(self, date, way=""): - # 展示数据 + # Show data. 展示数据 if way == "coord": for i, j in zip(date, self.coord_all): # print(i) diff --git a/mypalletizer_260/scripts/slider_control.py b/mypalletizer_260/scripts/slider_control.py index 254e4b2..bd2d246 100755 --- a/mypalletizer_260/scripts/slider_control.py +++ b/mypalletizer_260/scripts/slider_control.py @@ -25,7 +25,7 @@ def callback(data): data_list = [] for index, value in enumerate(data.position): data_list.append(value) - del data_list[3] # delete the angle of joint3 to joint4,because it do not exsist actually! + del data_list[3] # delete the angle of joint3 to joint4,because it do not exsist actually! 把joint3到joint4的角度删掉，因为它实际上不存在！ # data_list[3] = data_list[4] # print(data_list) mc.send_radians(data_list, 80) @@ -37,12 +37,13 @@ def listener(): rospy.init_node("control_slider", anonymous=True) rospy.Subscriber("joint_states", JointState, callback) - port = rospy.get_param("~port", "/dev/ttyUSB0") + port = rospy.get_param("~port", "/dev/ttyUSB0") # Select connected device. 选择连接设备 baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyPalletizer(port, baud) # spin() simply keeps python from exiting until this node is stopped + # spin() 只是阻止python退出，直到该节点停止 print("spin ...") rospy.spin() diff --git a/mypalletizer_260/scripts/teleop_keyboard.py b/mypalletizer_260/scripts/teleop_keyboard.py index c009615..a2fdf82 100755 --- a/mypalletizer_260/scripts/teleop_keyboard.py +++ b/mypalletizer_260/scripts/teleop_keyboard.py @@ -11,7 +11,7 @@ import time import roslib - +# Terminal output prompt information. 终端输出提示信息 msg = """\ Mypalletizer Teleop Keyboard Controller --------------------------- @@ -100,6 +100,7 @@ def teleop_keyboard(): try: print(msg) print(vels(speed, change_percent)) + # Keyboard keys call different motion functions. 键盘按键调用不同的运动功能 while 1: try: # print("\r current coords: %s" % record_coords, end="")