From bf8eceb407a31dbc704a210241ce55c4a0a0bba6 Mon Sep 17 00:00:00 2001 From: Zachary Date: Tue, 10 Aug 2021 14:02:12 +0800 Subject: [PATCH] add script comment. --- mycobot_280/scripts/detect_marker.py | 19 +++++++++++++++---- mycobot_280/scripts/listen_real.py | 6 ++++-- mycobot_280/scripts/listen_real_of_topic.py | 16 ++++++++++------ mycobot_280/scripts/slider_control.py | 11 +++++++++-- 4 files changed, 38 insertions(+), 14 deletions(-) diff --git a/mycobot_280/scripts/detect_marker.py b/mycobot_280/scripts/detect_marker.py index a577004..04ca483 100755 --- a/mycobot_280/scripts/detect_marker.py +++ b/mycobot_280/scripts/detect_marker.py @@ -27,6 +27,7 @@ class ImageConverter: ) self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() self.camera_matrix = None + # subscriber, listen wether has img come in. self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) def callback(self, data): @@ -36,6 +37,7 @@ class ImageConverter: pose to transforming. """ try: + # trans `rgb` to `gbr` for opencv. cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) @@ -43,6 +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. self.camera_matrix = np.array( [ [focal_length, 0, center[0]], @@ -52,12 +55,18 @@ class ImageConverter: 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 - ) + # detect aruco marker. + ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) + corners, ids = ret[0], ret[1] + # process marker data. if len(corners) > 0: if ids is not None: # print('corners:', corners, 'ids:', ids) + + # detect marker pose. + # argument: + # marker corners + # marker size (meter) ret = cv.aruco.estimatePoseSingleMarkers( corners, 0.05, self.camera_matrix, self.dist_coeffs ) @@ -66,6 +75,7 @@ class ImageConverter: print("rvec:", rvec, "tvec:", tvec) + # just select first one detected marker. for i in range(rvec.shape[0]): cv.aruco.drawDetectedMarkers(cv_image, corners) cv.aruco.drawAxis( @@ -77,16 +87,17 @@ class ImageConverter: 0.03, ) - # Just process first one detected. xyz = tvec[0, 0, :] xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] + # get quaternion for 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] self.br.sendTransform( xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" ) diff --git a/mycobot_280/scripts/listen_real.py b/mycobot_280/scripts/listen_real.py index a429714..d022c72 100755 --- a/mycobot_280/scripts/listen_real.py +++ b/mycobot_280/scripts/listen_real.py @@ -30,14 +30,14 @@ def talker(): 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) rospy.loginfo("start loop ...") while not rospy.is_shutdown(): - joint_state_send.header.stamp = rospy.Time.now() - + # get real angles from server. res = func() if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: continue @@ -51,6 +51,8 @@ def talker(): ] rospy.loginfo("res: {}".format(radians_list)) + # publish angles. + joint_state_send.header.stamp = rospy.Time.now() joint_state_send.position = radians_list pub.publish(joint_state_send) rate.sleep() diff --git a/mycobot_280/scripts/listen_real_of_topic.py b/mycobot_280/scripts/listen_real_of_topic.py index 6f1c119..169567a 100755 --- a/mycobot_280/scripts/listen_real_of_topic.py +++ b/mycobot_280/scripts/listen_real_of_topic.py @@ -1,6 +1,4 @@ #!/usr/bin/env python2 -# license removed for brevity -import time import math import rospy @@ -15,13 +13,19 @@ class Listener(object): rospy.loginfo("start ...") rospy.init_node("real_listener_1", anonymous=True) + # init publisher. self.pub = rospy.Publisher("joint_states", JointState, queue_size=10) + # init subscriber. self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback) - rate = rospy.Rate(30) # 30hz rospy.spin() def callback(self, data): - # pub joint state + """`mycobot/angles_real` subscriber callback method. + + Args: + data (MycobotAngles): callback argument. + """ + # ini publisher object. joint_state_send = JointState() joint_state_send.header = Header() @@ -35,8 +39,9 @@ class Listener(object): ] joint_state_send.velocity = [0] joint_state_send.effort = [] - joint_state_send.header.stamp = rospy.Time.now() + + # process callback data. radians_list = [ data.joint_1 * (math.pi / 180), data.joint_2 * (math.pi / 180), @@ -49,7 +54,6 @@ class Listener(object): joint_state_send.position = radians_list self.pub.publish(joint_state_send) - # rate.sleep() if __name__ == "__main__": diff --git a/mycobot_280/scripts/slider_control.py b/mycobot_280/scripts/slider_control.py index 04f4d83..6491795 100755 --- a/mycobot_280/scripts/slider_control.py +++ b/mycobot_280/scripts/slider_control.py @@ -1,6 +1,13 @@ #!/usr/bin/env python2 -# from std_msgs.msg import String -import time + +"""[summary] +This file obtains the joint angle of the manipulator in ROS, +and then sends it directly to the real manipulator using `pymycobot` API. +This file is [slider_control.launch] related script. +Passable parameters: + port: serial prot string. Defaults is '/dev/ttyUSB0' + baud: serial prot baudrate. Defaults is 115200. +""" import rospy from sensor_msgs.msg import JointState