From dd9e9f950a7f307bba5eb34d5e0bc857f98de382 Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Thu, 27 May 2021 16:45:20 +0800 Subject: [PATCH] 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()