00001
00002
00003
00004 import rospy
00005
00006 from geometry_msgs.msg import Twist
00007
00008 from rospeex_if import ROSpeexInterface
00009 import re
00010
00011 rospeex = None
00012 turtle_pub = None
00013
00014 OPERATION_TIME = 500
00015 TURN_TIME = 11
00016 TURN_ACTION_COUNTER_MAX = 7
00017 TARGET_SPEED = 0.12
00018 TARGET_TURN = 1.0
00019
00020 _target_speed = 0.0
00021 _control_speed = 0.0
00022 _target_turn = 0.0
00023 _control_turn = 0.0
00024 _operation_count = 0
00025 _turn_action = 0
00026 _started = False
00027 _force_stop = False
00028 _force_stop = False
00029 _during_right = False
00030 _during_left = False
00031
00032 _pattern_rule = ""
00033
00034 def set_ope_param( target_speed, target_turn ):
00035 global _target_speed, _target_turn, _operation_count
00036 _target_speed = target_speed
00037 _target_turn = target_turn
00038 _operation_count = 0
00039
00040 def speak(msg):
00041 rospeex.say( msg, language='en', engine='nict', voice_font='EF007')
00042
00043 def timer_callback(event):
00044 global _target_speed, _control_speed, _target_turn, _control_turn, _operation_count, _during_right, _during_left, _started, _force_stop, _turn_action, OPERATION_TIME, TARGET_SPEED, TURN_TIME, TURN_ACTION_COUNTER_MAX
00045 _operation_count += 1
00046 if _force_stop == True:
00047 _started = False
00048 _force_stop = False
00049 _during_right = False
00050 _during_left = False
00051 _turn_action = 0
00052 elif _during_right == True:
00053 if _operation_count > TURN_TIME:
00054 _turn_action += 1
00055 if _turn_action == TURN_ACTION_COUNTER_MAX:
00056 set_ope_param(0, TARGET_TURN)
00057 else:
00058 set_ope_param(TARGET_SPEED, 0)
00059 elif _during_left == True:
00060 if _operation_count > TURN_TIME:
00061 _turn_action += 1
00062 if _turn_action == TURN_ACTION_COUNTER_MAX:
00063 set_ope_param(0, -TARGET_TURN)
00064 else:
00065 set_ope_param(TARGET_SPEED, 0)
00066 elif _operation_count > OPERATION_TIME and _started == True:
00067 speak( "It's time over.")
00068 print "It's time over."
00069 set_ope_param( 0, 0 )
00070 _started = False
00071
00072 turtle_move( _target_speed, _target_turn )
00073 if _turn_action > TURN_ACTION_COUNTER_MAX:
00074 _turn_action = 0
00075 _during_right = False
00076 _during_left = False
00077
00078 def sr_response(message):
00079 rospy.loginfo(message)
00080 global TARGET_SPEED, TARGET_TURN, _during_right, _during_left, _started, _force_stop, _pattern_rule
00081 print 'you said : \"%s\"' %message
00082
00083 m = _pattern_rule.match(message.lower())
00084
00085 if m is not None:
00086 operation = m.group(2)
00087 print "recognized \"%s\"" %operation
00088
00089 if ( operation == "back" ) and _started == False:
00090 speak("I'm moving backward.")
00091 _started = True
00092 set_ope_param(-TARGET_SPEED, 0)
00093
00094 elif operation == "ready" and _started == False:
00095 print "Ready."
00096 _started = True
00097 set_ope_param(TARGET_SPEED*0.08, 0)
00098
00099 elif operation == "start" or operation == "straight":
00100 speak("I'm starting.")
00101 print "I'm starting."
00102 _started = True
00103 set_ope_param(TARGET_SPEED, 0)
00104
00105 elif operation == "right":
00106 if _during_right == True:
00107 speak("I'm already turning.")
00108 print "I'm already turning."
00109 elif _started == True:
00110 _during_right = True
00111 speak("I'm turning right.")
00112 print "I'm turning right."
00113 set_ope_param(0, -TARGET_TURN)
00114
00115 elif operation == "left":
00116 if _during_left == True:
00117 speak("I'm already turning.")
00118 print "I'm already turning."
00119 elif _started == True:
00120 _during_left = True
00121 speak("I'm turning left.")
00122 print "I'm turning left."
00123 set_ope_param(0, TARGET_TURN)
00124
00125 elif operation == "stop" or operation == 'wait':
00126 speak("I stopped.")
00127 print "I stopped."
00128 _force_stop = True
00129 set_ope_param(0, 0)
00130
00131 elif message != "":
00132 print "What?"
00133 rospeex.play_sound( "alert.wav" )
00134
00135 def turtle_move( target_speed, target_turn ):
00136 global _control_speed, _control_turn
00137
00138 if target_speed > _control_speed:
00139 _control_speed = min( target_speed, _control_speed + 0.02 )
00140 elif target_speed < _control_speed:
00141 _control_speed = max( target_speed, _control_speed - 0.02 )
00142 else:
00143 _control_speed = target_speed
00144
00145 if target_turn > _control_turn:
00146 _control_turn = min( target_turn, _control_turn + 0.1 )
00147 elif target_turn < _control_turn:
00148 _control_turn = max( target_turn, _control_turn - 0.1 )
00149 else:
00150 _control_turn = target_turn
00151
00152 control_speed = _control_speed
00153 control_turn = _control_turn
00154
00155 twist = Twist()
00156 twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
00157 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
00158 turtle_pub.publish(twist)
00159
00160 if __name__=="__main__":
00161
00162 _pattern_rule = re.compile( '.*(robot)*.*(back|ready|start|straight|right|left|stop|wait)')
00163
00164 _operation_count = 0
00165 _turn_action = 0
00166 _started = False
00167 _force_stop = False
00168 _force_stop = False
00169 _during_right = False
00170 _during_left = False
00171 rospy.init_node('rospeex_turtle')
00172 rospeex = ROSpeexInterface()
00173 rospeex.init()
00174 rospeex.register_sr_response( sr_response )
00175 rospeex.set_spi_config(language='en',engine='nict')
00176
00177 turtle_pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist)
00178 rospy.Timer(rospy.Duration(0,100000000), timer_callback)
00179
00180 try:
00181 rospy.spin();
00182 except:
00183 print e
00184 finally:
00185 twist = Twist()
00186 twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
00187 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
00188 turtle_pub.publish(twist)