turtle_demo.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
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)


rospeex_samples
Author(s): Komei Sugiura
autogenerated on Thu Apr 20 2017 03:09:00