#!/usr/bin/env python
#coding=utf-8
import rospy
import cv2
from aikit_lib import aikit_2wd
aikit=aikit_2wd.robot() 
p1=[0.9,1.58,90]
shibiequ_info=[0.3,3.15,90]
hamburger_info=[2.45,3.09,90]
bottle_info=[3.06,2.9,-90]
apple_info=[2.37,0.15,-90]
pitaya_info=[3.02,0.22,90]
chair1_info=[2.19,2.03,90]
object_dict={"shibiequ":shibiequ_info,"hamburger":hamburger_info,"bottle":bottle_info,"apple":apple_info,"pitaya":pitaya_info}

claw_dict={"bottle":0,"hamburger":15,"apple":8,"pitaya":10}
name_list=["bottle","hambuger","apple","pitaya"]

def nav_to_name(name_string):
    aikit.nav_to(object_dict[name_string][0],object_dict[name_string][1],object_dict[name_string][2])
def claw_name(name_string):
    aikit.set_claw(claw_dict[name_string])

if __name__=="__main__":
    aikit.set_claw(100)
    aikit.set_pose()
    aikit.play_sound("餐厅收纳任务开始，导航前往识别区")
    aikit.nav_to(1.76,1.58,90)

    object_count=0

    while True:
        aikit.set_claw(100)
        nav_to_name("shibiequ")
        aikit.play_sound("到达识别区，开始识别")
        result=aikit.object_label()
        print(result)
        if result in name_list:
            object_count+=1
            aikit.play_sound("识别结果为")
            aikit.play_sound(str(result))
            aikit.play_sound("把物品交给我吧")
            claw_name(result)
            aikit.play_sound("导航前往目标所在地")
            nav_to_name(result)
            aikit.go(0.2,1)
            aikit.set_claw(30)
            rospy.sleep(1)
            aikit.go(-0.2,1)
        if object_count>=3:
            break
    aikit.play_sound("任务完成")