#/usr/bin/env python
# -*- coding: utf-8 -*-
# -*- Python -*-
# Version : 1.0.0
# Vendor : RTC-Center
# date : 09/08/31 (ver1.0.0)
# abstract : script for demonstration
#
import sys
import os
import time

from math import *

from set_env import *

save_path = sys.path[:]
sys.path.append(Rh2LibDir)
#sys.path.append(RtmToolsDir+"/rtc_handle")
from rtc_handle import *

env = RtmEnv(sys.argv,[Rh2VoiceNS,Rh2BaseNS1,Rh2BaseNS2])

for ns in env.name_space :
  env.name_space[ns].list_obj()

sys.path.append(Rh2VoiceToolsDir)
import Rh2Voice_ctrl
from message import *
sys.path.append(Rh2BaseToolsDir)
import Rh2Base_ctrl
sys.path = save_path

Rh2Voice_ctrl.assign_variables(env)
Rh2Base_ctrl.assign_variables(env)

#
# import stub files
#
sys.path.append(Rh2IdlDir)
import _GlobalIDL

print '---[Start]---'
# 移動開始します。
Rh2Voice_ctrl.talk(mstr001)
Rh2Base_ctrl.navigation.outports['status'].open()
Rh2Base_ctrl.moveto("start")
time.sleep(1)
print '---[status] running---'
state    =0
old_state=0
beep_cnt =0
while state == 0:
  tmp = Rh2Base_ctrl.navigation.outports['status'].read().data
  state = tmp
  time.sleep(0.1)
while not state == 0:
  tmp = Rh2Base_ctrl.navigation.outports['status'].read().data
  state = tmp
  print state
  if state != old_state:
    if state==2:
      # 旋回します。
      Rh2Voice_ctrl.talk(mstr006)
    elif state==5:
      # 直進します。
      Rh2Voice_ctrl.talk(mstr002)
  time.sleep(0.1)
  if beep_cnt >=  50:
    Rh2Voice_ctrl.beep()
    beep_cnt = 0
  else:
    beep_cnt = beep_cnt + 1
  old_state = state
print '---[status] goal   ---'
Rh2Base_ctrl.stop()
Rh2Base_ctrl.navigation.outports['status'].close()
# 到着しました～
Rh2Voice_ctrl.talk(mstr007)
