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