kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Update follow_and_pump.py

Browse source 
add comment
This commit is contained in:
Zachary 2021-10-14 20:25:09 +08:00 • committed by GitHub
parent e0a94baae2
commit faa4590b7e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 19 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
mycobot_280/scripts/follow_and_pump.py
View file

@ -3,30 +3,38 @@ import rospy
from visualization_msgs.msg import Marker from visualization_msgs.msg import Marker
import time import time
# 与 mycobot 通信的消息类型
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
rospy.init_node("gipper_subscriber", anonymous=True) rospy.init_node("gipper_subscriber", anonymous=True)
# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
angle_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, 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) coord_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=5)
pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=5) pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=5)
# 实例化消息对象
angles = MycobotSetAngles() angles = MycobotSetAngles()
coords = MycobotSetCoords() coords = MycobotSetCoords()
pump = MycobotPumpStatus() pump = MycobotPumpStatus()
# 与 mycobot 真实位置的偏差值
x_offset = -20 x_offset = -20
y_offset = 20 y_offset = 20
z_offset = 110 z_offset = 110
# 通过该变量限制，抓取行为只做一次
flag = False flag = False
# 为了后面比较二维码是否移动
temp_x = temp_y = temp_z = 0.0 temp_x = temp_y = temp_z = 0.0
temp_time = time.time() 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.x = x
coords.y = y coords.y = y
coords.z = z coords.z = z
@ -40,6 +48,7 @@ def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
def pub_angles(a, b, c, d, e, f, sp): def pub_angles(a, b, c, d, e, f, sp):
"""发布角度"""
angles.joint_1 = float(a) angles.joint_1 = float(a)
angles.joint_2 = float(b) angles.joint_2 = float(b)
angles.joint_3 = float(c) angles.joint_3 = float(c)
@ -51,11 +60,13 @@ def pub_angles(a, b, c, d, e, f, sp):
def pub_pump(flag): def pub_pump(flag):
"""发布夹爪状态"""
pump.Status = flag pump.Status = flag
pump_pub.publish(pump) pump_pub.publish(pump)
def target_is_moving(x, y, z): def target_is_moving(x, y, z):
"""判断目标是否移动"""
count = 0 count = 0
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
print(o, n) print(o, n)
@ -68,16 +79,19 @@ def target_is_moving(x, y, z):
def grippercallback(data): def grippercallback(data):
"""回调函数""""
global flag, temp_x, temp_y, temp_z global flag, temp_x, temp_y, temp_z
# rospy.loginfo('gripper_subscriber get date :%s', data) # rospy.loginfo('gripper_subscriber get date :%s', data)
if flag: if flag:
return return
# 解析出坐标值
# pump length: 88mm # pump length: 88mm
x = float(format(data.pose.position.x * 1000, ".2f")) x = float(format(data.pose.position.x * 1000, ".2f"))
y = float(format(data.pose.position.y * 1000, ".2f")) y = float(format(data.pose.position.y * 1000, ".2f"))
z = float(format(data.pose.position.z * 1000, ".2f")) z = float(format(data.pose.position.z * 1000, ".2f"))
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
if ( if (
time.time() - temp_time < 30 time.time() - temp_time < 30
or (temp_x == temp_y == temp_z == 0.0) or (temp_x == temp_y == temp_z == 0.0)
@ -91,7 +105,8 @@ def grippercallback(data):
temp_x, temp_y, temp_z = x, y, z temp_x, temp_y, temp_z = x, y, z
return return
else: else: # 表示目标处于静止状态，可以尝试抓取
print(x, y, z) print(x, y, z)
# detect heigth + pump height + limit height + offset # detect heigth + pump height + limit height + offset
@ -146,7 +161,10 @@ def main():
pub_pump(False) pub_pump(False)
# time.sleep(2.5) # 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") print("gripper test")
rospy.spin() rospy.spin()