191 lines
4.9 KiB
Python
Executable File
191 lines
4.9 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*
|
|
|
|
import os
|
|
import sys
|
|
import tty, termios
|
|
import roslib
|
|
import rospy
|
|
from geometry_msgs.msg import Twist
|
|
from std_msgs.msg import String
|
|
|
|
# 全局变量
|
|
cmd = Twist()
|
|
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
|
|
grasp_pub = rospy.Publisher('/grasp', String, queue_size=1)
|
|
|
|
global can_grasp
|
|
global can_release
|
|
|
|
def grasp_status_cp(msg):
|
|
global can_release,can_grasp
|
|
# 物体抓取成功,让机器人回起始点
|
|
if msg.data=='1':
|
|
can_release=True
|
|
if msg.data=='0' or msg.data=='-1':
|
|
can_grasp=True
|
|
grasp_status=rospy.Subscriber('/grasp_status', String, grasp_status_cp, queue_size=1)
|
|
|
|
def keyboardLoop():
|
|
rospy.init_node('teleop')
|
|
#初始化监听键盘按钮时间间隔
|
|
rate = rospy.Rate(rospy.get_param('~hz', 10))
|
|
|
|
#速度变量
|
|
# 慢速
|
|
slow_walk_vel_ = rospy.get_param('walk_vel',0.05)
|
|
walk_vel_ = rospy.get_param('walk_vel', 0.1)
|
|
# 快速
|
|
run_vel_ = rospy.get_param('run_vel', 0.5)
|
|
yaw_rate_ = rospy.get_param('yaw_rate', 0.9)
|
|
yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
|
|
# walk_vel_前后速度
|
|
max_tv = walk_vel_
|
|
# yaw_rate_旋转速度
|
|
max_rv = yaw_rate_
|
|
# 参数初始化
|
|
speed=0
|
|
global can_release,can_grasp
|
|
can_grasp=True
|
|
can_release=False
|
|
|
|
print ("使用[WASD]控制机器人")
|
|
print ("按[G]抓取物体，按[H]放下物体")
|
|
print ("按[Q]退出" )
|
|
print ("B紧急停止")
|
|
|
|
#读取按键循环
|
|
while not rospy.is_shutdown():
|
|
# linux下读取键盘按键
|
|
fd = sys.stdin.fileno()
|
|
turn =0
|
|
old_settings = termios.tcgetattr(fd)
|
|
#不产生回显效果
|
|
old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO
|
|
try :
|
|
tty.setraw( fd )
|
|
ch = sys.stdin.read( 1 )
|
|
finally :
|
|
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
|
|
# ch代表获取的键盘按键
|
|
if ch == 'e':
|
|
if can_grasp:
|
|
msg=String()
|
|
msg.data='1'
|
|
grasp_pub.publish(msg)
|
|
can_grasp=False
|
|
speed = 0
|
|
turn = 0
|
|
elif ch == '1':
|
|
if can_release:
|
|
msg=String()
|
|
msg.data='0'
|
|
grasp_pub.publish(msg)
|
|
can_release=False
|
|
speed = 0
|
|
turn = 0
|
|
elif ch == '2':
|
|
if can_release:
|
|
msg=String()
|
|
msg.data='2'
|
|
grasp_pub.publish(msg)
|
|
can_release=False
|
|
speed = 0
|
|
turn = 0
|
|
elif ch == '3':
|
|
if can_release:
|
|
msg=String()
|
|
msg.data='3'
|
|
grasp_pub.publish(msg)
|
|
can_release=False
|
|
speed = 0
|
|
turn = 0
|
|
|
|
elif ch == 'z':
|
|
if can_release:
|
|
msg=String()
|
|
msg.data='2'
|
|
grasp_pub.publish(msg)
|
|
can_release=False
|
|
speed = 0
|
|
turn = 0
|
|
elif ch == 'w':
|
|
max_tv = walk_vel_
|
|
speed = 1
|
|
turn = 0
|
|
elif ch == 's':
|
|
max_tv = walk_vel_
|
|
speed = -1.2
|
|
turn = 0
|
|
elif ch == 'a':
|
|
max_rv = yaw_rate_
|
|
speed = 0
|
|
turn = 0.15
|
|
elif ch == 'd':
|
|
max_rv = yaw_rate_
|
|
speed = 0
|
|
turn = -0.15
|
|
elif ch == 'W':
|
|
max_tv = run_vel_
|
|
speed = 1
|
|
turn = 0
|
|
elif ch == 'S':
|
|
max_tv = run_vel_
|
|
speed = -1
|
|
turn = 0
|
|
elif ch == 'A':
|
|
max_rv = yaw_rate_run_
|
|
speed = 0
|
|
turn = 1
|
|
elif ch == 'D':
|
|
max_rv = yaw_rate_run_
|
|
speed = 0
|
|
turn = -1
|
|
elif ch == 'i':
|
|
max_tv = slow_walk_vel_
|
|
speed = 1
|
|
turn = 0
|
|
elif ch == 'k':
|
|
max_tv = slow_walk_vel_
|
|
speed = -1
|
|
turn = 0
|
|
elif ch == 'j':
|
|
max_rv = yaw_rate_
|
|
speed = 0
|
|
turn = 0.075
|
|
elif ch == 'l':
|
|
max_rv = yaw_rate_
|
|
speed = 0
|
|
turn = -0.075
|
|
elif ch == 'b' or ch == 'B':
|
|
max_rv = yaw_rate_
|
|
speed = 0
|
|
turn = 0
|
|
elif ch == 'q':
|
|
exit()
|
|
else:
|
|
max_tv = walk_vel_
|
|
max_rv = yaw_rate_
|
|
speed = 0
|
|
turn = 0
|
|
|
|
#发送消息
|
|
cmd.linear.x = speed * max_tv
|
|
cmd.angular.z = turn * max_rv
|
|
pub.publish(cmd)
|
|
rate.sleep()
|
|
#停止机器人
|
|
#stop_robot()
|
|
|
|
def stop_robot():
|
|
cmd.linear.x = 0.0
|
|
cmd.angular.z = 0.0
|
|
pub.publish(cmd)
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
keyboardLoop()
|
|
except rospy.ROSInterruptException:
|
|
pass
|
|
|