Source: train_hand.py
#coding=utf-8
import rospy
from std_msgs.msg import Int16
import time
from geometry_msgs.msg import Twist #导å
¥éè¦çrosçpackage
import numpy as np
is_done=Int16()
is_done.data=0
#å®ä¹åéæ°æ®ç彿°:åªå䏿¬¡ï¼å¯ä»¥èèåå¾å¤æ¬¡ï¼æç»è¦çææå¦ä½ï¼
def data_publish(u):
#åå§ånode
#åå§åpublisher
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # topicåæ¶æ¯ç±»åéè¦åè岸æåéç¡®å®ä¸ä¸
#åå§åtwist
twist = Twist()
# ç»twistèµå¼
twist.linear.x = u[0] #线é度
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0 #è§é度
twist.angular.y = 0.0
twist.angular.z = u[1]
#è°è¯ä»£ç
# print("twist")
# print(twist)
#åétwist
pub.publish(twist) #å°æ°æ®åéåºå»
#åétwistï¼ä¸ç´åæä»¤ï¼ç´å°ä¸æ¬¡è¿å¨ç»æä¹åå°±ä¸åæä»¤
def callback_2(data):
#æ¥åæ°æ®
is_done.data= data.data
#print(finish)
#å¤æç¶æ
# if (finish.data == 1):#å®æè½¬å¨
# is_done = 1
# # print("callback_2",is_done)
# else:#æ²¡å®æè½¬å¨
# is_done = 0
# print("moving")
if __name__ == '__main__':
#åå§å
rospy.init_node('train_hand', anonymous=True)
rospy.Subscriber("is_done", Int16, callback_2)
start_flag = 1 #å¼å§æä»¤
twist = Twist() #åé声æ
u = np.zeros(2) #æ§å¶é声æ
time.sleep(1)
print(is_done.data)
#æ§å¶è¯´æ
print("åè¿ï¼w å·¦æï¼a 峿:d åéï¼s")
time.sleep(0.1)
#å¼å§æ§å¶
while start_flag == 1: #ä¸ç´å¾ªç¯æ¥åæ°æ®
control_key = raw_input("raw_inputï¼") #è·å¾é®çè¾å
¥
if control_key == "w": #åè¿
#æ¾ç¤ºæ§å¶é
print("control is w")
#设置é¢ç
#没æ¶å°ç»æä¿¡æ¯ä¸ç´åéæ§å¶é
if(is_done.data==1):
#èµå¼çº¿é度
u[0] = 1
u[1] = 0
#åå¸çº¿éåº¦æ¶æ¯
data_publish(u)
#以10HZé¢çåéæ°æ®
elif control_key == "a": #左转
#æ¾ç¤ºæ§å¶é
print("control is a")
#设置é¢ç
rate = rospy.Rate(10) # 10hz
#没æ¶å°ç»æä¿¡æ¯ä¸ç´åéæ§å¶é
if(is_done.data==1):
#èµå¼çº¿é度
u[0] = 0
u[1] = -1
#åå¸çº¿éåº¦æ¶æ¯
data_publish(u)
#å¦ææ¥æ¶å°ç»æä¿¡æ¯ï¼is_done设为1,ç»æå¾ªç¯
#以10HZé¢çåéæ°æ®
rate.sleep()
elif control_key == "d": #å³è½¬
#æ¾ç¤ºæ§å¶é
print("control is d")
#设置é¢ç
rate = rospy.Rate(10) # 10hz
if(is_done.data==1):
#èµå¼çº¿é度
u[0] = 0
u[1] = 1
#åå¸çº¿éåº¦æ¶æ¯
data_publish(u)
#å¦ææ¥æ¶å°ç»æä¿¡æ¯ï¼is_done设为1,ç»æå¾ªç¯
#以10HZé¢çåéæ°æ®
rate.sleep()
elif control_key == "s": #åé
#æ¾ç¤ºæ§å¶é
print("control is s")
#设置é¢ç
rate = rospy.Rate(10) # 10hz
if(is_done.data==1):
#èµå¼çº¿é度
u[0] = -1
u[1] = 0
#åå¸çº¿éåº¦æ¶æ¯
data_publish(u)
#å¦ææ¥æ¶å°ç»æä¿¡æ¯ï¼is_done设为1,ç»æå¾ªç¯
#以10HZé¢çåéæ°æ®
rate.sleep()
<<<<<<< HEAD
elif control_key == "q": #å
è¶³æ¥æ
#æ¾ç¤ºæ§å¶é
print("control is s")
#设置é¢ç
rate = rospy.Rate(10) # 10hz
if(is_done.data==1):
#èµå¼çº¿é度
u[0] = 2
u[1] = 0
#åå¸çº¿éåº¦æ¶æ¯
data_publish(u)
#å¦ææ¥æ¶å°ç»æä¿¡æ¯ï¼is_done设为1,ç»æå¾ªç¯
#以10HZé¢çåéæ°æ®
rate.sleep()
=======
>>>>>>> 6ca41d4ff84544147f430048270163452df0609e
#å°is_downå¤ä½
print("is_doneå·²éç½®")
is_done.data = 0