From 8cc1b7c0b1b1dccd171858baa3dad0785c205834 Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Mon, 24 May 2021 16:06:33 +0800 Subject: [PATCH 1/7] improve vision recogonization. --- CMakeLists.txt | 1 + ...mycobot_detect_marker_with_services.launch | 22 ++++ scripts/detect_marker.py | 1 + scripts/following_marker.py | 2 +- scripts/gripper_control.py | 111 ++++++++++++++++++ 5 files changed, 136 insertions(+), 1 deletion(-) create mode 100644 launch/mycobot_detect_marker_with_services.launch create mode 100755 scripts/gripper_control.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f87f48..8ed4cf7 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,7 @@ catkin_install_python(PROGRAMS scripts/client.py scripts/detect_marker.py scripts/following_marker.py + scripts/gripper_control.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/launch/mycobot_detect_marker_with_services.launch b/launch/mycobot_detect_marker_with_services.launch new file mode 100644 index 0000000..a13c536 --- /dev/null +++ b/launch/mycobot_detect_marker_with_services.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/scripts/detect_marker.py b/scripts/detect_marker.py index b61710d..b738dfb 100755 --- a/scripts/detect_marker.py +++ b/scripts/detect_marker.py @@ -64,6 +64,7 @@ class image_converter: cv.aruco.drawAxis(cv_image, self.camera_matrix, self.dist_coeffs, rvec[i, :, :], tvec[i, :, :], 0.03) xyz = tvec[0, 0, :] + xyz = [xyz[0] - 0.035, xyz[1], xyz[2] - 0.03] euler = rvec[0, 0, :] tf_change = tf.transformations.quaternion_from_euler(euler[0], euler[1], euler[2]) diff --git a/scripts/following_marker.py b/scripts/following_marker.py index 8f6be50..dcafbd2 100755 --- a/scripts/following_marker.py +++ b/scripts/following_marker.py @@ -48,7 +48,7 @@ def talker(): # marker position initial marker_.pose.position.x = trans[0] marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - 0.02 + marker_.pose.position.z = trans[2] marker_.pose.orientation.x = rot[0] marker_.pose.orientation.y = rot[1] marker_.pose.orientation.z = rot[2] diff --git a/scripts/gripper_control.py b/scripts/gripper_control.py new file mode 100755 index 0000000..6baac16 --- /dev/null +++ b/scripts/gripper_control.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python2 +# license removed for brevity + + +import rospy +from visualization_msgs import msg +from visualization_msgs.msg import Marker +# from pymycobot.mycobot import MyCobot +# from pymycobot.genre import Coord +# from pymycobot import PI_PORT, PI_BAUD # For raspberry pi version of mycobot. +import time +from mycobot_ros.srv import ( + GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) + + +set_coords = None +set_angles = None + +x_offset = 0 +y_offset = 30 +z_offset = 60 + +flag = False + + +def grippercallback(data): + global flag + print(type(data)) + # rospy.loginfo('gripper_subscriber get date :%s', data) + # print(Marker.) + # coord_datas = mc.get_coords() + # print(coord_datas) + if flag: + 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')) + + + print(x, y, z) + + # detect heigth + pump height + limit height + offset + x += x_offset + y += y_offset + z = z + 88 + 25 + z_offset + + try: + set_coords(x, y, z, -175, 0, -90, 70, 2) + time.sleep(2.5) + except Exception: + pass + + + for i in range(1,4): + try: + set_coords(x, y, z - i * 10, -175, 0, -90, 70, 2) + time.sleep(.2) + except Exception: + pass + + # pump on + try: + set_coords(x, y, z + 20, -175, 0, -90, 70, 2) + time.sleep(2.5) + except Exception: + pass + + # ... + + # finally + flag = True + + + + + + +def gipper_subscriber(): + global set_coords, set_angles + # 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') + 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) + except: + print('start error ...') + exit(1) + + try: + set_angles(0, 30, -50, -40, 0, 0, 50) + except Exception: + pass + time.sleep(2.5) + rospy.init_node('gipper_subscriber',anonymous=True) + rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1) + print 'gripper test' + rospy.spin() + + + + +if __name__ == '__main__': + gipper_subscriber() + \ No newline at end of file From 21731507cff0b1b40caedec5a8c60a088edbdbe6 Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Tue, 25 May 2021 14:42:31 +0800 Subject: [PATCH 2/7] fix: Service cannot process error. --- CMakeLists.txt | 1 + scripts/mycobot_services.py | 19 +++++++++++++++++++ srv/GetAngles.srv | 3 --- srv/GripperStatus.srv | 4 +++- srv/PumpStatus.srv | 5 +++++ srv/SetAngles.srv | 4 +++- srv/SetCoords.srv | 4 +++- 7 files changed, 34 insertions(+), 6 deletions(-) create mode 100644 srv/PumpStatus.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index b135055..6754309 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ add_service_files(FILES GetCoords.srv SetCoords.srv GripperStatus.srv + PumpStatus.srv ) ## Generate added messages and services diff --git a/scripts/mycobot_services.py b/scripts/mycobot_services.py index b6f2940..5bd0cb3 100755 --- a/scripts/mycobot_services.py +++ b/scripts/mycobot_services.py @@ -24,6 +24,7 @@ def create_services(): rospy.Service('set_joint_coords', SetCoords, set_coords) rospy.Service('get_joint_coords', GetCoords, get_coords) rospy.Service('switch_gripper_status', GripperStatus, switch_status) + rospy.Service('switch_pump_status', PumpStatus, toggle_pump) rospy.loginfo('ready') rospy.spin() @@ -42,6 +43,8 @@ def set_angles(req): if mc: mc.send_angles(angles, sp) + return SetAnglesResponse(True) + def get_angles(req): if mc: @@ -63,6 +66,8 @@ def set_coords(req): if mc: mc.send_coords(coords, sp, mod) + + return SetCoordsResponse(True) def get_coords(req): @@ -78,6 +83,20 @@ def switch_status(req): else: mc.set_gripper_state(1, 80) + return GripperStatusResponse(True) + + +def toggle_pump(req): + if mc: + if req.Status: + mc.set_basic_output(2, 0) + mc.set_basic_output(5, 0) + else: + mc.set_basic_output(2, 1) + mc.set_basic_output(5, 1) + + return PumpStatusResponse(True) + robot_msg = ''' MyCobot Status diff --git a/srv/GetAngles.srv b/srv/GetAngles.srv index 6992699..9aa7d97 100644 --- a/srv/GetAngles.srv +++ b/srv/GetAngles.srv @@ -1,6 +1,3 @@ - ---- - float32 joint_1 float32 joint_2 float32 joint_3 diff --git a/srv/GripperStatus.srv b/srv/GripperStatus.srv index 424572f..3887d75 100644 --- a/srv/GripperStatus.srv +++ b/srv/GripperStatus.srv @@ -1,3 +1,5 @@ bool Status ---- \ No newline at end of file +--- + +bool Flag \ No newline at end of file diff --git a/srv/PumpStatus.srv b/srv/PumpStatus.srv new file mode 100644 index 0000000..3887d75 --- /dev/null +++ b/srv/PumpStatus.srv @@ -0,0 +1,5 @@ +bool Status + +--- + +bool Flag \ No newline at end of file diff --git a/srv/SetAngles.srv b/srv/SetAngles.srv index 1ad93cd..6f8d877 100644 --- a/srv/SetAngles.srv +++ b/srv/SetAngles.srv @@ -7,4 +7,6 @@ float32 joint_6 int8 speed ---- \ No newline at end of file +--- + +bool Flag \ No newline at end of file diff --git a/srv/SetCoords.srv b/srv/SetCoords.srv index b264760..e5b1e52 100644 --- a/srv/SetCoords.srv +++ b/srv/SetCoords.srv @@ -8,4 +8,6 @@ float32 rz int8 speed int8 model ---- \ No newline at end of file +--- + +bool Flag \ No newline at end of file From 2f93c0a79fcdc210e05b9564a67fda833e75895e Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Tue, 25 May 2021 15:54:53 +0800 Subject: [PATCH 3/7] feat: add somt topics. --- CMakeLists.txt | 7 +- msg/MycobotAngles.msg | 6 + msg/MycobotCoords.msg | 6 + msg/{Gripper.msg => MycobotGripperStatus.msg} | 0 msg/MycobotPumpStatus.msg | 1 + msg/MycobotSetAngles.msg | 8 + msg/MycobotSetCoords.msg | 9 ++ scripts/mycobot_topics.py | 137 ++++++++++++++++++ 8 files changed, 173 insertions(+), 1 deletion(-) create mode 100644 msg/MycobotAngles.msg create mode 100644 msg/MycobotCoords.msg rename msg/{Gripper.msg => MycobotGripperStatus.msg} (100%) create mode 100644 msg/MycobotPumpStatus.msg create mode 100644 msg/MycobotSetAngles.msg create mode 100644 msg/MycobotSetCoords.msg create mode 100644 scripts/mycobot_topics.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 6754309..5eec60b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,12 @@ find_package(catkin REQUIRED COMPONENTS ) add_message_files(FILES - Gripper.msg + MycobotAngles.msg + MycobotCoords.msg + MycobotSetAngles.msg + MycobotSetCoords.msg + MycobotGripperStatus.msg + MycobotPumpStatus.msg ) add_service_files(FILES diff --git a/msg/MycobotAngles.msg b/msg/MycobotAngles.msg new file mode 100644 index 0000000..9aa7d97 --- /dev/null +++ b/msg/MycobotAngles.msg @@ -0,0 +1,6 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 diff --git a/msg/MycobotCoords.msg b/msg/MycobotCoords.msg new file mode 100644 index 0000000..08f6dbd --- /dev/null +++ b/msg/MycobotCoords.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz \ No newline at end of file diff --git a/msg/Gripper.msg b/msg/MycobotGripperStatus.msg similarity index 100% rename from msg/Gripper.msg rename to msg/MycobotGripperStatus.msg diff --git a/msg/MycobotPumpStatus.msg b/msg/MycobotPumpStatus.msg new file mode 100644 index 0000000..635a864 --- /dev/null +++ b/msg/MycobotPumpStatus.msg @@ -0,0 +1 @@ +bool Status diff --git a/msg/MycobotSetAngles.msg b/msg/MycobotSetAngles.msg new file mode 100644 index 0000000..3888b50 --- /dev/null +++ b/msg/MycobotSetAngles.msg @@ -0,0 +1,8 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 + +int8 speed diff --git a/msg/MycobotSetCoords.msg b/msg/MycobotSetCoords.msg new file mode 100644 index 0000000..d4c7e54 --- /dev/null +++ b/msg/MycobotSetCoords.msg @@ -0,0 +1,9 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz + +int8 speed +int8 model diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py new file mode 100644 index 0000000..d9738e5 --- /dev/null +++ b/scripts/mycobot_topics.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python2 +import time +import threading + +import rospy + +from mycobot_ros.msg import (MycobotAngles, MycobotCoords, MycobotSetAngles, MycobotSetCoords, MycobotGripperStatus, MycobotPumpStatus) +from sensor_msgs.msg import JointState + +from pymycobot.mycobot import MyCobot + + +class MycobotTopics(object): + + def __init__(self): + super(MycobotTopics, self).__init__() + + rospy.init_node('mycobot_topics') + rospy.loginfo('start ...') + port = rospy.get_param('~port') + baud = rospy.get_param('~baud') + rospy.loginfo("%s,%s" % (port, baud)) + self.mc = MyCobot(port, baud) + self.lock = threading.Lock() + + def start(self): + pa = threading.Thread(target=self.pub_real_angles) + pb = threading.Thread(target=self.pub_real_coords) + sa = threading.Thread(target=self.sub_set_angles) + sb = threading.Thread(target=self.sub_set_coords) + sg = threading.Thread(target=self.sub_gripper_status) + sp = threading.Thread(target=self.sub_pump_status) + + pa.setDaemon(True) + pa.start() + pb.setDaemon(True) + pb.start() + sa.setDaemon(True) + sa.start() + sb.setDaemon(True) + sb.start() + sg.setDaemon(True) + 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) + ma = MycobotAngles() + while True: + self.lock.acquire() + angles = self.mc.get_angles() + self.lock.release() + if angles: + ma.joint_1 = angles[0] + ma.joint_2 = angles[1] + ma.joint_3 = angles[2] + ma.joint_4 = angles[3] + ma.joint_5 = angles[4] + ma.joint_6 = angles[5] + pub.publish(ma) + time.sleep(.25) + + def pub_real_coords(self): + pub = rospy.Publisher('mycobot/coords_real', MycobotCoords, queue_size=5) + ma = MycobotCoords() + + while True: + self.lock.acquire() + coords = self.mc.get_coords() + self.lock.release() + if coords: + ma.x = coords[0] + ma.y = coords[1] + ma.z = coords[2] + ma.rx = coords[3] + ma.ry = coords[4] + ma.rz = coords[5] + pub.publish(ma) + time.sleep(.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] + sp = int(data.speed) + self.mc.send_angles(angles, sp) + + sub = rospy.Subscriber('mycobot/angles_goal', MycobotSetAngles, callback=callback) + rospy.spin() + + def sub_set_coords(self): + def callback(data): + angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] + sp = int(data.speed) + model = int(data.model) + self.mc.send_angles(angles, sp, model) + + sub = rospy.Subscriber('mycobot/coords_goal', MycobotSetCoords, callback=callback) + rospy.spin() + + def sub_gripper_status(self): + def callback(data): + if data.Status: + self.mc.set_gripper_state(0, 80) + else: + self.mc.set_gripper_state(1, 80) + + sub = rospy.Subscriber('mycobot/gripper_status', MycobotGripperStatus, callback=callback) + rospy.spin() + + def sub_pump_status(self): + def callback(data): + if data.Status: + self.mc.set_basic_output(2, 0) + self.mc.set_basic_output(5, 0) + else: + self.mc.set_basic_output(2, 1) + self.mc.set_basic_output(5, 1) + + sub = rospy.Subscriber('mycobot/pump_status', MycobotPumpStatus, callback=callback) + rospy.spin() + + +if __name__ == '__main__': + 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 From 5cec7311587b76a71dabf1a3726b4a34994367fd Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Thu, 27 May 2021 11:51:42 +0800 Subject: [PATCH 4/7] feat: Improve mycobot topics, add launch file. - add listen real angle script of topic. - fix srv error. --- CMakeLists.txt | 2 ++ launch/communication_topic.launch | 10 ++++++ scripts/listen_real.py | 2 ++ scripts/listen_real_of_topic.py | 60 +++++++++++++++++++++++++++++++ scripts/mycobot_topics.py | 55 +++++++++++++++++++++++++--- srv/GetAngles.srv | 3 ++ 6 files changed, 128 insertions(+), 4 deletions(-) create mode 100644 launch/communication_topic.launch create mode 100755 scripts/listen_real_of_topic.py mode change 100644 => 100755 scripts/mycobot_topics.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 5eec60b..a932d7d 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,9 @@ catkin_install_python(PROGRAMS scripts/slider_control.py scripts/control_marker.py scripts/mycobot_services.py + scripts/mycobot_topics.py scripts/listen_real.py + scripts/listen_real_of_topic.py scripts/teleop_keyboard.py scripts/sync_plan.py scripts/client.py diff --git a/launch/communication_topic.launch b/launch/communication_topic.launch new file mode 100644 index 0000000..9e062ec --- /dev/null +++ b/launch/communication_topic.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/scripts/listen_real.py b/scripts/listen_real.py index 8549633..97a1d2e 100755 --- a/scripts/listen_real.py +++ b/scripts/listen_real.py @@ -39,6 +39,8 @@ def talker(): joint_state_send.header.stamp = rospy.Time.now() res = func() + if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: + continue radians_list = [ res.joint_1 * (math.pi / 180), res.joint_2 * (math.pi / 180), diff --git a/scripts/listen_real_of_topic.py b/scripts/listen_real_of_topic.py new file mode 100755 index 0000000..cbcca0c --- /dev/null +++ b/scripts/listen_real_of_topic.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python2 +# license removed for brevity +import time +import math + +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from mycobot_ros.msg import MycobotAngles + + +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) + rate = rospy.Rate(30) # 30hz + rospy.spin() + + def callback(self, data): + # pub joint state + joint_state_send = JointState() + 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' + ] + joint_state_send.velocity = [0] + joint_state_send.effort = [] + + while not rospy.is_shutdown(): + joint_state_send.header.stamp = rospy.Time.now() + radians_list = [ + data.joint_1 * (math.pi / 180), + data.joint_2 * (math.pi / 180), + data.joint_3 * (math.pi / 180), + data.joint_4 * (math.pi / 180), + data.joint_5 * (math.pi / 180), + data.joint_6 * (math.pi / 180), + ] + rospy.loginfo('res: {}'.format(radians_list)) + + joint_state_send.position = radians_list + self.pub.publish(joint_state_send) + # rate.sleep() + + +if __name__ == '__main__': + try: + Listener() + except rospy.ROSInterruptException: + pass diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py old mode 100644 new mode 100755 index d9738e5..93e1f52 --- a/scripts/mycobot_topics.py +++ b/scripts/mycobot_topics.py @@ -1,5 +1,8 @@ #!/usr/bin/env python2 import time +import os +import sys +import signal import threading import rospy @@ -10,6 +13,49 @@ 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 MycobotTopics(object): def __init__(self): @@ -17,8 +63,8 @@ class MycobotTopics(object): rospy.init_node('mycobot_topics') rospy.loginfo('start ...') - port = rospy.get_param('~port') - baud = rospy.get_param('~baud') + 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() @@ -54,7 +100,7 @@ class MycobotTopics(object): def pub_real_angles(self): pub = rospy.Publisher('mycobot/angles_real', MycobotAngles, queue_size=5) ma = MycobotAngles() - while True: + while not rospy.is_shutdown(): self.lock.acquire() angles = self.mc.get_angles() self.lock.release() @@ -72,7 +118,7 @@ class MycobotTopics(object): pub = rospy.Publisher('mycobot/coords_real', MycobotCoords, queue_size=5) ma = MycobotCoords() - while True: + while not rospy.is_shutdown(): self.lock.acquire() coords = self.mc.get_coords() self.lock.release() @@ -129,6 +175,7 @@ class MycobotTopics(object): if __name__ == '__main__': + Watcher() mc_topics = MycobotTopics() mc_topics.start() # while True: diff --git a/srv/GetAngles.srv b/srv/GetAngles.srv index 9aa7d97..6992699 100644 --- a/srv/GetAngles.srv +++ b/srv/GetAngles.srv @@ -1,3 +1,6 @@ + +--- + float32 joint_1 float32 joint_2 float32 joint_3 From f65d40da2fac32754aaeab854972e818c7f7aa4d Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Thu, 27 May 2021 16:43:24 +0800 Subject: [PATCH 5/7] change offset of calibrate camera. --- scripts/detect_marker.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/detect_marker.py b/scripts/detect_marker.py index 70f7913..384ef77 100755 --- a/scripts/detect_marker.py +++ b/scripts/detect_marker.py @@ -69,7 +69,7 @@ class ImageConverter: # Just process first one detected. xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.035, xyz[1], xyz[2] - 0.03] + xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.01] euler = rvec[0, 0, :] tf_change = tf.transformations.quaternion_from_euler(euler[0], euler[1], euler[2]) From dd9e9f950a7f307bba5eb34d5e0bc857f98de382 Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Thu, 27 May 2021 16:45:20 +0800 Subject: [PATCH 6/7] chore: test detect marker and pump. --- ...mycobot_detect_marker_with_services.launch | 17 +- scripts/follow_and_pump copy.py | 133 ++++++++++++++++ scripts/follow_and_pump.py | 148 ++++++++++++++++++ scripts/gripper_control.py | 73 +++++---- scripts/listen_real_of_topic.py | 27 ++-- scripts/mycobot_topics.py | 2 +- 6 files changed, 349 insertions(+), 51 deletions(-) create mode 100755 scripts/follow_and_pump copy.py create mode 100755 scripts/follow_and_pump.py diff --git a/launch/mycobot_detect_marker_with_services.launch b/launch/mycobot_detect_marker_with_services.launch index a13c536..5d98cc7 100644 --- a/launch/mycobot_detect_marker_with_services.launch +++ b/launch/mycobot_detect_marker_with_services.launch @@ -8,14 +8,21 @@ - - - + + + + + + + + + - - + + + diff --git a/scripts/follow_and_pump copy.py b/scripts/follow_and_pump copy.py new file mode 100755 index 0000000..638cb99 --- /dev/null +++ b/scripts/follow_and_pump copy.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python2 +import rospy +from rospy.timer import sleep +from visualization_msgs import msg +from visualization_msgs.msg import Marker +import time +from mycobot_ros.srv import ( + GetCoords, SetCoords, GetAngles, SetAngles, PumpStatus) + + +set_coords = None +set_angles = None +toggle_pump = None + +x_offset = 15 +y_offset = 30 +z_offset = 100 + +temp_x = 0 +temp_y = 0 +temp_z = 0 +follow_z = 220 + +flag = False + + +def compare_offset(old, new): + count = 0 + for o, n in zip(old, new): + if abs(o - n) < 10: + count += 1 + + if count == len(old): + return True + + return False + + + +def grippercallback(data): + global flag, temp_x, temp_y,temp_z + print(type(data)) + if flag: + 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')) + + if (temp_x == temp_y == temp_z == 0.0) or (not compare_offset((temp_x, temp_y, temp_z), (x, y, z))): + set_coords(x, y, follow_z, -175, 0, -90, 70, 2) + temp_x, temp_y, temp_z = x, y, z + time.sleep(1) + return + + else: + print(x, y, z) + + # detect heigth + pump height + limit height + offset + x += x_offset + y += y_offset + z = z + 88 + 25 + z_offset + + + set_coords(x, y, z, -175, 0, -90, 70, 2) + time.sleep(1.5) + + for i in range(1,5): + + set_coords(x, y, z - i * 10, -175, 0, -90, 15, 2) + time.sleep(.3) + + time.sleep(2) + + + toggle_pump(1) + # pump on + + set_coords(x, y, z + 20, -165, 0, -90, 70, 2) + time.sleep(1.5) + + set_angles(0, 30, -50, -40, 0, 0, 50) + time.sleep(1.5) + + put_z = 140 + set_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2) + time.sleep(1.5) + + for i in range (1,5): + set_coords(39.4, -174.7, put_z-i*20, -177.13, -4.13, -152.59,15,2) + time.sleep(.3) + + toggle_pump(0) + + set_angles(0, 30, -50, -40, 0, 0, 50) + time.sleep(1.5) + + # finally + flag = True + +def gipper_subscriber(): + global set_coords, set_angles, toggle_pump + # 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_pump_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) + toggle_pump =rospy.ServiceProxy('switch_pump_status', PumpStatus) + except: + print('start error ...') + exit(1) + + + set_angles(0, 30, -50, -40, 0, 0, 50) + + time.sleep(2.5) + rospy.init_node('gipper_subscriber',anonymous=True) + rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1) + print('gripper test') + rospy.spin() + + + + +if __name__ == '__main__': + gipper_subscriber() + \ No newline at end of file diff --git a/scripts/follow_and_pump.py b/scripts/follow_and_pump.py new file mode 100755 index 0000000..c043f8e --- /dev/null +++ b/scripts/follow_and_pump.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python2 +import rospy +from visualization_msgs.msg import Marker +import time +import random + +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) + +angles = MycobotSetAngles() +coords = MycobotSetCoords() +pump = MycobotPumpStatus() + +x_offset = -20 +y_offset = 20 +z_offset = 120 + +flag = False + +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): + coords.x = x + coords.y = y + coords.z = z + coords.rx = rx + coords.ry = ry + coords.rz = rz + coords.speed = 70 + coords.model = m + # 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) + angles.joint_3 = float(c) + angles.joint_4 = float(d) + angles.joint_5 = float(e) + angles.joint_6 = float(f) + 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)): + print(o, n) + if abs(o - n) < 2: + count += 1 + print(count) + if count == 3: + return False + return True + + +def grippercallback(data): + global flag, temp_x, temp_y, temp_z + # rospy.loginfo('gripper_subscriber get date :%s', data) + if flag: + 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')) + + 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, y, 280) + time.sleep(.1) + + temp_x, temp_y, temp_z = x, y, z + return + else: + print(x, y, z) + + # detect heigth + pump height + limit height + offset + x += x_offset + y += y_offset + z = z + 88 + 25 + z_offset + + + pub_coords(x, y, z) + time.sleep(2.5) + + # down + for i in range(1,14): + pub_coords(x, y, z - i * 5,rx=-160, sp=10) + time.sleep(.1) + + time.sleep(2) + + pub_pump(True) + # pump on + + pub_coords(x, y, z + 20, -165) + time.sleep(1.5) + + pub_angles(0, 30, -50, -40, 0, 0, 50) + time.sleep(1.5) + + put_z = 140 + 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,7): + pub_coords(39.4, -174.7, put_z-i*5, -177.13, -4.13, -152.59,15,2) + time.sleep(.1) + + pub_pump(False) + + pub_angles(0, 30, -50, -40, 0, 0, 50) + time.sleep(1.5) + + # 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_pump(False) + # time.sleep(2.5) + rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1) + print 'gripper test' + rospy.spin() + + +if __name__ == '__main__': + main() + \ No newline at end of file diff --git a/scripts/gripper_control.py b/scripts/gripper_control.py index 6baac16..c34ffe8 100755 --- a/scripts/gripper_control.py +++ b/scripts/gripper_control.py @@ -3,6 +3,7 @@ import rospy +from rospy.timer import sleep from visualization_msgs import msg from visualization_msgs.msg import Marker # from pymycobot.mycobot import MyCobot @@ -10,15 +11,18 @@ from visualization_msgs.msg import Marker # from pymycobot import PI_PORT, PI_BAUD # For raspberry pi version of mycobot. import time from mycobot_ros.srv import ( - GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) + GetCoords, SetCoords, GetAngles, SetAngles, PumpStatus) set_coords = None set_angles = None +toggle_pump = None -x_offset = 0 + + +x_offset = 15 y_offset = 30 -z_offset = 60 +z_offset = 100 flag = False @@ -47,56 +51,63 @@ def grippercallback(data): y += y_offset z = z + 88 + 25 + z_offset - try: - set_coords(x, y, z, -175, 0, -90, 70, 2) - time.sleep(2.5) - except Exception: - pass + + set_coords(x, y, z, -175, 0, -90, 70, 2) + time.sleep(2.5) + + for i in range(1,5): + + set_coords(x, y, z - i * 10, -175, 0, -90, 15, 2) + time.sleep(.3) + + time.sleep(2) - for i in range(1,4): - try: - set_coords(x, y, z - i * 10, -175, 0, -90, 70, 2) - time.sleep(.2) - except Exception: - pass - + toggle_pump(1) # pump on - try: - set_coords(x, y, z + 20, -175, 0, -90, 70, 2) - time.sleep(2.5) - except Exception: - pass - # ... + set_coords(x, y, z + 20, -165, 0, -90, 70, 2) + time.sleep(1.5) + + set_angles(0, 30, -50, -40, 0, 0, 50) + time.sleep(1.5) + + put_z = 140 + set_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2) + time.sleep(1.5) + + for i in range (1,5): + set_coords(39.4, -174.7, put_z-i*20, -177.13, -4.13, -152.59,15,2) + time.sleep(.3) + + toggle_pump(0) + + set_angles(0, 30, -50, -40, 0, 0, 50) + time.sleep(1.5) # finally flag = True - - - - - def gipper_subscriber(): - global set_coords, set_angles + global set_coords, set_angles, toggle_pump # 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_pump_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) + toggle_pump =rospy.ServiceProxy('switch_pump_status', PumpStatus) except: print('start error ...') exit(1) - try: - set_angles(0, 30, -50, -40, 0, 0, 50) - except Exception: - pass + + set_angles(0, 30, -50, -40, 0, 0, 50) + time.sleep(2.5) rospy.init_node('gipper_subscriber',anonymous=True) rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1) diff --git a/scripts/listen_real_of_topic.py b/scripts/listen_real_of_topic.py index cbcca0c..7a20a0c 100755 --- a/scripts/listen_real_of_topic.py +++ b/scripts/listen_real_of_topic.py @@ -36,21 +36,20 @@ class Listener(object): joint_state_send.velocity = [0] joint_state_send.effort = [] - while not rospy.is_shutdown(): - joint_state_send.header.stamp = rospy.Time.now() - radians_list = [ - data.joint_1 * (math.pi / 180), - data.joint_2 * (math.pi / 180), - data.joint_3 * (math.pi / 180), - data.joint_4 * (math.pi / 180), - data.joint_5 * (math.pi / 180), - data.joint_6 * (math.pi / 180), - ] - rospy.loginfo('res: {}'.format(radians_list)) + joint_state_send.header.stamp = rospy.Time.now() + radians_list = [ + data.joint_1 * (math.pi / 180), + data.joint_2 * (math.pi / 180), + data.joint_3 * (math.pi / 180), + data.joint_4 * (math.pi / 180), + data.joint_5 * (math.pi / 180), + data.joint_6 * (math.pi / 180), + ] + rospy.loginfo('res: {}'.format(radians_list)) - joint_state_send.position = radians_list - self.pub.publish(joint_state_send) - # rate.sleep() + joint_state_send.position = radians_list + self.pub.publish(joint_state_send) + # rate.sleep() if __name__ == '__main__': diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py index 93e1f52..7c0426a 100755 --- a/scripts/mycobot_topics.py +++ b/scripts/mycobot_topics.py @@ -146,7 +146,7 @@ class MycobotTopics(object): angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] sp = int(data.speed) model = int(data.model) - self.mc.send_angles(angles, sp, model) + self.mc.send_coords(angles, sp, model) sub = rospy.Subscriber('mycobot/coords_goal', MycobotSetCoords, callback=callback) rospy.spin() From c0a2a5d7f440fa4d965b365f39f6131a495d9301 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E7=AB=8B=E5=86=9B?= Date: Mon, 7 Jun 2021 10:17:07 +0800 Subject: [PATCH 7/7] rename and improve. --- ...ces.launch => mycobot_detect_marker_with_topic.launch} | 0 scripts/detect_marker.py | 2 +- scripts/follow_and_pump.py | 8 +++++--- 3 files changed, 6 insertions(+), 4 deletions(-) rename launch/{mycobot_detect_marker_with_services.launch => mycobot_detect_marker_with_topic.launch} (100%) diff --git a/launch/mycobot_detect_marker_with_services.launch b/launch/mycobot_detect_marker_with_topic.launch similarity index 100% rename from launch/mycobot_detect_marker_with_services.launch rename to launch/mycobot_detect_marker_with_topic.launch diff --git a/scripts/detect_marker.py b/scripts/detect_marker.py index 384ef77..d6734fe 100755 --- a/scripts/detect_marker.py +++ b/scripts/detect_marker.py @@ -69,7 +69,7 @@ class ImageConverter: # Just process first one detected. xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.01] + 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]) diff --git a/scripts/follow_and_pump.py b/scripts/follow_and_pump.py index c043f8e..b2fc2dd 100755 --- a/scripts/follow_and_pump.py +++ b/scripts/follow_and_pump.py @@ -18,7 +18,7 @@ pump = MycobotPumpStatus() x_offset = -20 y_offset = 20 -z_offset = 120 +z_offset = 110 flag = False @@ -98,7 +98,7 @@ def grippercallback(data): time.sleep(2.5) # down - for i in range(1,14): + for i in range(1,17): pub_coords(x, y, z - i * 5,rx=-160, sp=10) time.sleep(.1) @@ -117,12 +117,14 @@ def grippercallback(data): 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,7): + 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) pub_pump(False) + time.sleep(.5) + pub_angles(0, 30, -50, -40, 0, 0, 50) time.sleep(1.5)