From cd4ad7922c77806ac9b2f97adffdef19317ec369 Mon Sep 17 00:00:00 2001 From: Zachary Date: Tue, 6 Jul 2021 14:25:19 +0800 Subject: [PATCH] chore: format code. --- scripts/cilibrate_camera.py | 11 +- scripts/client.py | 22 +- scripts/control_marker.py | 40 +-- scripts/mycobot/detect_marker.py | 79 +++-- scripts/mycobot/follow_and_pump.py | 64 ++-- scripts/mycobot/follow_display.py | 46 +-- scripts/mycobot/following_marker.py | 15 +- scripts/mycobot/listen_real.py | 30 +- scripts/mycobot/listen_real_of_topic.py | 26 +- scripts/mycobot/simple_gui.py | 453 +++++++++++++++--------- scripts/mycobot/slider_control.py | 13 +- scripts/mycobot/teleop_keyboard.py | 84 +++-- scripts/mycobot_moveit/sync_plan.py | 14 +- scripts/mycobot_services.py | 45 +-- scripts/mycobot_topics.py | 142 ++++---- scripts/slider_600.py | 91 ++--- scripts/test.py | 62 ++-- src/camera_display.cpp | 55 ++- src/opencv_camera.cpp | 105 +++--- 19 files changed, 780 insertions(+), 617 deletions(-) diff --git a/scripts/cilibrate_camera.py b/scripts/cilibrate_camera.py index 16b3e7d..58fbd65 100644 --- a/scripts/cilibrate_camera.py +++ b/scripts/cilibrate_camera.py @@ -3,14 +3,15 @@ import cv2 import cv2.aruco as aruco import glob + def calibrateKd(im_fpath, aruco_len=60.0): (w, h) = (6, 4) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01) - objp = np.zeros((w*h, 3), np.float32) + objp = np.zeros((w * h, 3), np.float32) objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2) objp *= aruco_len objpoints, imgpoints = [], [] - images = glob.glob(f'{im_fpath}/*') + images = glob.glob(f"{im_fpath}/*") for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) @@ -21,11 +22,13 @@ def calibrateKd(im_fpath, aruco_len=60.0): imgpoints.append(corners2) img = cv2.drawChessboardCorners(img, (6, 4), corners2, ret) - ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shap[::-1], None, None) + ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera( + objpoints, imgpoints, gray.shap[::-1], None, None + ) tot_error = 0 for i in range(len(objpoints)): imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist) - error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2)/len(imgpoints2) + error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2) tot_error += error print(tot_error) return mtx, dist diff --git a/scripts/client.py b/scripts/client.py index ec11c81..901b4fb 100755 --- a/scripts/client.py +++ b/scripts/client.py @@ -5,15 +5,25 @@ from mycobot_ros.srv import * def test(): - rospy.wait_for_service('get_joint_angles') + rospy.wait_for_service("get_joint_angles") try: - func = rospy.ServiceProxy('set_joint_coords', SetCoords) - res = func(*[104.9000015258789, -61.79999923706055, 381.0, - - 106.43000030517578, 2.1600000858306885, -87.80999755859375, 50, 0]) - print('res:', res) + func = rospy.ServiceProxy("set_joint_coords", SetCoords) + res = func( + *[ + 104.9000015258789, + -61.79999923706055, + 381.0, + -106.43000030517578, + 2.1600000858306885, + -87.80999755859375, + 50, + 0, + ] + ) + print("res:", res) except: pass -if __name__ == '__main__': +if __name__ == "__main__": test() diff --git a/scripts/control_marker.py b/scripts/control_marker.py index 82e9b18..de85ad1 100755 --- a/scripts/control_marker.py +++ b/scripts/control_marker.py @@ -40,8 +40,7 @@ def processFeedback(feedback): center_x_changed = current_x center_y_changed = current_y center_z_changed = current_z - print(center_x_changed, center_y_changed, center_z_changed, - _x, _y, _z) + print(center_x_changed, center_y_changed, center_z_changed, _x, _y, _z) coords = [ current_y * 1000, @@ -165,33 +164,33 @@ def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof=Fal server.insert(int_marker, processFeedback) menu_handler.apply(server, int_marker.name) + # marker box finish def listener(): global server - rospy.init_node('control_marker', anonymous=True) + rospy.init_node("control_marker", anonymous=True) coords = mycobot.get_coords() # print(coords) # crate a timer to update the pushlished transforms - server = InteractiveMarkerServer('mycobot_controller') - menu_handler.insert('First Entry', callback=processFeedback) - menu_handler.insert('Second Entry', callback=processFeedback) + server = InteractiveMarkerServer("mycobot_controller") + menu_handler.insert("First Entry", callback=processFeedback) + menu_handler.insert("Second Entry", callback=processFeedback) if not coords: coords = [0, 0, 0, 0, 0, 0] - rospy.loginfo('error [101]: can not get coord values') + rospy.loginfo("error [101]: can not get coord values") # initial position position = Point(coords[1] / -1000, coords[0] / 1000, coords[2] / 1000) # orientation = Quaternion(coords[4] / 100, coords[3] / 100, coords[5] / 100, 1) orientation = Quaternion(0, 0, 0, 1) - make6DofMarker(True, InteractiveMarkerControl.NONE, - position, orientation, True) + make6DofMarker(True, InteractiveMarkerControl.NONE, position, orientation, True) server.applyChanges() - pub = rospy.Publisher('joint_states', JointState, queue_size=10) + pub = rospy.Publisher("joint_states", JointState, queue_size=10) rate = rospy.Rate(30) # 10hz # pub joint state @@ -199,19 +198,19 @@ def listener(): joint_state_send.header = Header() joint_state_send.name = [ - 'joint2_to_joint1', - 'joint3_to_joint2', - 'joint4_to_joint3', - 'joint5_to_joint4', - 'joint6_to_joint5', - 'joint6output_to_joint6' + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + "joint6output_to_joint6", ] joint_state_send.velocity = [0] joint_state_send.effort = [] marker_ = Marker() - marker_.header.frame_id = '/joint1' - marker_.ns = 'my_namespace' + marker_.header.frame_id = "/joint1" + marker_.ns = "my_namespace" while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() @@ -232,8 +231,7 @@ def listener(): rate.sleep() -if __name__ == '__main__': - port = subprocess.check_output(['echo -n /dev/ttyUSB*'], - shell=True).decode() +if __name__ == "__main__": + port = subprocess.check_output(["echo -n /dev/ttyUSB*"], shell=True).decode() mycobot = MyCobot(port) listener() diff --git a/scripts/mycobot/detect_marker.py b/scripts/mycobot/detect_marker.py index d6734fe..1eb5628 100755 --- a/scripts/mycobot/detect_marker.py +++ b/scripts/mycobot/detect_marker.py @@ -2,15 +2,12 @@ import rospy import cv2 as cv import numpy as np -from geometry_msgs.msg import Twist from cv_bridge import CvBridge, CvBridgeError from sensor_msgs.msg import Image -from visualization_msgs.msg import Marker import tf from tf.broadcaster import TransformBroadcaster import tf_conversions -from mycobot_ros.srv import ( - GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) +from mycobot_ros.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus class ImageConverter: @@ -19,63 +16,73 @@ class ImageConverter: self.bridge = CvBridge() self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage('calibrationFileName.xml', cv.FILE_STORAGE_READ) - self.dist_coeffs = calibrationParams.getNode('distCoeffs').mat() + calibrationParams = cv.FileStorage( + "calibrationFileName.xml", cv.FILE_STORAGE_READ + ) + self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() self.camera_matrix = None - # rospy.wait_for_service('get_joint_coords') - # rospy.wait_for_service('set_joint_coords') - - # try: - # self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords) - # self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords) - # except: - # print('Error: cannot connect service...') - # exit(1) self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - def callback(self, data): - '''Callback function. + """Callback function. Process image with OpenCV, detect Mark to get the pose. Then acccording the pose to transforming. - ''' + """ try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) size = cv_image.shape focal_length = size[1] - center = [size[1]/2, size[0]/2] + center = [size[1] / 2, size[0] / 2] if self.camera_matrix is None: - self.camera_matrix = np.array([ - [focal_length,0,center[0]], - [0, focal_length, center[1]], - [0,0,1], - ], dtype=np.float32) + self.camera_matrix = np.array( + [ + [focal_length, 0, center[0]], + [0, focal_length, center[1]], + [0, 0, 1], + ], + dtype=np.float32, + ) gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - corners, ids, rejectImaPoint = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) + corners, ids, rejectImaPoint = cv.aruco.detectMarkers( + gray, self.aruco_dict, parameters=self.aruo_params + ) if len(corners) > 0: if ids is not None: # print('corners:', corners, 'ids:', ids) - rvec, tvec, _ = cv.aruco.estimatePoseSingleMarkers(corners, 0.05, self.camera_matrix, self.dist_coeffs) + rvec, tvec, _ = cv.aruco.estimatePoseSingleMarkers( + corners, 0.05, self.camera_matrix, self.dist_coeffs + ) (rvec - tvec).any() - print('rvec:', rvec, 'tvec:', tvec) + print("rvec:", rvec, "tvec:", tvec) for i in range(rvec.shape[0]): cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis(cv_image, self.camera_matrix, self.dist_coeffs, rvec[i, :, :], tvec[i, :, :], 0.03) + cv.aruco.drawAxis( + cv_image, + self.camera_matrix, + self.dist_coeffs, + rvec[i, :, :], + tvec[i, :, :], + 0.03, + ) # Just process first one detected. xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] -0.03] + xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler(euler[0], euler[1], euler[2]) - print('tf_change:', tf_change) + tf_change = tf.transformations.quaternion_from_euler( + euler[0], euler[1], euler[2] + ) + print("tf_change:", tf_change) - self.br.sendTransform(xyz, tf_change, rospy.Time.now(), 'basic_shapes', 'joint6_flange' ) + self.br.sendTransform( + xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" + ) # [x, y, z, -172, 3, -46.8] cv.imshow("Image", cv_image) @@ -84,13 +91,15 @@ class ImageConverter: try: pass except CvBridgeError as e: - print e -if __name__ == '__main__': + print(e) + + +if __name__ == "__main__": try: rospy.init_node("detect_marker") rospy.loginfo("Starting cv_bridge_test node") ImageConverter() rospy.spin() except KeyboardInterrupt: - print "Shutting down cv_bridge_test node." + print("Shutting down cv_bridge_test node.") cv.destroyAllWindows() diff --git a/scripts/mycobot/follow_and_pump.py b/scripts/mycobot/follow_and_pump.py index 0bf6b15..33b28a5 100755 --- a/scripts/mycobot/follow_and_pump.py +++ b/scripts/mycobot/follow_and_pump.py @@ -4,13 +4,13 @@ from visualization_msgs.msg import Marker import time import random -from mycobot_ros.msg import (MycobotSetAngles, MycobotSetCoords,MycobotPumpStatus) +from mycobot_ros.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus -rospy.init_node('gipper_subscriber',anonymous=True) -angle_pub = rospy.Publisher('mycobot/angles_goal', MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher('mycobot/coords_goal', MycobotSetCoords, queue_size=5) -pump_pub = rospy.Publisher('mycobot/pump_status', MycobotPumpStatus, queue_size=5) +rospy.init_node("gipper_subscriber", anonymous=True) +angle_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=5) +coord_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=5) +pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=5) angles = MycobotSetAngles() coords = MycobotSetCoords() @@ -27,7 +27,7 @@ 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): +def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): coords.x = x coords.y = y coords.z = z @@ -39,6 +39,7 @@ def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70 , m=2): # print(coords) coord_pub.publish(coords) + def pub_angles(a, b, c, d, e, f, sp): angles.joint_1 = float(a) angles.joint_2 = float(b) @@ -46,13 +47,15 @@ def pub_angles(a, b, c, d, e, f, sp): angles.joint_4 = float(d) angles.joint_5 = float(e) angles.joint_6 = float(f) - angles.speed = sp + angles.speed = sp angle_pub.publish(angles) + def pub_pump(flag): pump.Status = flag pump_pub.publish(pump) + def target_is_moving(x, y, z): count = 0 for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): @@ -72,16 +75,20 @@ def grippercallback(data): return # pump lenght: 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')) + 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")) - if time.time() - temp_time < 30 or (temp_x == temp_y == temp_z == 0.0) or target_is_moving(x - x_offset, y - y_offset ,z): + if ( + time.time() - temp_time < 30 + or (temp_x == temp_y == temp_z == 0.0) + or target_is_moving(x - x_offset, y - y_offset, z) + ): x -= x_offset y -= y_offset - pub_coords(x-20, y, 280) - time.sleep(.1) + pub_coords(x - 20, y, 280) + time.sleep(0.1) temp_x, temp_y, temp_z = x, y, z return @@ -91,16 +98,15 @@ def grippercallback(data): # detect heigth + pump height + limit height + offset x += x_offset y += y_offset - z = z + 88 + z_offset - + z = z + 88 + z_offset pub_coords(x, y, z) time.sleep(2.5) # down - for i in range(1,17): - pub_coords(x, y, z - i * 5,rx=-160, sp=10) - time.sleep(.1) + for i in range(1, 17): + pub_coords(x, y, z - i * 5, rx=-160, sp=10) + time.sleep(0.1) time.sleep(2) @@ -114,16 +120,16 @@ def grippercallback(data): time.sleep(1.5) put_z = 140 - pub_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2) + pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) time.sleep(1.5) - for i in range (1,8): - pub_coords(39.4, -174.7, put_z-i*5, -177.13, -4.13, -152.59,15,2) - time.sleep(.1) + for i in range(1, 8): + pub_coords(39.4, -174.7, put_z - i * 5, -177.13, -4.13, -152.59, 15, 2) + time.sleep(0.1) pub_pump(False) - time.sleep(.5) + time.sleep(0.5) pub_angles(0, 30, -50, -40, 0, 0, 50) time.sleep(1.5) @@ -131,20 +137,20 @@ def grippercallback(data): # finally flag = True + 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) - time.sleep(.5) + # 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) # time.sleep(2.5) - rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1) - print 'gripper test' + rospy.Subscriber("visualization_marker", Marker, grippercallback, queue_size=1) + print("gripper test") rospy.spin() -if __name__ == '__main__': +if __name__ == "__main__": main() - \ No newline at end of file diff --git a/scripts/mycobot/follow_display.py b/scripts/mycobot/follow_display.py index 238dcea..0f42e37 100755 --- a/scripts/mycobot/follow_display.py +++ b/scripts/mycobot/follow_display.py @@ -11,28 +11,30 @@ from pymycobot.mycobot import MyCobot def talker(): - rospy.init_node('display', anonymous=True) + rospy.init_node("display", anonymous=True) - print('Try connect real mycobot...') - port = rospy.get_param('~port', '/dev/ttyUSB0') - baud = rospy.get_param('~baud', 115200) - print('port: {}, baud: {}\n'.format(port, baud)) + print("Try connect real mycobot...") + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + print("port: {}, baud: {}\n".format(port, baud)) try: mycobot = MyCobot(port, baud) except Exception as e: print(e) - print('''\ + print( + """\ \rTry connect mycobot failed! \rPlease check wether connected with mycobot. \rPlease chckt wether the port or baud is right. - ''') + """ + ) exit(1) mycobot.release_all_servos() - time.sleep(.1) - print('Rlease all servos over.\n') + 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) + 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 @@ -40,21 +42,21 @@ def talker(): joint_state_send.header = Header() joint_state_send.name = [ - 'joint2_to_joint1', - 'joint3_to_joint2', - 'joint4_to_joint3', - 'joint5_to_joint4', - 'joint6_to_joint5', - 'joint6output_to_joint6' + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + "joint6output_to_joint6", ] joint_state_send.velocity = [0] joint_state_send.effort = [] marker_ = Marker() - marker_.header.frame_id = '/joint1' - marker_.ns = 'my_namespace' + marker_.header.frame_id = "/joint1" + marker_.ns = "my_namespace" - print('publishing ...') + print("publishing ...") while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() @@ -82,7 +84,7 @@ def talker(): # print(coords) if not coords: coords = [0, 0, 0, 0, 0, 0] - rospy.loginfo('error [101]: can not get coord values') + rospy.loginfo("error [101]: can not get coord values") marker_.pose.position.x = coords[1] / 1000 * -1 marker_.pose.position.y = coords[0] / 1000 @@ -95,7 +97,7 @@ def talker(): rate.sleep() -if __name__ == '__main__': +if __name__ == "__main__": try: talker() except rospy.ROSInterruptException: diff --git a/scripts/mycobot/following_marker.py b/scripts/mycobot/following_marker.py index e2cb6a5..4afe3f9 100755 --- a/scripts/mycobot/following_marker.py +++ b/scripts/mycobot/following_marker.py @@ -9,24 +9,23 @@ import tf def talker(): - rospy.init_node('following_marker', anonymous=True) + rospy.init_node("following_marker", anonymous=True) - pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10) + pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) rate = rospy.Rate(20) - listener = tf.TransformListener() marker_ = Marker() - marker_.header.frame_id = '/joint1' - marker_.ns = 'basic_cube' + marker_.header.frame_id = "/joint1" + marker_.ns = "basic_cube" - print('publishing ...') + print("publishing ...") while not rospy.is_shutdown(): now = rospy.Time.now() - rospy.Duration(0.1) try: - trans, rot = listener.lookupTransform('joint1', 'basic_shapes', now) + trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) except Exception as e: print(e) continue @@ -58,7 +57,7 @@ def talker(): rate.sleep() -if __name__ == '__main__': +if __name__ == "__main__": try: talker() except rospy.ROSInterruptException: diff --git a/scripts/mycobot/listen_real.py b/scripts/mycobot/listen_real.py index 97a1d2e..962f168 100755 --- a/scripts/mycobot/listen_real.py +++ b/scripts/mycobot/listen_real.py @@ -10,9 +10,9 @@ from mycobot_ros.srv import GetAngles def talker(): - rospy.loginfo('start ...') - rospy.init_node('real_listener', anonymous=True) - pub = rospy.Publisher('joint_states', JointState, queue_size=10) + rospy.loginfo("start ...") + rospy.init_node("real_listener", anonymous=True) + pub = rospy.Publisher("joint_states", JointState, queue_size=10) rate = rospy.Rate(30) # 30hz # pub joint state @@ -20,21 +20,21 @@ def talker(): joint_state_send.header = Header() joint_state_send.name = [ - 'joint2_to_joint1', - 'joint3_to_joint2', - 'joint4_to_joint3', - 'joint5_to_joint4', - 'joint6_to_joint5', - 'joint6output_to_joint6' + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + "joint6output_to_joint6", ] joint_state_send.velocity = [0] joint_state_send.effort = [] - rospy.loginfo('wait service') - rospy.wait_for_service('get_joint_angles') - func = rospy.ServiceProxy('get_joint_angles', GetAngles) + rospy.loginfo("wait service") + rospy.wait_for_service("get_joint_angles") + func = rospy.ServiceProxy("get_joint_angles", GetAngles) - rospy.loginfo('start loop ...') + rospy.loginfo("start loop ...") while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() @@ -49,14 +49,14 @@ def talker(): res.joint_5 * (math.pi / 180), res.joint_6 * (math.pi / 180), ] - rospy.loginfo('res: {}'.format(radians_list)) + rospy.loginfo("res: {}".format(radians_list)) joint_state_send.position = radians_list pub.publish(joint_state_send) rate.sleep() -if __name__ == '__main__': +if __name__ == "__main__": try: talker() except rospy.ROSInterruptException: diff --git a/scripts/mycobot/listen_real_of_topic.py b/scripts/mycobot/listen_real_of_topic.py index 7a20a0c..caabe5f 100755 --- a/scripts/mycobot/listen_real_of_topic.py +++ b/scripts/mycobot/listen_real_of_topic.py @@ -13,10 +13,10 @@ class Listener(object): def __init__(self): super(Listener, self).__init__() - rospy.loginfo('start ...') - rospy.init_node('real_listener_1', anonymous=True) - self.pub = rospy.Publisher('joint_states', JointState, queue_size=10) - self.sub = rospy.Subscriber('mycobot/angles_real', MycobotAngles, self.callback) + rospy.loginfo("start ...") + rospy.init_node("real_listener_1", anonymous=True) + self.pub = rospy.Publisher("joint_states", JointState, queue_size=10) + self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback) rate = rospy.Rate(30) # 30hz rospy.spin() @@ -26,12 +26,12 @@ class Listener(object): joint_state_send.header = Header() joint_state_send.name = [ - 'joint2_to_joint1', - 'joint3_to_joint2', - 'joint4_to_joint3', - 'joint5_to_joint4', - 'joint6_to_joint5', - 'joint6output_to_joint6' + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + "joint6output_to_joint6", ] joint_state_send.velocity = [0] joint_state_send.effort = [] @@ -45,14 +45,14 @@ class Listener(object): data.joint_5 * (math.pi / 180), data.joint_6 * (math.pi / 180), ] - rospy.loginfo('res: {}'.format(radians_list)) + rospy.loginfo("res: {}".format(radians_list)) joint_state_send.position = radians_list self.pub.publish(joint_state_send) # rate.sleep() - -if __name__ == '__main__': + +if __name__ == "__main__": try: Listener() except rospy.ROSInterruptException: diff --git a/scripts/mycobot/simple_gui.py b/scripts/mycobot/simple_gui.py index 76dea22..6b5d7ce 100755 --- a/scripts/mycobot/simple_gui.py +++ b/scripts/mycobot/simple_gui.py @@ -1,38 +1,38 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- import Tkinter as tk -from mycobot_ros.srv import ( - GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) +from mycobot_ros.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import time from rospy import ServiceException - + + class Window: - def __init__(self,handle): + def __init__(self, handle): self.win = handle - self.win.resizable(0,0) # 固定窗口大小 - + self.win.resizable(0, 0) # 固定窗口大小 + self.model = 0 self.speed = rospy.get_param("~speed", 50) - - #设置默认速度 + + # 设置默认速度 self.speed_d = tk.StringVar() self.speed_d.set(str(self.speed)) # print(self.speed) self.connect_ser() # 获取机械臂数据 - self.record_coords = [0, 0, 0, 0, 0, 0, self.speed,self.model] - self.res_angles = [0, 0, 0, 0, 0, 0, self.speed,self.model] + self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model] + self.res_angles = [0, 0, 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 + self.ws = self.win.winfo_screenwidth() # width of the screen + self.hs = self.win.winfo_screenheight() # height of the screen # calculate x and y coordinates for the Tk root window - x = (self.ws/2) - 190 - y = (self.hs/2) - 250 - self.win.geometry('430x370+{}+{}'.format(x,y)) + x = (self.ws / 2) - 190 + y = (self.hs / 2) - 250 + self.win.geometry("430x370+{}+{}".format(x, y)) # 布局 self.set_layout() # 输入部分 @@ -40,64 +40,72 @@ class Window: # 展示部分 self.show_init() - # joint 设置按钮 - tk.Button(self.frmLT,text="设置",width=5, command=self.get_joint_input).grid(row=6, column=1, sticky="w", padx=3, pady=2) + tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid( + row=6, column=1, sticky="w", padx=3, pady=2 + ) # coordination 设置按钮 - tk.Button(self.frmRT,text="设置",width=5, command=self.get_coord_input).grid(row=6, column=1, sticky="w", padx=3, pady=2) + tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid( + row=6, column=1, sticky="w", padx=3, pady=2 + ) # 夹爪开关按钮 - tk.Button(self.frmLB,text="夹爪(开)",command=self.gripper_open,width=5).grid(row=1, column=0, sticky="w", padx=3, pady=50) - tk.Button(self.frmLB,text="夹爪(关)",command=self.gripper_close,width=5).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) + tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid( + row=1, column=0, sticky="w", padx=3, pady=50 + ) + tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid( + row=1, column=1, sticky="w", padx=3, pady=2 + ) - rospy.wait_for_service('get_joint_angles') - 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') + def connect_ser(self): + rospy.init_node("simple_gui", anonymous=True, disable_signals=True) + + rospy.wait_for_service("get_joint_angles") + 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") try: - self.get_coords = 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.set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles) + self.get_coords = 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.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles) self.switch_gripper = rospy.ServiceProxy( - 'switch_gripper_status', GripperStatus) + "switch_gripper_status", GripperStatus + ) except: - print('start error ...') + print("start error ...") exit(1) - print('Connect service success.') - + print("Connect service success.") + def set_layout(self): self.frmLT = tk.Frame(width=200, height=200) self.frmLC = tk.Frame(width=200, height=200) self.frmLB = tk.Frame(width=200, height=200) self.frmRT = tk.Frame(width=200, height=200) - self.frmLT.grid(row=0, column=0,padx=1,pady=3) - self.frmLC.grid(row=1, column=0,padx=1,pady=3) - self.frmLB.grid(row=1, column=1,padx=2,pady=3) - self.frmRT.grid(row=0, column=1,padx=2,pady=3) - + self.frmLT.grid(row=0, column=0, padx=1, pady=3) + self.frmLC.grid(row=1, column=0, padx=1, pady=3) + self.frmLB.grid(row=1, column=1, padx=2, pady=3) + self.frmRT.grid(row=0, column=1, padx=2, pady=3) + def need_input(self): # 输入提示 - 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) - tk.Label(self.frmLT,text="Joint 4 ").grid(row=3) - tk.Label(self.frmLT,text="Joint 5 ").grid(row=4) - tk.Label(self.frmLT,text="Joint 6 ").grid(row=5) - - 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) - tk.Label(self.frmRT,text=" rx ").grid(row=3) - tk.Label(self.frmRT,text=" ry ").grid(row=4) - tk.Label(self.frmRT,text=" rz ").grid(row=5) - + 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) + tk.Label(self.frmLT, text="Joint 4 ").grid(row=3) + tk.Label(self.frmLT, text="Joint 5 ").grid(row=4) + tk.Label(self.frmLT, text="Joint 6 ").grid(row=5) + + 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) + tk.Label(self.frmRT, text=" rx ").grid(row=3) + tk.Label(self.frmRT, text=" ry ").grid(row=4) + tk.Label(self.frmRT, text=" rz ").grid(row=5) + # 设置输入框的默认值 self.j1_default = tk.StringVar() self.j1_default.set(self.res_angles[0]) @@ -111,7 +119,7 @@ class Window: self.j5_default.set(self.res_angles[4]) self.j6_default = tk.StringVar() self.j6_default.set(self.res_angles[5]) - + self.x_default = tk.StringVar() self.x_default.set(self.record_coords[0]) self.y_default = tk.StringVar() @@ -124,99 +132,148 @@ class Window: self.ry_default.set(self.record_coords[4]) self.rz_default = tk.StringVar() self.rz_default.set(self.record_coords[5]) - + # joint 输入框 self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default) - self.J_1.grid(row=0,column=1,pady=3) + self.J_1.grid(row=0, column=1, pady=3) self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default) - self.J_2.grid(row=1,column=1,pady=3) + 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) + self.J_3.grid(row=2, column=1, pady=3) self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default) - self.J_4.grid(row=3,column=1,pady=3) + self.J_4.grid(row=3, column=1, pady=3) self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default) - self.J_5.grid(row=4,column=1,pady=3) + self.J_5.grid(row=4, column=1, pady=3) self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default) - self.J_6.grid(row=5,column=1,pady=3) - + self.J_6.grid(row=5, column=1, pady=3) + # coord 输入框 self.x = tk.Entry(self.frmRT, textvariable=self.x_default) - self.x.grid(row=0,column=1,pady=3,padx=0) + self.x.grid(row=0, column=1, pady=3, padx=0) self.y = tk.Entry(self.frmRT, textvariable=self.y_default) - self.y.grid(row=1,column=1,pady=3) + 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) + self.z.grid(row=2, column=1, pady=3) self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default) - self.rx.grid(row=3,column=1,pady=3) + self.rx.grid(row=3, column=1, pady=3) self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default) - self.ry.grid(row=4,column=1,pady=3) + self.ry.grid(row=4, column=1, pady=3) self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default) - self.rz.grid(row=5,column=1,pady=3) - + self.rz.grid(row=5, column=1, pady=3) + # 所有输入框，用于拿输入的数据 - self.all_j = [self.J_1,self.J_2,self.J_3,self.J_4,self.J_5,self.J_6] - self.all_c = [self.x,self.y,self.z,self.rx,self.ry,self.rz] - + self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6] + self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz] + # 速度输入框 - tk.Label(self.frmLB,text="speed",).grid(row=0,column=0) - self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d,width=10) - self.get_speed.grid(row=0,column=1) - + tk.Label( + self.frmLB, + text="speed", + ).grid(row=0, column=0) + self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10) + self.get_speed.grid(row=0, column=1) + def show_init(self): # 显示 - 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) - tk.Label(self.frmLC,text="Joint 4 ").grid(row=3) - tk.Label(self.frmLC,text="Joint 5 ").grid(row=4) - tk.Label(self.frmLC,text="Joint 6 ").grid(row=5) + 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) + tk.Label(self.frmLC, text="Joint 4 ").grid(row=3) + tk.Label(self.frmLC, text="Joint 5 ").grid(row=4) + tk.Label(self.frmLC, text="Joint 6 ").grid(row=5) # get数据 - + # ，展示出来 self.cont_1 = tk.StringVar(self.frmLC) - self.cont_1.set(str(self.res_angles[0])+"°") + self.cont_1.set(str(self.res_angles[0]) + "°") self.cont_2 = tk.StringVar(self.frmLC) - self.cont_2.set(str(self.res_angles[1])+"°") + self.cont_2.set(str(self.res_angles[1]) + "°") self.cont_3 = tk.StringVar(self.frmLC) - self.cont_3.set(str(self.res_angles[2])+"°") + self.cont_3.set(str(self.res_angles[2]) + "°") self.cont_4 = tk.StringVar(self.frmLC) - self.cont_4.set(str(self.res_angles[3])+"°") + self.cont_4.set(str(self.res_angles[3]) + "°") self.cont_5 = tk.StringVar(self.frmLC) - self.cont_5.set(str(self.res_angles[4])+"°") + self.cont_5.set(str(self.res_angles[4]) + "°") self.cont_6 = tk.StringVar(self.frmLC) - self.cont_6.set(str(self.res_angles[5])+"°") - self.cont_all = [self.cont_1,self.cont_2,self.cont_3,self.cont_4,self.cont_5,self.cont_6,self.speed,self.model] - - self.show_j1 = tk.Label(self.frmLC, - textvariable=self.cont_1, - font=('Arial',9),width=7,height=1,bg='white').grid(row=0,column=1,padx=0,pady=5) - - self.show_j2 = tk.Label(self.frmLC, - textvariable=self.cont_2, - font=('Arial',9),width=7,height=1,bg='white').grid(row=1,column=1,padx=0,pady=5) - self.show_j3 = tk.Label(self.frmLC, - textvariable=self.cont_3, - font=('Arial',9),width=7,height=1,bg='white').grid(row=2,column=1,padx=0,pady=5) - self.show_j4 = tk.Label(self.frmLC, - textvariable=self.cont_4, - font=('Arial',9),width=7,height=1,bg='white').grid(row=3,column=1,padx=0,pady=5) - self.show_j5 = tk.Label(self.frmLC, - textvariable=self.cont_5, - font=('Arial',9),width=7,height=1,bg='white').grid(row=4,column=1,padx=0,pady=5) - self.show_j6 = tk.Label(self.frmLC, - textvariable=self.cont_6, - font=('Arial',9),width=7,height=1,bg='white').grid(row=5,column=1,padx=5,pady=5) - - self.all_jo = [self.show_j1,self.show_j2,self.show_j3,self.show_j4,self.show_j5,self.show_j6] - + self.cont_6.set(str(self.res_angles[5]) + "°") + self.cont_all = [ + self.cont_1, + self.cont_2, + self.cont_3, + self.cont_4, + self.cont_5, + self.cont_6, + self.speed, + self.model, + ] + + self.show_j1 = tk.Label( + self.frmLC, + textvariable=self.cont_1, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=0, column=1, padx=0, pady=5) + + self.show_j2 = tk.Label( + self.frmLC, + textvariable=self.cont_2, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=1, column=1, padx=0, pady=5) + self.show_j3 = tk.Label( + self.frmLC, + textvariable=self.cont_3, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=2, column=1, padx=0, pady=5) + self.show_j4 = tk.Label( + self.frmLC, + textvariable=self.cont_4, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=3, column=1, padx=0, pady=5) + self.show_j5 = tk.Label( + self.frmLC, + textvariable=self.cont_5, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=4, column=1, padx=0, pady=5) + self.show_j6 = tk.Label( + self.frmLC, + textvariable=self.cont_6, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=5, column=1, padx=5, pady=5) + + self.all_jo = [ + self.show_j1, + self.show_j2, + self.show_j3, + self.show_j4, + self.show_j5, + self.show_j6, + ] + # 显示 - 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=" z ").grid(row=2,column=3) - tk.Label(self.frmLC,text=" rx ").grid(row=3,column=3) - tk.Label(self.frmLC,text=" ry ").grid(row=4,column=3) - tk.Label(self.frmLC,text=" rz ").grid(row=5,column=3) + 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=" z ").grid(row=2, column=3) + tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3) + tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3) + tk.Label(self.frmLC, text=" rz ").grid(row=5, column=3) self.coord_x = tk.StringVar() self.coord_x.set(str(self.record_coords[0])) self.coord_y = tk.StringVar() @@ -229,54 +286,97 @@ class Window: self.coord_ry.set(str(self.record_coords[4])) self.coord_rz = tk.StringVar() self.coord_rz.set(str(self.record_coords[5])) - - self.coord_all = [self.coord_x,self.coord_y,self.coord_z,self.coord_rx,self.coord_ry,self.coord_rz,self.speed,self.model] - - self.show_x = tk.Label(self.frmLC, - textvariable=self.coord_x, - font=('Arial',9),width=7,height=1,bg='white').grid(row=0,column=4,padx=5,pady=5) - self.show_y = tk.Label(self.frmLC, - textvariable=self.coord_y, - font=('Arial',9),width=7,height=1,bg='white').grid(row=1,column=4,padx=5,pady=5) - self.show_z = tk.Label(self.frmLC, - textvariable=self.coord_z, - font=('Arial',9),width=7,height=1,bg='white').grid(row=2,column=4,padx=5,pady=5) - self.show_rx = tk.Label(self.frmLC, - textvariable=self.coord_rx, - font=('Arial',9),width=7,height=1,bg='white').grid(row=3,column=4,padx=5,pady=5) - self.show_ry = tk.Label(self.frmLC, - textvariable=self.coord_ry, - font=('Arial',9),width=7,height=1,bg='white').grid(row=4,column=4,padx=5,pady=5) - self.show_rz = tk.Label(self.frmLC, - textvariable=self.coord_rz, - font=('Arial',9),width=7,height=1,bg='white').grid(row=5,column=4,padx=5,pady=5) - + + self.coord_all = [ + self.coord_x, + self.coord_y, + self.coord_z, + self.coord_rx, + self.coord_ry, + self.coord_rz, + self.speed, + self.model, + ] + + self.show_x = tk.Label( + self.frmLC, + textvariable=self.coord_x, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=0, column=4, padx=5, pady=5) + self.show_y = tk.Label( + self.frmLC, + textvariable=self.coord_y, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=1, column=4, padx=5, pady=5) + self.show_z = tk.Label( + self.frmLC, + textvariable=self.coord_z, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=2, column=4, padx=5, pady=5) + self.show_rx = tk.Label( + self.frmLC, + textvariable=self.coord_rx, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=3, column=4, padx=5, pady=5) + self.show_ry = tk.Label( + self.frmLC, + textvariable=self.coord_ry, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=4, column=4, padx=5, pady=5) + self.show_rz = tk.Label( + self.frmLC, + textvariable=self.coord_rz, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=5, column=4, padx=5, pady=5) + # mm 单位展示 self.unit = tk.StringVar() - self.unit.set('mm') + self.unit.set("mm") for i in range(6): - tk.Label(self.frmLC,textvariable=self.unit,font=('Arial',9)).grid(row=i,column=5) - + 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: # 可能由于该方法没有返回值，服务抛出无法处理的错误 pass - + def gripper_close(self): try: self.switch_gripper(False) except ServiceException: pass - + def get_coord_input(self): # 获取 coord 输入的数据，发送给机械臂 c_value = [] for i in self.all_c: # print(type(i.get())) c_value.append(float(i.get())) - self.speed = int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + self.speed = ( + int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + ) c_value.append(self.speed) c_value.append(self.model) # print(c_value) @@ -284,15 +384,17 @@ class Window: self.set_coords(*c_value) except ServiceException: pass - self.show_j_date(c_value[:-2],'coord') - + self.show_j_date(c_value[:-2], "coord") + def get_joint_input(self): # 获取joint输入的数据，发送给机械臂 j_value = [] for i in self.all_j: # print(type(i.get())) j_value.append(float(i.get())) - self.speed = int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + self.speed = ( + int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + ) j_value.append(self.speed) try: @@ -301,8 +403,7 @@ class Window: pass self.show_j_date(j_value[:-1]) # return j_value,c_value,speed - - + def get_date(self): # 拿机械臂的数据，用于展示 t = time.time() @@ -310,33 +411,47 @@ class Window: self.res = self.get_coords() if self.res.x > 1: break - time.sleep(.1) - + time.sleep(0.1) + t = time.time() while time.time() - t < 2: self.angles = self.get_angles() if self.angles.joint_1 > 1: break - time.sleep(.1) + 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), round(self.res.rx,2), round(self.res.ry,2), round(self.res.rz,2), self.speed, self.model + round(self.res.x, 2), + round(self.res.y, 2), + round(self.res.z, 2), + round(self.res.rx, 2), + round(self.res.ry, 2), + round(self.res.rz, 2), + self.speed, + self.model, + ] + self.res_angles = [ + round(self.angles.joint_1, 2), + round(self.angles.joint_2, 2), + round(self.angles.joint_3, 2), + round(self.angles.joint_4, 2), + round(self.angles.joint_5, 2), + round(self.angles.joint_6, 2), ] - self.res_angles = [round(self.angles.joint_1,2),round(self.angles.joint_2,2),round(self.angles.joint_3,2),round(self.angles.joint_4,2),round(self.angles.joint_5,2),round(self.angles.joint_6,2)] # print('coord:',self.record_coords) # print('angles:',self.res_angles) - + # def send_input(self,dates): - def show_j_date(self,date,way=""): + def show_j_date(self, date, way=""): # 展示数据 if way == "coord": - for i,j in zip(date,self.coord_all): + for i, j in zip(date, self.coord_all): # print(i) j.set(str(i)) else: - for i,j in zip(date,self.cont_all): - j.set(str(i)+"°") - + for i, j in zip(date, self.cont_all): + j.set(str(i) + "°") + def run(self): while True: try: @@ -348,10 +463,12 @@ class Window: else: raise + def main(): window = tk.Tk() - window.title('mycobot ros GUI') + window.title("mycobot ros GUI") Window(window).run() - + + if __name__ == "__main__": main() diff --git a/scripts/mycobot/slider_control.py b/scripts/mycobot/slider_control.py index bb2db53..04f4d83 100755 --- a/scripts/mycobot/slider_control.py +++ b/scripts/mycobot/slider_control.py @@ -1,7 +1,6 @@ #!/usr/bin/env python2 # from std_msgs.msg import String import time -import subprocess import rospy from sensor_msgs.msg import JointState @@ -13,7 +12,7 @@ mc = None def callback(data): - #rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) print(data.position) data_list = [] for index, value in enumerate(data.position): @@ -25,18 +24,18 @@ def callback(data): def listener(): global mc - rospy.init_node('control_slider', anonymous=True) + rospy.init_node("control_slider", anonymous=True) rospy.Subscriber("joint_states", JointState, callback) - port = rospy.get_param('~port', '/dev/ttyUSB0') - baud = rospy.get_param('~baud', 115200) + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyCobot(port, baud) # spin() simply keeps python from exiting until this node is stopped - print('sping ...') + print("spin ...") rospy.spin() -if __name__ == '__main__': +if __name__ == "__main__": listener() diff --git a/scripts/mycobot/teleop_keyboard.py b/scripts/mycobot/teleop_keyboard.py index 219d3c6..18d41c3 100755 --- a/scripts/mycobot/teleop_keyboard.py +++ b/scripts/mycobot/teleop_keyboard.py @@ -1,20 +1,19 @@ #!/usr/bin/env python from __future__ import print_function -from mycobot_ros.srv import ( - GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) +from mycobot_ros.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import sys import select import termios import tty -import copy import time import roslib -roslib.load_manifest('mycobot_ros') + +roslib.load_manifest("mycobot_ros") -msg = """ +msg = """\ Mycobot Teleop Keyboard Controller --------------------------- Movimg options(control coordinations [x,y,z,rx,ry,rz]): @@ -44,7 +43,7 @@ def vels(speed, turn): class Raw(object): - def __init__(self,stream): + def __init__(self, stream): self.stream = stream self.fd = self.stream.fileno() @@ -57,29 +56,28 @@ class Raw(object): def teleop_keyboard(): - rospy.init_node('teleop_keyboard') + rospy.init_node("teleop_keyboard") model = 0 speed = rospy.get_param("~speed", 50) - change_percent= rospy.get_param("~change_percent", 5) + change_percent = rospy.get_param("~change_percent", 5) - change_angle = 180 * change_percent / 100 + change_angle = 180 * change_percent / 100 change_len = 250 * change_percent / 100 - rospy.wait_for_service('get_joint_angles') - 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("get_joint_angles") + 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") try: - get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords) - set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords) - get_angles = rospy.ServiceProxy('get_joint_angles', GetAngles) - set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles) - switch_gripper = rospy.ServiceProxy( - 'switch_gripper_status', GripperStatus) + get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords) + set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords) + get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles) + set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles) + switch_gripper = rospy.ServiceProxy("switch_gripper_status", GripperStatus) except: - print('start error ...') + print("start error ...") exit(1) init_pose = [0, 0, 0, 0, 0, 0, speed] @@ -91,68 +89,66 @@ def teleop_keyboard(): res = get_coords() if res.x > 1: break - time.sleep(.1) + time.sleep(0.1) - record_coords = [ - res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model - ] + record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] print(record_coords) try: print(msg) print(vels(speed, change_percent)) - while(1): + while 1: try: # print("\r current coords: %s" % record_coords, end="") with Raw(sys.stdin): key = sys.stdin.read(1) - if key == 'q': + if key == "q": break - elif key in ['w', 'W']: + elif key in ["w", "W"]: record_coords[0] += change_len set_coords(*record_coords) - elif key in ['s', 'S']: + elif key in ["s", "S"]: record_coords[0] -= change_len set_coords(*record_coords) - elif key in ['a', 'A']: + elif key in ["a", "A"]: record_coords[1] -= change_len set_coords(*record_coords) - elif key in ['d', 'D']: + elif key in ["d", "D"]: record_coords[1] += change_len set_coords(*record_coords) - elif key in ['z', 'Z']: + elif key in ["z", "Z"]: record_coords[2] -= change_len set_coords(*record_coords) - elif key in ['x', 'X']: + elif key in ["x", "X"]: record_coords[2] += change_len set_coords(*record_coords) - elif key in ['u', 'U']: + elif key in ["u", "U"]: record_coords[3] += change_angle set_coords(*record_coords) - elif key in ['j', 'J']: + elif key in ["j", "J"]: record_coords[3] -= change_angle set_coords(*record_coords) - elif key in ['i', 'I']: + elif key in ["i", "I"]: record_coords[4] += change_angle set_coords(*record_coords) - elif key in ['k', 'K']: + elif key in ["k", "K"]: record_coords[4] -= change_angle set_coords(*record_coords) - elif key in ['o', 'O']: + elif key in ["o", "O"]: record_coords[5] += change_angle set_coords(*record_coords) - elif key in ['l', 'L']: + elif key in ["l", "L"]: record_coords[5] -= change_angle set_coords(*record_coords) - elif key in ['g', 'G']: + elif key in ["g", "G"]: switch_gripper(True) - elif key in ['h', 'H']: + elif key in ["h", "H"]: switch_gripper(False) - elif key == '1': + elif key == "1": rsp = set_angles(*init_pose) - elif key in '2': + elif key in "2": rsp = set_angles(*home_pose) - elif key in '3': + elif key in "3": rep = get_angles() home_pose[0] = rep.joint_1 home_pose[1] = rep.joint_2 diff --git a/scripts/mycobot_moveit/sync_plan.py b/scripts/mycobot_moveit/sync_plan.py index 913a642..c49cef9 100755 --- a/scripts/mycobot_moveit/sync_plan.py +++ b/scripts/mycobot_moveit/sync_plan.py @@ -18,13 +18,14 @@ def callback(data): data_list.append(value) mc.send_radians(data_list, 80) - + + def listener(): global mc - rospy.init_node('mycobot_reciver', anonymous=True) + rospy.init_node("mycobot_reciver", anonymous=True) - port = rospy.get_param('~port', '/dev/ttyUSB0') - baud = rospy.get_param('~baud', 115200) + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyCobot(port, baud) @@ -33,5 +34,6 @@ def listener(): # spin() simply keeps python from exiting until this node is stopped rospy.spin() -if __name__ == '__main__': - listener() \ No newline at end of file + +if __name__ == "__main__": + listener() diff --git a/scripts/mycobot_services.py b/scripts/mycobot_services.py index 5bd0cb3..e5e1054 100755 --- a/scripts/mycobot_services.py +++ b/scripts/mycobot_services.py @@ -10,22 +10,22 @@ mc = None def create_handle(): global mc - rospy.init_node('mycobot_services') - rospy.loginfo('start ...') - port = rospy.get_param('~port') - baud = rospy.get_param('~baud') + rospy.init_node("mycobot_services") + rospy.loginfo("start ...") + port = rospy.get_param("~port") + baud = rospy.get_param("~baud") rospy.loginfo("%s,%s" % (port, baud)) mc = MyCobot(port, baud) def create_services(): - rospy.Service('set_joint_angles', SetAngles, set_angles) - rospy.Service('get_joint_angles', GetAngles, get_angles) - 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.loginfo('ready') + rospy.Service("set_joint_angles", SetAngles, set_angles) + rospy.Service("get_joint_angles", GetAngles, get_angles) + 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.loginfo("ready") rospy.spin() @@ -66,7 +66,7 @@ def set_coords(req): if mc: mc.send_coords(coords, sp, mod) - + return SetCoordsResponse(True) @@ -98,7 +98,7 @@ def toggle_pump(req): return PumpStatusResponse(True) -robot_msg = ''' +robot_msg = """ MyCobot Status -------------------------------- Joint Limit: @@ -116,29 +116,30 @@ Servo Infomation: %s Servo Temperature: %s Atom Version: %s -''' +""" def output_robot_message(): connect_status = False - servo_infomation = 'unknown' - servo_temperature = 'unknown' - atom_version = 'unknown' + servo_infomation = "unknown" + servo_temperature = "unknown" + atom_version = "unknown" if mc: cn = mc.is_controller_connected() if cn == 1: connect_status = True - time.sleep(.1) + time.sleep(0.1) si = mc.is_all_servo_enable() if si == 1: - servo_infomation = 'all connected' + servo_infomation = "all connected" - print(robot_msg % (connect_status, servo_infomation, - servo_temperature, atom_version)) + print( + robot_msg % (connect_status, servo_infomation, servo_temperature, atom_version) + ) -if __name__ == '__main__': +if __name__ == "__main__": # print(MyCobot.__dict__) create_handle() output_robot_message() diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py index 7c0426a..d46f50e 100755 --- a/scripts/mycobot_topics.py +++ b/scripts/mycobot_topics.py @@ -7,64 +7,71 @@ import threading import rospy -from mycobot_ros.msg import (MycobotAngles, MycobotCoords, MycobotSetAngles, MycobotSetCoords, MycobotGripperStatus, MycobotPumpStatus) +from mycobot_ros.msg import ( + MycobotAngles, + MycobotCoords, + MycobotSetAngles, + MycobotSetCoords, + MycobotGripperStatus, + MycobotPumpStatus, +) from sensor_msgs.msg import JointState from pymycobot.mycobot import MyCobot -class Watcher: - """this class solves two problems with multithreaded - programs in Python, (1) a signal might be delivered - to any thread (which is just a malfeature) and (2) if - the thread that gets the signal is waiting, the signal - is ignored (which is a bug). - - The watcher is a concurrent process (not thread) that - waits for a signal and the process that contains the - threads. See Appendix A of The Little Book of Semaphores. - http://greenteapress.com/semaphores/ - - I have only tested this on Linux. I would expect it to - work on the Macintosh and not work on Windows. - """ - - def __init__(self): - """ Creates a child thread, which returns. The parent - thread waits for a KeyboardInterrupt and then kills - the child thread. - """ - self.child = os.fork() - if self.child == 0: - return - else: - self.watch() - - def watch(self): - try: - os.wait() - except KeyboardInterrupt: - # I put the capital B in KeyBoardInterrupt so I can - # tell when the Watcher gets the SIGINT - print 'KeyBoardInterrupt' - self.kill() - sys.exit() - - def kill(self): - try: - os.kill(self.child, signal.SIGKILL) - except OSError: pass +class Watcher: + """this class solves two problems with multithreaded + programs in Python, (1) a signal might be delivered + to any thread (which is just a malfeature) and (2) if + the thread that gets the signal is waiting, the signal + is ignored (which is a bug). + + The watcher is a concurrent process (not thread) that + waits for a signal and the process that contains the + threads. See Appendix A of The Little Book of Semaphores. + http://greenteapress.com/semaphores/ + + I have only tested this on Linux. I would expect it to + work on the Macintosh and not work on Windows. + """ + + def __init__(self): + """Creates a child thread, which returns. The parent + thread waits for a KeyboardInterrupt and then kills + the child thread. + """ + self.child = os.fork() + if self.child == 0: + return + else: + self.watch() + + def watch(self): + try: + os.wait() + except KeyboardInterrupt: + # I put the capital B in KeyBoardInterrupt so I can + # tell when the Watcher gets the SIGINT + print("KeyBoardInterrupt") + self.kill() + sys.exit() + + def kill(self): + try: + os.kill(self.child, signal.SIGKILL) + except OSError: + pass class MycobotTopics(object): - def __init__(self): super(MycobotTopics, self).__init__() - rospy.init_node('mycobot_topics') - rospy.loginfo('start ...') - port = rospy.get_param('~port', '/dev/ttyUSB0') - baud = rospy.get_param('~baud', 115200) + rospy.init_node("mycobot_topics") + rospy.loginfo("start ...") + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) self.mc = MyCobot(port, baud) self.lock = threading.Lock() @@ -89,16 +96,16 @@ class MycobotTopics(object): sg.start() sp.setDaemon(True) sp.start() - + pa.join() pb.join() sa.join() sb.join() sg.join() sp.join() - + def pub_real_angles(self): - pub = rospy.Publisher('mycobot/angles_real', MycobotAngles, queue_size=5) + pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5) ma = MycobotAngles() while not rospy.is_shutdown(): self.lock.acquire() @@ -112,10 +119,10 @@ class MycobotTopics(object): ma.joint_5 = angles[4] ma.joint_6 = angles[5] pub.publish(ma) - time.sleep(.25) + time.sleep(0.25) def pub_real_coords(self): - pub = rospy.Publisher('mycobot/coords_real', MycobotCoords, queue_size=5) + pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5) ma = MycobotCoords() while not rospy.is_shutdown(): @@ -130,15 +137,24 @@ class MycobotTopics(object): ma.ry = coords[4] ma.rz = coords[5] pub.publish(ma) - time.sleep(.25) + time.sleep(0.25) def sub_set_angles(self): def callback(data): - angles = [data.joint_1, data.joint_2, data.joint_3, data.joint_4, data.joint_5, data.joint_6] + angles = [ + data.joint_1, + data.joint_2, + data.joint_3, + data.joint_4, + data.joint_5, + data.joint_6, + ] sp = int(data.speed) self.mc.send_angles(angles, sp) - sub = rospy.Subscriber('mycobot/angles_goal', MycobotSetAngles, callback=callback) + sub = rospy.Subscriber( + "mycobot/angles_goal", MycobotSetAngles, callback=callback + ) rospy.spin() def sub_set_coords(self): @@ -148,7 +164,9 @@ class MycobotTopics(object): model = int(data.model) self.mc.send_coords(angles, sp, model) - sub = rospy.Subscriber('mycobot/coords_goal', MycobotSetCoords, callback=callback) + sub = rospy.Subscriber( + "mycobot/coords_goal", MycobotSetCoords, callback=callback + ) rospy.spin() def sub_gripper_status(self): @@ -158,7 +176,9 @@ class MycobotTopics(object): else: self.mc.set_gripper_state(1, 80) - sub = rospy.Subscriber('mycobot/gripper_status', MycobotGripperStatus, callback=callback) + sub = rospy.Subscriber( + "mycobot/gripper_status", MycobotGripperStatus, callback=callback + ) rospy.spin() def sub_pump_status(self): @@ -170,15 +190,17 @@ class MycobotTopics(object): self.mc.set_basic_output(2, 1) self.mc.set_basic_output(5, 1) - sub = rospy.Subscriber('mycobot/pump_status', MycobotPumpStatus, callback=callback) + sub = rospy.Subscriber( + "mycobot/pump_status", MycobotPumpStatus, callback=callback + ) rospy.spin() -if __name__ == '__main__': +if __name__ == "__main__": Watcher() mc_topics = MycobotTopics() mc_topics.start() # while True: # mc_topics.pub_real_coords() # mc_topics.sub_set_angles() - pass \ No newline at end of file + pass diff --git a/scripts/slider_600.py b/scripts/slider_600.py index 1125824..24e624d 100755 --- a/scripts/slider_600.py +++ b/scripts/slider_600.py @@ -14,7 +14,7 @@ mutex = Lock() class ElephantRobot(object): - def __init__(self,host,port): + def __init__(self, host, port): # setup connection self.BUFFSIZE = 2048 self.ADDR = (host, port) @@ -23,28 +23,28 @@ class ElephantRobot(object): def start_client(self): try: self.tcp_client.connect(self.ADDR) - return '' + return "" except error, e: return e def stop_client(self): self.tcp_client.close() - def send_command(self,command): + def send_command(self, command): with mutex: self.tcp_client.send(command.encode()) recv_data = self.tcp_client.recv(self.BUFFSIZE).decode() res_str = str(recv_data) - print 'recv = ' + res_str + print "recv = " + res_str res_arr = res_str.split(":") if len(res_arr) == 2: return res_arr[1] else: - return '' + return "" def string_to_coords(self, data): - data = data.replace('[','') - data = data.replace(']','') + data = data.replace("[", "") + data = data.replace("]", "") data_arr = data.split(",") if len(data_arr) == 6: try: @@ -54,8 +54,7 @@ class ElephantRobot(object): coords_4 = float(data_arr[3]) coords_5 = float(data_arr[4]) coords_6 = float(data_arr[5]) - coords = [coords_1, coords_2, coords_3, - coords_4, coords_5, coords_6] + coords = [coords_1, coords_2, coords_3, coords_4, coords_5, coords_6] return coords except: return invalid_coords() @@ -80,80 +79,80 @@ class ElephantRobot(object): return coords def get_angles(self): - command = 'get_angles()\n' + command = "get_angles()\n" res = self.send_command(command) return self.string_to_coords(res) def get_coords(self): - command = 'get_coords()\n' + command = "get_coords()\n" res = self.send_command(command) return self.string_to_coords(res) def get_speed(self): - command = 'get_speed()\n' + command = "get_speed()\n" res = self.send_command(command) return self.string_to_double(res) def power_on(self): - command = 'power_on()\n' + command = "power_on()\n" res = self.send_command(command) return True def power_off(self): - command = 'power_off()\n' + command = "power_off()\n" res = self.send_command(command) return True def check_running(self): - command = 'check_running()\n' + command = "check_running()\n" res = self.send_command(command) - return res == '1' + return res == "1" def state_check(self): - command = 'state_check()\n' + command = "state_check()\n" res = self.send_command(command) - return res == '1' + return res == "1" def program_open(self, file_path): - command = 'program_open(' + file_path + ")\n" + command = "program_open(" + file_path + ")\n" res = self.send_command(command) return self.string_to_int(res) def program_run(self, start_line): - command = 'program_run(' + str(start_line) + ")\n" + command = "program_run(" + str(start_line) + ")\n" res = self.send_command(command) return self.string_to_int(res) def read_next_error(self): - command = 'read_next_error()\n' + command = "read_next_error()\n" res = self.send_command(command) return res - def write_coords(self,coords,speed): + def write_coords(self, coords, speed): command = "set_coords(" for item in coords: - command += str(item) + ',' - command += str(speed) + ')\n' + command += str(item) + "," + command += str(speed) + ")\n" self.send_command(command) def write_coord(self, axis, value, speed): coords = self.get_coords() if coords != self.invalid_coords(): coords[axis] = value - self.write_coords(coords,speed) + self.write_coords(coords, speed) - def write_angles(self,angles,speed): + def write_angles(self, angles, speed): command = "set_angles(" for item in angles: - command += str(item) + ',' - command += str(speed) + ')\n' + command += str(item) + "," + command += str(speed) + ")\n" self.send_command(command) def write_angle(self, joint, value, speed): angles = self.get_angles() if angles != self.invalid_coords(): angles[joint] = value - self.write_angles(angles,speed) + self.write_angles(angles, speed) def set_speed(self, percentage): command = "set_speed(" + str(percentage) + ")\n" @@ -187,11 +186,15 @@ class ElephantRobot(object): self.send_command(command) def jog_angle(self, joint_str, direction, speed): - command = "jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n" + command = ( + "jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n" + ) self.send_command(command) def jog_coord(self, axis_str, direction, speed): - command = "jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n" + command = ( + "jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n" + ) self.send_command(command) def get_digital_in(self, pin_number): @@ -228,7 +231,7 @@ class ElephantRobot(object): self.send_command(command) def assign_variable(self, var_name, var_value): - command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ')\n' + command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ")\n" self.send_command(command) def get_variable(self, var_name): @@ -236,41 +239,41 @@ class ElephantRobot(object): return self.send_command(command) - old_list = [] + def callback(data): global old_list - #rospy.loginfo(rospy.get_caller_id() + "%s", data.position) - print('position', data.position) + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + print ("position", data.position) data_list = [] for index, value in enumerate(data.position): value = value * 180 / math.pi data_list.append(value) - print('data', data_list) + print ("data", data_list) if not old_list: old_list = data_list mc.write_angles(data_list, 1999) elif old_list != data_list: old_list = data_list - if (mc.check_running()): + if mc.check_running(): mc.task_stop() time.sleep(0.05) - + mc.write_angles(data_list, 1999) def listener(): global mc - rospy.init_node('control_slider', anonymous=True) + rospy.init_node("control_slider", anonymous=True) - ip = rospy.get_param('~ip', '192.168.10.169') - print(ip) + ip = rospy.get_param("~ip", "192.168.10.169") + print (ip) mc = ElephantRobot(ip, 5001) # START CLIENT res = mc.start_client() - if res != '': + if res != "": print res sys.exit(1) print ep.wait(5) @@ -282,9 +285,9 @@ def listener(): rospy.Subscriber("joint_states", JointState, callback) # spin() simply keeps python from exiting until this node is stopped - print('sping ...') + print ("sping ...") rospy.spin() -if __name__ == '__main__': +if __name__ == "__main__": listener() diff --git a/scripts/test.py b/scripts/test.py index 232a3a9..44496a3 100644 --- a/scripts/test.py +++ b/scripts/test.py @@ -1,71 +1,69 @@ #!/usr/bin/env python -''' +""" This package need `pymycobot`. This file for test the API if right. Just can run in Linux. -''' +""" import time, random, subprocess from pymycobot.mycobot import MyCobot from pymycobot.genre import Angle, Coord -if __name__ == '__main__': - sys_ = subprocess.check_output(['uname'], - shell=True).decode() - if not sys_ == 'Linux': - print('This script just can run on Linux.') +if __name__ == "__main__": + sys_ = subprocess.check_output(["uname"], shell=True).decode() + if not sys_ == "Linux": + print("This script just can run on Linux.") exit(0) - port = subprocess.check_output(['echo -n /dev/ttyUSB*'], - shell=True).decode() + port = subprocess.check_output(["echo -n /dev/ttyUSB*"], shell=True).decode() mycobot = MyCobot(port) - print('Start check api\n') + print("Start check api\n") - print('::get_angles()') - print('==> degrees: {}\n'.format(mycobot.get_angles())) + print("::get_angles()") + print("==> degrees: {}\n".format(mycobot.get_angles())) time.sleep(0.5) - print('::get_radians()') - print('==> radians: {}\n'.format(mycobot.get_radians())) + print("::get_radians()") + print("==> radians: {}\n".format(mycobot.get_radians())) time.sleep(0.5) - print('::send_angles()') - mycobot.send_angles([0,0,0,0,0,0], 80) - print('==> set angles [0,0,0,0,0,0], speed 80\n') - print('Is moving: {}'.format(mycobot.is_moving())) + print("::send_angles()") + mycobot.send_angles([0, 0, 0, 0, 0, 0], 80) + print("==> set angles [0,0,0,0,0,0], speed 80\n") + print("Is moving: {}".format(mycobot.is_moving())) time.sleep(3) - print('::send_radians') - mycobot.send_radians([1,1,1,1,1,1], 70) - print('==> set raidans [1,1,1,1,1,1], speed 70\n') + print("::send_radians") + mycobot.send_radians([1, 1, 1, 1, 1, 1], 70) + print("==> set raidans [1,1,1,1,1,1], speed 70\n") time.sleep(1.5) - print('::send_angle()') + print("::send_angle()") mycobot.send_angle(Angle.J2.value, 10, 50) - print('==> angle: joint2, degree: 10, speed: 50\n') + print("==> angle: joint2, degree: 10, speed: 50\n") time.sleep(1) - print('::get_coords()') - print('==> coords {}\n'.format(mycobot.get_coords())) + print("::get_coords()") + print("==> coords {}\n".format(mycobot.get_coords())) time.sleep(0.5) - print('::send_coords()') + print("::send_coords()") coord_list = [160, 160, 160, 0, 0, 0] mycobot.send_coords(coord_list, 70, 0) - print('==> send coords [160,160,160,0,0,0], speed 70, mode 0\n') + print("==> send coords [160,160,160,0,0,0], speed 70, mode 0\n") time.sleep(3.0) print(mycobot.is_in_position(coord_list, 1)) time.sleep(1) - print('::send_coord()') + print("::send_coord()") mycobot.send_coord(Coord.X.value, -40, 70) - print('==> send coord id: X, coord value: -40, speed: 70\n') + print("==> send coord id: X, coord value: -40, speed: 70\n") time.sleep(2) - print('::release_all_servos()') + print("::release_all_servos()") mycobot.release_all_servos() - print('==> into free moving mode.') - print('=== check end <==\n') + print("==> into free moving mode.") + print("=== check end <==\n") diff --git a/src/camera_display.cpp b/src/camera_display.cpp index 8df928d..1f3abeb 100644 --- a/src/camera_display.cpp +++ b/src/camera_display.cpp @@ -1,30 +1,29 @@ -#include -#include -#include -#include +#include +#include +#include +#include -void imageCallback(const sensor_msgs::ImageConstPtr& msg) -{ - try - { - cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); - cv::waitKey(30); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); - } -} +void imageCallback(const sensor_msgs::ImageConstPtr &msg) +{ + try + { + cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); + cv::waitKey(30); + } + catch (cv_bridge::Exception &e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } +} -int main(int argc, char **argv) -{ - ros::init(argc, argv, "image_listener"); - ros::NodeHandle nh; - cv::namedWindow("view"); - cv::startWindowThread(); - image_transport::ImageTransport it(nh); - image_transport::Subscriber sub = it.subscribe("camera/image", 1,imageCallback); - ros::spin(); - cv::destroyWindow("view"); - -} \ No newline at end of file +int main(int argc, char **argv) +{ + ros::init(argc, argv, "image_listener"); + ros::NodeHandle nh; + cv::namedWindow("view"); + cv::startWindowThread(); + image_transport::ImageTransport it(nh); + image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback); + ros::spin(); + cv::destroyWindow("view"); +} diff --git a/src/opencv_camera.cpp b/src/opencv_camera.cpp index 9ace009..44b4522 100644 --- a/src/opencv_camera.cpp +++ b/src/opencv_camera.cpp @@ -1,60 +1,59 @@ -#include -#include -#include -#include -#include // for converting the command line parameter to integer +#include +#include +#include +#include +#include // for converting the command line parameter to integer -int main(int argc, char** argv) -{ - // Check if video source has been passed as a parameter - if(argv[1] == NULL) - { - ROS_INFO("argv[1]=NULL\n"); - return 1; - } +int main(int argc, char **argv) +{ + // Check if video source has been passed as a parameter + if (argv[1] == NULL) + { + ROS_INFO("argv[1]=NULL\n"); + return 1; + } ros::init(argc, argv, "image_publisher"); // Initialize node - ros::NodeHandle nh; - image_transport::ImageTransport it(nh); - image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic + ros::NodeHandle nh; + image_transport::ImageTransport it(nh); + image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic ros::Rate loop_rate(200); // refresh Hz. - // Convert the passed as command line parameter index for the video device to an integer - std::istringstream video_sourceCmd(argv[1]); - int video_source; - // Check if it is indeed a number - if(!(video_sourceCmd >> video_source)) - { - ROS_INFO("video_sourceCmd is %d\n",video_source); - return 1; - } - - cv::VideoCapture cap(video_source); - // Check if video device can be opened with the given index - if(!cap.isOpened()) - { - ROS_INFO("can not opencv video device\n"); - return 1; - } - cv::Mat frame; - sensor_msgs::ImagePtr msg; - - while (nh.ok()) - { - cap >> frame; - // cv::imshow("veiwer", frame); - // Check if grabbed frame is actually full with some content - if(!frame.empty()) - { - msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); - pub.publish(msg); - //cv::Wait(1); - } - ros::spinOnce(); - loop_rate.sleep(); - // if(cv::waitKey(2) >= 0) - // break; + // Convert the passed as command line parameter index for the video device to an integer + std::istringstream video_sourceCmd(argv[1]); + int video_source; + // Check if it is indeed a number + if (!(video_sourceCmd >> video_source)) + { + ROS_INFO("video_sourceCmd is %d\n", video_source); + return 1; } - -} \ No newline at end of file + + cv::VideoCapture cap(video_source); + // Check if video device can be opened with the given index + if (!cap.isOpened()) + { + ROS_INFO("can not opencv video device\n"); + return 1; + } + cv::Mat frame; + sensor_msgs::ImagePtr msg; + + while (nh.ok()) + { + cap >> frame; + // cv::imshow("veiwer", frame); + // Check if grabbed frame is actually full with some content + if (!frame.empty()) + { + msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); + pub.publish(msg); + //cv::Wait(1); + } + ros::spinOnce(); + loop_rate.sleep(); + // if(cv::waitKey(2) >= 0) + // break; + } +}