00001
00002
00003
00004
00005 try:
00006 import roslib
00007 roslib.load_manifest('turtlebot_teleop')
00008 roslib.load_manifest('rospeex_if')
00009 except:
00010 pass
00011
00012 import rospy
00013 from geometry_msgs.msg import Twist
00014
00015 from rospeex_if import ROSpeexInterface
00016 import sys, termios, re
00017
00018
00019 rospeex = None
00020 turtle_pub = None
00021
00022 OPERATION_TIME = 100
00023 TARGET_SPEED = 0.2
00024 TARGET_TURN = 0.2
00025
00026 _target_speed = 0.0
00027 _control_speed = 0.0
00028 _target_turn = 0.0
00029 _control_turn = 0.0
00030 _operation_count = 0
00031 _start_flag = False
00032
00033 def set_operation( target_speed, control_speed, target_turn, control_turn ):
00034 global _target_speed, _control_speed, _target_turn, _control_turn, _operation_count
00035 _target_speed = target_speed
00036 _control_speed = control_speed
00037 _target_turn = target_turn
00038 _control_turn = control_turn
00039 _operation_count = 0
00040
00041 def timer_callback(event):
00042 global _target_speed, _control_speed, _target_turn, _control_turn, _operation_count, OPERATION_TIME, TARGET_SPEED
00043 _operation_count += 1
00044 if _operation_count < OPERATION_TIME:
00045 turtle_move( _target_speed, _control_speed, _target_turn, _control_turn )
00046 elif _start_flag == True:
00047 turtle_move( TARGET_SPEED, TARGET_SPEED, 0, 0 )
00048 else:
00049 turtle_move( 0, 0, 0, 0 )
00050
00051 def sr_response(message):
00052 global TARGET_SPEED, TARGET_TURN, _start_flag
00053 print 'you said : %s' %message
00054 rule = re.compile(u'(?P<operation>(右|左|前|後|スタート|ストップ))')
00055 uni_msg = unicode(message, 'utf-8')
00056 m = rule.match(uni_msg)
00057
00058 if m is not None:
00059 operation = m.group('operation')
00060 if operation == u"前":
00061 rospeex.say("前に進みます")
00062 set_operation(TARGET_SPEED, TARGET_SPEED, 0, 0)
00063 elif operation == u"後":
00064 rospeex.say("後に進みます")
00065 set_operation(-TARGET_SPEED, -TARGET_SPEED, 0, 0)
00066 elif operation == u"右":
00067 rospeex.say("右に進みます")
00068 set_operation(TARGET_SPEED, TARGET_SPEED, -TARGET_TURN, -TARGET_TURN)
00069 elif operation == u"左":
00070 rospeex.say("左に進みます")
00071 set_operation(TARGET_SPEED, TARGET_SPEED, TARGET_TURN, TARGET_TURN)
00072 elif operation == u"スタート":
00073 rospeex.say("スタートします")
00074 _start_flag = True
00075 elif operation == u"ストップ":
00076 rospeex.say("ストップします")
00077 _start_flag = False
00078
00079 def turtle_move(target_speed, control_speed, target_turn, control_turn ):
00080 if target_speed > control_speed:
00081 control_speed = min( target_speed, control_speed + 0.02 )
00082 elif target_speed < control_speed:
00083 control_speed = max( target_speed, control_speed - 0.02 )
00084 else:
00085 control_speed = target_speed
00086
00087 if target_turn > control_turn:
00088 control_turn = min( target_turn, control_turn + 0.1 )
00089 elif target_turn < control_turn:
00090 control_turn = max( target_turn, control_turn - 0.1 )
00091 else:
00092 control_turn = target_turn
00093
00094 twist = Twist()
00095 twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
00096 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
00097 turtle_pub.publish(twist)
00098
00099 if __name__=="__main__":
00100 settings = termios.tcgetattr(sys.stdin)
00101 rospy.init_node('rospeex_turtle')
00102 rospeex = ROSpeexInterface()
00103 rospeex.init()
00104 rospeex.register_sr_response( sr_response )
00105 rospeex.set_spi_config(language='ja',engine='nict')
00106 turtle_pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist)
00107 rospy.Timer(rospy.Duration(0,100000000), timer_callback)
00108
00109 try:
00110 rospy.spin();
00111 except Exception as e:
00112 print str(e)
00113 finally:
00114 twist = Twist()
00115 twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
00116 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
00117 turtle_pub.publish(twist)
00118
00119 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)