From 74eb9b3dcf9f433a19224a79036d815fe7d64ab0 Mon Sep 17 00:00:00 2001 From: weiquan Date: Tue, 26 Oct 2021 10:16:18 +0800 Subject: [PATCH] Adapted to seeed --- .gitignore | 1 + mycobot_280/scripts/follow_and_pump.py | 45 +++++++++++++------ .../msg/MycobotPumpStatus.msg | 2 + .../scripts/mycobot_services.py | 11 ++--- .../scripts/mycobot_topics.py | 14 +++--- mycobot_communication/srv/PumpStatus.srv | 2 + 6 files changed, 50 insertions(+), 25 deletions(-) diff --git a/.gitignore b/.gitignore index a1b5bbc..9771279 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,7 @@ .VSCodeCounter/ .vscode/ +.history # Editor directories and files .idea diff --git a/mycobot_280/scripts/follow_and_pump.py b/mycobot_280/scripts/follow_and_pump.py index cfb0b30..f26b669 100755 --- a/mycobot_280/scripts/follow_and_pump.py +++ b/mycobot_280/scripts/follow_and_pump.py @@ -1,7 +1,9 @@ #!/usr/bin/env python2 +# coding:utf-8 import rospy from visualization_msgs.msg import Marker import time +import os # 与 mycobot 通信的消息类型 from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus @@ -10,9 +12,20 @@ from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, Mycobo rospy.init_node("gipper_subscriber", anonymous=True) # 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -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) +angle_pub = rospy.Publisher("mycobot/angles_goal", + MycobotSetAngles, queue_size=5) +coord_pub = rospy.Publisher("mycobot/coords_goal", + MycobotSetCoords, queue_size=5) +# Judging equipment: ttyUSB* is M5；ttyACM* is wio +robot = os.popen("ls /dev/ttyUSB*").readline() + +if "dev" in robot: + Pin = [2, 5] +else: + Pin = [20, 21] + +pump_pub = rospy.Publisher("mycobot/pump_status", + MycobotPumpStatus, queue_size=5) # 实例化消息对象 angles = MycobotSetAngles() @@ -59,9 +72,11 @@ def pub_angles(a, b, c, d, e, f, sp): angle_pub.publish(angles) -def pub_pump(flag): +def pub_pump(flag, Pin): """发布夹爪状态""" pump.Status = flag + pump.Pin1 = Pin[0] + pump.Pin2 = Pin[1] pump_pub.publish(pump) @@ -79,7 +94,7 @@ def target_is_moving(x, y, z): def grippercallback(data): - """回调函数"""" + """回调函数""" global flag, temp_x, temp_y, temp_z # rospy.loginfo('gripper_subscriber get date :%s', data) if flag: @@ -105,8 +120,8 @@ def grippercallback(data): temp_x, temp_y, temp_z = x, y, z return - else: # 表示目标处于静止状态，可以尝试抓取 - + else: # 表示目标处于静止状态，可以尝试抓取 + print(x, y, z) # detect heigth + pump height + limit height + offset @@ -124,7 +139,7 @@ def grippercallback(data): time.sleep(2) - pub_pump(True) + pub_pump(True, Pin) # pump on pub_coords(x, y, z + 20, -165) @@ -138,10 +153,11 @@ def grippercallback(data): 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) + 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) + pub_pump(False, Pin) time.sleep(0.5) @@ -159,12 +175,13 @@ def main(): # 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) + pub_pump(False, Pin) # time.sleep(2.5) - + # mark 信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, grippercallback, queue_size=1) - + rospy.Subscriber("visualization_marker", Marker, + grippercallback, queue_size=1) + print("gripper test") rospy.spin() diff --git a/mycobot_communication/msg/MycobotPumpStatus.msg b/mycobot_communication/msg/MycobotPumpStatus.msg index 635a864..2373241 100644 --- a/mycobot_communication/msg/MycobotPumpStatus.msg +++ b/mycobot_communication/msg/MycobotPumpStatus.msg @@ -1 +1,3 @@ bool Status +int8 Pin1 +int8 Pin2 diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py index dd5fcc0..c0e118a 100755 --- a/mycobot_communication/scripts/mycobot_services.py +++ b/mycobot_communication/scripts/mycobot_services.py @@ -89,11 +89,11 @@ def switch_status(req): def toggle_pump(req): if mc: if req.Status: - mc.set_basic_output(2, 0) - mc.set_basic_output(5, 0) + mc.set_basic_output(req.Pin1, 0) + mc.set_basic_output(req.Pin2, 0) else: - mc.set_basic_output(2, 1) - mc.set_basic_output(5, 1) + mc.set_basic_output(req.Pin1, 1) + mc.set_basic_output(req.Pin2, 1) return PumpStatusResponse(True) @@ -135,7 +135,8 @@ def output_robot_message(): servo_infomation = "all connected" print( - robot_msg % (connect_status, servo_infomation, servo_temperature, atom_version) + robot_msg % (connect_status, servo_infomation, + servo_temperature, atom_version) ) diff --git a/mycobot_communication/scripts/mycobot_topics.py b/mycobot_communication/scripts/mycobot_topics.py index 7eca681..65e3813 100755 --- a/mycobot_communication/scripts/mycobot_topics.py +++ b/mycobot_communication/scripts/mycobot_topics.py @@ -104,7 +104,8 @@ class MycobotTopics(object): 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() @@ -121,7 +122,8 @@ class MycobotTopics(object): 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(): @@ -183,11 +185,11 @@ class MycobotTopics(object): 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) + self.mc.set_basic_output(data.Pin1, 0) + self.mc.set_basic_output(data.Pin2, 0) else: - self.mc.set_basic_output(2, 1) - self.mc.set_basic_output(5, 1) + self.mc.set_basic_output(data.Pin1, 1) + self.mc.set_basic_output(data.Pin2, 1) sub = rospy.Subscriber( "mycobot/pump_status", MycobotPumpStatus, callback=callback diff --git a/mycobot_communication/srv/PumpStatus.srv b/mycobot_communication/srv/PumpStatus.srv index 3887d75..8bd4b53 100644 --- a/mycobot_communication/srv/PumpStatus.srv +++ b/mycobot_communication/srv/PumpStatus.srv @@ -1,4 +1,6 @@ bool Status +int8 Pin1 +int8 Pin2 ---