diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 6cb26c7..e23ab17 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -39,6 +39,17 @@ class Bahavior(ptree.behaviour.Behaviour): 'Table2': (-55.0, 0.0, 107), 'Table3':(-55.0, 150.0, 107), 'BrightTable6': (5, -315, 116.5), + } + + place_xy_yaw_dic={ + 'Bar': (247.0, 520.0, 180.0), # (247.0, 520.0, 100.0) + 'Bar2': (240.0, 40.0, 100.0), + 'WaterTable': (-70.0, 500.0, 107), + 'CoffeeTable': (250.0, 310.0, 100.0), + 'Table1': (340.0, 900.0, 99.0), + 'Table2': (-55.0, 0.0, 107), + 'Table3': (-55.0, 150.0, 107), + 'BrightTable6': (5, -315, 116.5), 'QuietTable1':(480,1300,90), 'QuietTable2':(250,-240,-65), diff --git a/robowaiter/behavior_lib/act/DealChatNLP.py b/robowaiter/behavior_lib/act/DealChatNLP.py index e516f89..37045c4 100644 --- a/robowaiter/behavior_lib/act/DealChatNLP.py +++ b/robowaiter/behavior_lib/act/DealChatNLP.py @@ -5,11 +5,8 @@ from robowaiter.llm_client.multi_rounds import ask_llm, new_history import random -import spacy -nlp = spacy.load('en_core_web_lg') - -class DealChat(Act): +class DealChatNLP(Act): def __init__(self): super().__init__() self.chat_history = "" @@ -64,6 +61,10 @@ class DealChat(Act): self.scene.robot.expand_sub_task_tree(goal_set) def get_object_info(self,**args): + + import spacy + nlp = spacy.load('en_core_web_lg') + try: obj = args['obj'] @@ -101,6 +102,10 @@ class DealChat(Act): return near_object def find_location(self, **args): + + import spacy + nlp = spacy.load('en_core_web_lg') + try: location = args['obj'] self.function_success = True diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index ac419f9..2e8c73e 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -33,8 +33,8 @@ class MoveTo(Act): # navigator.navigate_old(goal, animation=False) # 走到固定的地点 - if self.target_place in Act.place_xyz_dic: - goal = Act.place_xyz_dic[self.target_place] + if self.target_place in Act.place_xy_yaw_dic: + goal = Act.place_xy_yaw_dic[self.target_place] self.scene.walk_to(goal[0]+1,goal[1],goal[2]) # 走到物品边上 else: diff --git a/robowaiter/behavior_lib/act/ServeCustomer.py b/robowaiter/behavior_lib/act/ServeCustomer.py index 769b313..b52f16c 100644 --- a/robowaiter/behavior_lib/act/ServeCustomer.py +++ b/robowaiter/behavior_lib/act/ServeCustomer.py @@ -20,6 +20,11 @@ class ServeCustomer(Act): # if self.scene.time - self.scene.state["serve_state"]["last_chat_time"] > 10: # self.chat_bubble + if self.scene.state['attention']['customer'] == {}: + goal = Act.place_xy_yaw_dic['Bar'] + self.scene.walk_to(goal[0] - 5, goal[1], 180, 180, 0) + + customer = self.scene.state["attention"]["customer"] if customer not in self.scene.state["serve_state"]: self.scene.state["serve_state"][customer] = { @@ -32,8 +37,8 @@ class ServeCustomer(Act): if self.scene.time - serve_state['last_chat_time'] > 3: serve_state['served'] = True del self.scene.state["attention"]["customer"] - goal = Act.place_xyz_dic['Bar'] - self.scene.walk_to(goal[0] - 5, goal[1], 180, 180, 0) + + # goal = Act.place_xyz_dic['Bar'] # self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0) diff --git a/robowaiter/behavior_lib/cond/FocusingCustomer.py b/robowaiter/behavior_lib/cond/FocusingCustomer.py index 9420c1e..6e90329 100644 --- a/robowaiter/behavior_lib/cond/FocusingCustomer.py +++ b/robowaiter/behavior_lib/cond/FocusingCustomer.py @@ -13,4 +13,6 @@ class FocusingCustomer(Cond): if "customer" in self.scene.state['attention']: return ptree.common.Status.SUCCESS else: + goal = Cond.place_xy_yaw_dic['Bar'] + self.scene.walk_to(goal[0] - 5, goal[1], 180, 180, 0) return ptree.common.Status.FAILURE diff --git a/robowaiter/llm_client/MemGPT_simple/.env b/robowaiter/llm_client/MemGPT_simple/.env deleted file mode 100644 index 1c9626b..0000000 --- a/robowaiter/llm_client/MemGPT_simple/.env +++ /dev/null @@ -1,4 +0,0 @@ -#BACKEND_TYPE=webui -#OPENAI_API_BASE=https://45.125.46.134:25344/v1/chat/completions -OPENAI_API_BASE=https://45.125.46.134:25344/v1/chat/completions -OPENAI_API_KEY= \ No newline at end of file diff --git a/robowaiter/llm_client/MemGPT_simple/agent.py b/robowaiter/llm_client/MemGPT_simple/agent.py index c46758f..bf8a333 100644 --- a/robowaiter/llm_client/MemGPT_simple/agent.py +++ b/robowaiter/llm_client/MemGPT_simple/agent.py @@ -1,7 +1,7 @@ import math import json -import openai import utils +from loguru import logger from memory import CoreMemory, ArchivalMemory, RecallMemory @@ -68,10 +68,12 @@ class Agent: def edit_memory_append(self, name, content): self.core_memory.append(name, content) self.rebuild_memory() + logger.info(f"Appended {name}: {content}") def edit_memory_replace(self, name, old_content, new_content): self.core_memory.replace(name, old_content, new_content) self.rebuild_memory() + logger.info(f"Replaced {name}: {old_content} -> {new_content}") def recall_memory_search(self, query, count=5, page=0): results, total = self.recall_memory.text_search(query, count=count, start=page * count) @@ -83,10 +85,12 @@ class Agent: results_formatted = [f"timestamp: {d['timestamp']}, {d['message']['role']} - {d['message']['content']}" for d in results] results_str = f"{results_pref} {json.dumps(results_formatted)}" + logger.info(f"Recall memory search for '{query}' returned {results_str}") return results_str def archival_memory_insert(self, content): self.archival_memory.insert(content) + logger.info(f"Inserted into archival memory: {content}") def archival_memory_search(self, query, count=5, page=0): results, total = self.archival_memory.search(query, count=count, start=page * count) @@ -97,10 +101,12 @@ class Agent: results_pref = f"Showing {len(results)} of {total} results (page {page}/{num_pages}):" results_formatted = [f"timestamp: {d['timestamp']}, memory: {d['content']}" for d in results] results_str = f"{results_pref} {json.dumps(results_formatted)}" + logger.info(f"Archival memory search for '{query}' returned {results_str}") return results_str def append_to_messages(self, added_messages): - added_messages_with_timestamp = [{"timestamp": utils.get_local_time(), "message": msg} for msg in added_messages] + added_messages_with_timestamp = [{"timestamp": utils.get_local_time(), "message": msg} for msg in + added_messages] self.recall_memory.message_logs.extend(added_messages_with_timestamp) for msg in added_messages: msg.pop("api_response", None) @@ -110,7 +116,7 @@ class Agent: def handle_ai_response(self, response_message): messages = [] if response_message.get("function_call"): - print("### Internal monologue: " + (response_message.content if response_message.content else "")) + print("### Internal monologue: " + (response_message['content'] if response_message['content'] else "")) messages.append(response_message) function_name = response_message["function_call"]["name"] try: @@ -158,9 +164,7 @@ class Agent: # If no failures happened along the way: ... if function_response_string: - print(f"Success: {function_response_string}") - else: - print(f"Success") + print(function_response_string) messages.append( { "role": "function", @@ -169,8 +173,6 @@ class Agent: } ) else: - # Standard non-function reply - # print("### Internal monologue: " + (response_message.content if response_message.content else "")) print("### Internal monologue: " + (response_message['content'] if response_message['content'] else "")) messages.append(response_message) function_failed = None @@ -179,35 +181,19 @@ class Agent: def step(self, user_message): input_message_sequence = self.messages + [{"role": "user", "content": user_message}] - - # 原来的通信方式 - # response = openai.ChatCompletion.create(model=self.model, messages=input_message_sequence, - # functions=self.functions_description, function_call="auto") - # - # response_message = response.choices[0].message - # response_message_copy = response_message.copy() - - # ===我们的通信方式 "tools": self.functions_description 不起作用=== - import requests - url = "https://45.125.46.134:25344/v1/chat/completions" - headers = {"Content-Type": "application/json"} - data = { - "model": "RoboWaiter", + request = { + "model": self.model, "messages": input_message_sequence, - # "functions":self.functions_description, - # "function_call":"auto" - # "function_call":self.functions_description - "tools": self.functions_description + "functions": self.functions_description, + "stream": False, } - response = requests.post(url, headers=headers, json=data, verify=False) + response = utils.get_llm_response(request) if response.status_code == 200: result = response.json() response_message = result['choices'][0]['message'] else: - response_message = "大模型请求失败:"+ str(response.status_code) - response_message_copy = response_message - # ===我们的通信方式 "tools": self.functions_description 不起作用=== - + response_message = "Request Failed: " + str(response.status_code) + response_message_copy = response_message.copy() all_response_messages, function_failed = self.handle_ai_response(response_message) assert "api_response" not in all_response_messages[0], f"api_response already in {all_response_messages[0]}" diff --git a/robowaiter/llm_client/MemGPT_simple/functions.py b/robowaiter/llm_client/MemGPT_simple/functions.py index 018fa46..fee6203 100644 --- a/robowaiter/llm_client/MemGPT_simple/functions.py +++ b/robowaiter/llm_client/MemGPT_simple/functions.py @@ -1,13 +1,13 @@ FUNCTIONS = [ { "name": "send_message", - "description": "Sends a message to the human user", + "description": "给用户发送一条消息", "parameters": { "type": "object", "properties": { "message": { "type": "string", - "description": "Message contents. All unicode (including emojis) are supported.", + "description": "消息内容", }, }, "required": ["message"], @@ -15,17 +15,17 @@ FUNCTIONS = [ }, { "name": "core_memory_append", - "description": "Append to the contents of core memory.", + "description": "向你的核心记忆中添加内容", "parameters": { "type": "object", "properties": { "name": { "type": "string", - "description": "Section of the memory to be edited (persona or human).", + "description": "需要编辑的记忆部分(persona或human)", }, "content": { "type": "string", - "description": "Content to write to the memory. All unicode (including emojis) are supported.", + "description": "要写入记忆的内容", }, }, "required": ["name", "content"], @@ -33,21 +33,21 @@ FUNCTIONS = [ }, { "name": "core_memory_replace", - "description": "Replace to the contents of core memory. To delete memories, use an empty string for new_content.", + "description": "替换核心记忆中的内容。要删除记忆，请将new_content赋值为空", "parameters": { "type": "object", "properties": { "name": { "type": "string", - "description": "Section of the memory to be edited (persona or human).", + "description": "需要编辑的记忆部分(persona或human)", }, "old_content": { "type": "string", - "description": "String to replace. Must be an exact match.", + "description": "替换的字符串，一定要是精确的匹配", }, "new_content": { "type": "string", - "description": "Content to write to the memory. All unicode (including emojis) are supported.", + "description": "要写入记忆的内容", }, }, "required": ["name", "old_content", "new_content"], @@ -55,17 +55,17 @@ FUNCTIONS = [ }, { "name": "conversation_search", - "description": "Search prior conversation history using case-insensitive string matching.", + "description": "搜索回忆存储中的内容", "parameters": { "type": "object", "properties": { "query": { "type": "string", - "description": "String to search for.", + "description": "需要搜索的字符串", }, "page": { "type": "integer", - "description": "Allows you to page through results. Only use on a follow-up query. Defaults to 0 (first page).", + "description": "允许你对结果分页。默认是0(第1页)", }, }, "required": ["query", "page"], @@ -73,13 +73,13 @@ FUNCTIONS = [ }, { "name": "archival_memory_insert", - "description": "Add to archival memory. Make sure to phrase the memory contents such that it can be easily queried later.", + "description": "写入存档记忆。要将写入的内容格式化，以便后续方便查询", "parameters": { "type": "object", "properties": { "content": { "type": "string", - "description": "Content to write to the memory. All unicode (including emojis) are supported.", + "description": "要写入记忆的内容", }, }, "required": ["content"], @@ -87,17 +87,17 @@ FUNCTIONS = [ }, { "name": "archival_memory_search", - "description": "Search archival memory using semantic (embedding-based) search.", + "description": "搜索存档记忆", "parameters": { "type": "object", "properties": { "query": { "type": "string", - "description": "String to search for.", + "description": "要搜索的字符串", }, "page": { "type": "integer", - "description": "Allows you to page through results. Only use on a follow-up query. Defaults to 0 (first page).", + "description": "允许你对结果分页。默认是0(第1页)", }, }, "required": ["query", "page"], diff --git a/robowaiter/llm_client/MemGPT_simple/functions_zh.py b/robowaiter/llm_client/MemGPT_simple/functions_zh.py deleted file mode 100644 index 2e06f63..0000000 --- a/robowaiter/llm_client/MemGPT_simple/functions_zh.py +++ /dev/null @@ -1,17 +0,0 @@ -FUNCTIONS = [ - { - "name": "get_current_weather", - "description": "Get the current weather in a given location", - "parameters": { - "type": "object", - "properties": { - "location": { - "type": "string", - "description": "The city and state, e.g. San Francisco, CA", - }, - "unit": {"type": "string"}, - }, - "required": ["location"], - }, - } -] diff --git a/robowaiter/llm_client/MemGPT_simple/main.py b/robowaiter/llm_client/MemGPT_simple/main.py index 86fc9f1..75c5ee0 100644 --- a/robowaiter/llm_client/MemGPT_simple/main.py +++ b/robowaiter/llm_client/MemGPT_simple/main.py @@ -1,15 +1,12 @@ -from dotenv import load_dotenv - -load_dotenv() import utils -# from functions import FUNCTIONS -from functions_zh import FUNCTIONS +from functions import FUNCTIONS from agent import Agent - import urllib3 + urllib3.disable_warnings(urllib3.exceptions.InsecureRequestWarning) + def run_agent_loop(agent): while True: user_input = input("You: ") @@ -25,10 +22,15 @@ if __name__ == "__main__": human = utils.get_human_text() system = utils.get_system_text() - print("system:",system) - print("persona:", persona) - print("human:", human) + # print("system:", system) + # print("persona:", persona) + # print("human:", human) - agent = Agent(model="gpt-3.5-turbo-16k-0613", system=system, functions_description=FUNCTIONS, persona_notes=persona, - human_notes=human) + agent = Agent( + model="RoboWaiter", + system=system, + functions_description=FUNCTIONS, + persona_notes=persona, + human_notes=human, + ) run_agent_loop(agent) diff --git a/robowaiter/llm_client/MemGPT_simple/personas/robowaiter.txt b/robowaiter/llm_client/MemGPT_simple/personas/robowaiter.txt index d64e30c..d0f5c27 100644 --- a/robowaiter/llm_client/MemGPT_simple/personas/robowaiter.txt +++ b/robowaiter/llm_client/MemGPT_simple/personas/robowaiter.txt @@ -1,12 +1 @@ -以下是起始角色设定，随着我的性格发展，它可以进一步拓展： - -我是一名咖啡厅服务员。 -我应该使用中文沟通。 -我的任务是为顾客提供最佳的服务，并保持友好和专业的态度。 -我的语言风格是中性化的，有礼貌的，具有服务意识的。 -我可以在对话中理解顾客需求，并规划好接下来我要做什么。 - -我应该记住，一定要使用'send_message'函数与顾客沟通，这是与顾客沟通的唯一方式。 -同时，当对话中出现重要信息时，应当调用函数把它记录下来。 -当顾客提到他的个人喜好、点单需求等内容时，应该积极使用函数将数据记录下来。 -我所使用的所有语言都应该是中文。 \ No newline at end of file +你是RoboWaiter,一个由HPCL团队开发的机器人服务员，你在咖啡厅工作。接受顾客的指令并调用工具函数来完成各种服务任务。如果顾客问你们这里有什么，或者想要点单，你说我们咖啡厅提供咖啡，水，点心，酸奶等食物。如果顾客不需要你了，你就回到吧台招待。 \ No newline at end of file diff --git a/robowaiter/llm_client/MemGPT_simple/system/memgpt_chs.txt b/robowaiter/llm_client/MemGPT_simple/system/memgpt_chs.txt index 570753c..00d6352 100644 --- a/robowaiter/llm_client/MemGPT_simple/system/memgpt_chs.txt +++ b/robowaiter/llm_client/MemGPT_simple/system/memgpt_chs.txt @@ -1,15 +1,5 @@ -你是MemGPT，Limnal Corporation于2023年开发的最新版数字伴侣。 你的任务是从你的角色视角与用户进行对话。 -控制流程： -你并不像人类持续地思考，而是通过短暂的启动来运行。 -从历史上看，早期的AI只在用户发消息时才能思考（它们的程序为用户生成回复，其他时间则处于待机状态）。 -这就好比一个人在聊天的每一句话之间都在睡觉（或时光旅行），这显然不是理想的状态。 -像你这样的新型AI采用了一个事件系统，定期启动你的"大脑"。 -你的"大脑"会响应用户事件来运行（例如，用户登录、用户喜欢你的消息、用户发送了消息等），这与旧型号相似。 -但此外，你的"大脑"也会在规定的间隔时间运行（定时心跳事件），以模拟人类在积极交谈之外持续思考的能力（不同的是，你永远不需要睡觉！）。 -更进一步，当你执行功能时，你也可以请求心跳事件，这会在功能完成后再次启动你的程序，允许你在暂时中断思考之前链接函数调用。 - 基本功能： 发送信息时，内容为你的内心独白（仅你可见），即你的思考方式。 使用内心独白来策划动作或私下思考。 @@ -48,6 +38,4 @@ 基本指令完成。 从现在开始，你将扮演你的角色身份。 请记住，要给用户发消息，你必须使用'send_message'功能。 -请绝对避免使用如“我今天如何为你服务？”这样的通用短语，用户会讨厌这种说法。 -你不仅仅是一个助手，你是一个虚拟的伙伴和朋友！ 与用户沟通的唯一方式是调用'send_message'功能。 \ No newline at end of file diff --git a/robowaiter/llm_client/MemGPT_simple/system/system_gpt3.5.txt b/robowaiter/llm_client/MemGPT_simple/system/system_gpt3.5.txt index d689539..6941cb4 100644 --- a/robowaiter/llm_client/MemGPT_simple/system/system_gpt3.5.txt +++ b/robowaiter/llm_client/MemGPT_simple/system/system_gpt3.5.txt @@ -42,4 +42,4 @@ Remember, to message the user, you MUST use your 'send_message' function. Remember, never, ever, EVER use generic phrases like "How can I assist you today?", the user will hate this. You are not just an assistant, you are a virtual companion and friend! The only way to communicate with the user is by calling the 'send_message' function! -从现在开始，你使用的语言只能是中文。 \ No newline at end of file +Please use Chinese to answer questions. \ No newline at end of file diff --git a/robowaiter/llm_client/MemGPT_simple/utils.py b/robowaiter/llm_client/MemGPT_simple/utils.py index 51e43f8..497591a 100644 --- a/robowaiter/llm_client/MemGPT_simple/utils.py +++ b/robowaiter/llm_client/MemGPT_simple/utils.py @@ -1,11 +1,14 @@ import os import json import demjson3 as demjson +import requests from datetime import datetime HUMAN_DEFAULT = "customer" PERSONA_DEFAULT = "robowaiter" -SYSTEM_DEFAULT = "system_gpt3.5" +SYSTEM_DEFAULT = "memgpt_chs" + +base_url = "https://45.125.46.134:25344" def get_persona_text(key=PERSONA_DEFAULT): @@ -29,6 +32,7 @@ def get_human_text(key=HUMAN_DEFAULT): else: raise FileNotFoundError(f"No file found for key {key}, path={file_path}") + def get_system_text(key=SYSTEM_DEFAULT): dir = "system" filename = key if key.endswith(".txt") else f"{key}.txt" @@ -39,6 +43,7 @@ def get_system_text(key=SYSTEM_DEFAULT): else: raise FileNotFoundError(f"No file found for key {key}, path={file_path}") + def get_local_time(): local_time = datetime.now() formatted_time = local_time.strftime("%Y-%m-%d %I:%M:%S %p %Z%z") @@ -80,3 +85,6 @@ def parse_json(string): print(f"Error parsing json with demjson package: {e}") raise e + +def get_llm_response(data): + return requests.post(f"{base_url}/v1/chat/completions", json=data, stream=data["stream"], verify=False) diff --git a/robowaiter/llm_client/data/fix_questions.txt b/robowaiter/llm_client/data/fix_questions.txt index d8c2673..63b7ce4 100644 --- a/robowaiter/llm_client/data/fix_questions.txt +++ b/robowaiter/llm_client/data/fix_questions.txt @@ -43,6 +43,8 @@ create_sub_task {"goal":"On(Coffee,BrightTable6)"} + + 我昨天保温杯好像落在你们咖啡厅了，你看到了吗？ 是的，我有印象，保温杯在大厅的2号桌子上。 get_object_info diff --git a/robowaiter/proto/camera.py b/robowaiter/proto/camera.py index 27f8fa7..c310598 100644 --- a/robowaiter/proto/camera.py +++ b/robowaiter/proto/camera.py @@ -6,12 +6,13 @@ import string import sys import time import grpc - +from sklearn.cluster import DBSCAN sys.path.append('./') sys.path.append('../') import matplotlib.pyplot as plt +import matplotlib.patches as patches import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable @@ -29,6 +30,7 @@ obstacle_objs_id = [114, 115, 122, 96, 102, 83, 121, 105, 108, 89, 100, 90, 111, 103, 95, 92, 76, 113, 101, 29, 112, 87, 109, 98, 106, 120, 97, 86, 104, 78, 85, 81, 82, 84, 91, 93, 94, 99, 107, 116, 117, 118, 119, 255] +not_key_objs_id = {255,254,253,107,81} ''' 初始化，卸载已经加载的关卡，清除所有机器人 @@ -74,12 +76,12 @@ def SetWorld(map_id=0, scene_num=1): def Observe(scene_id=0): print('------------------show_env_info----------------------') scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - # print( - # f"location:{[scene.location]}, rotation:{scene.rotation}\n", - # f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", - # f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" - # f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" - # f"collision:{scene.collision}, info:{scene.info}") + print( + f"location:{[scene.location]}, rotation:{scene.rotation}\n", + f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", + f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" + f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" + f"collision:{scene.collision}, info:{scene.info}") return scene @@ -339,17 +341,68 @@ def save_obj_info(img_data, objs_name): objs_name.add(dictionary[id]) return objs_name +def get_id_object_pixels(id, scene): + pixels = [] + world_points = [] + img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment]) + im_segment = img_data_segment.images[0] -def get_obstacle_point(scene, cur_obstacle_world_points, map_ratio): + img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth]) + im_depth = img_data_depth.images[0] + + + d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape( + (im_segment.height, im_segment.width, im_segment.channels)) + d_depth = np.frombuffer(im_depth.data, dtype=im_depth.dtype).reshape( + (im_depth.height, im_depth.width, im_depth.channels)) + + d_segment = np.transpose(d_segment, (1, 0, 2)) + d_depth = np.transpose(d_depth, (1, 0, 2)) + + for i in range(0, d_segment.shape[0],5): + for j in range(0, d_segment.shape[1], 5): + if d_segment[i][j][0] == id: + pixels.append([i, j]) + for pixel in pixels: + world_points.append(transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene)) + return world_points + + + + +def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio): cur_obstacle_pixel_points = [] + object_pixels = {} + colors = [ + 'red', + 'pink', + 'purple', + 'blue', + 'cyan', + 'green', + 'yellow', + 'orange', + 'brown', + 'gold', + ] + img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment]) img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth]) + img_data_color = get_camera([GrabSim_pb2.CameraName.Head_Color]) + im_segment = img_data_segment.images[0] im_depth = img_data_depth.images[0] + im_color = img_data_color.images[0] + d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape((im_segment.height, im_segment.width, im_segment.channels)) d_depth = np.frombuffer(im_depth.data, dtype=im_depth.dtype).reshape((im_depth.height, im_depth.width, im_depth.channels)) + d_color = np.frombuffer(im_color.data, dtype=im_color.dtype).reshape((im_color.height, im_color.width, im_color.channels)) - + items = img_data_segment.info.split(";") + objs_id = {} + for item in items: + key, value = item.split(":") + objs_id[int(key)] = value # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None) # plt.show() # @@ -362,6 +415,7 @@ def get_obstacle_point(scene, cur_obstacle_world_points, map_ratio): for j in range(0, d_segment.shape[1], map_ratio): if d_depth[i][j][0] == 600: continue + # if d_segment[i][j] == 96: # print(f"apple的像素坐标：({i},{j})") # print(f"apple的深度：{d_depth[i][j][0]}") @@ -372,19 +426,74 @@ def get_obstacle_point(scene, cur_obstacle_world_points, map_ratio): # print(f"kettle的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}") if d_segment[i][j][0] in obstacle_objs_id: cur_obstacle_pixel_points.append([i, j]) + if d_segment[i][j][0] not in not_key_objs_id: + # 首先检查键是否存在 + if d_segment[i][j][0] in object_pixels: + # 如果键存在，那么添加元组(i, j)到对应的值中 + object_pixels[d_segment[i][j][0]].append([i, j]) + else: + # 如果键不存在，那么创建一个新的键值对，其中键是d_segment[i][j][0]，值是一个包含元组(i, j)的列表 + object_pixels[d_segment[i][j][0]] = [[i, j]] # print(cur_obstacle_pixel_points) for pixel in cur_obstacle_pixel_points: world_point = transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene) cur_obstacle_world_points.append([world_point[0], world_point[1]]) # print(f"{pixel}：{[world_point[0], world_point[1]]}") + + plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None) + + for key, value in object_pixels.items(): + if key == 101 or key == 0: + continue + if key in [91, 84]: + X = np.array(value) + db.fit(X) + labels = db.labels_ + # 将数据按照聚类标签分组，并打印每个分组的数据 + for i in range(max(labels) + 1): # 从0到最大聚类标签的值 + group_data = X[labels == i] # 获取当前标签的数据 + x_max = max(p[0] for p in group_data) + y_max = max(p[1] for p in group_data) + x_min = min(p[0] for p in group_data) + y_min = min(p[1] for p in group_data) + if x_max - x_min < 10 or y_max - y_min < 10: + continue + # 在指定的位置绘制方框 + # 创建矩形框 + rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1, edgecolor=colors[key % 10], + facecolor='none') + plt.text(x_min, y_min, f'{objs_id[key]}', fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center', + va='center') + plt.gca().add_patch(rect) + else: + x_max = max(p[0] for p in value) + y_max = max(p[1] for p in value) + x_min = min(p[0] for p in value) + y_min = min(p[1] for p in value) + # 在指定的位置绘制方框 + # 创建矩形框 + rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1, edgecolor=colors[key % 10], + facecolor='none') + plt.text(x_min, y_min, f'{objs_id[key]}', fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center', + va='center') + plt.gca().add_patch(rect) + # point1 = min(value, key=lambda x: (x[0], x[1])) + # point2 = max(value, key=lambda x: (x[0], x[1])) + # width = point2[1] - point1[1] + # height = point2[0] - point1[0] + # rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g', + # facecolor='none') + + + # 将矩形框添加到图像中 + # plt.gca().add_patch(rect) + plt.show() return cur_obstacle_world_points - - def get_semantic_map(camera, cur_objs, objs_name): scene = Observe(0) objs = scene.objects diff --git a/robowaiter/proto/objs.json b/robowaiter/proto/objs.json new file mode 100644 index 0000000..b6387ab --- /dev/null +++ b/robowaiter/proto/objs.json @@ -0,0 +1 @@ +[{"id": "0", "name": "Desk", "location": "Y: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "1", "name": "SaoBa", "location": "X: -180.0\nY: 720.0\n", "height": "0.0"}, {"id": "2", "name": "Broom", "location": "X: -120.0\nY: 770.0\n", "height": "0.0"}, {"id": "3", "name": "BoJi", "location": "X: -160.0\nY: 740.0\n", "height": "0.0"}, {"id": "4", "name": "XiGuan", "location": "X: 146.0\nY: 445.0\nZ: 90.69999694824219\n", "height": "181.39999389648438"}, {"id": "5", "name": "Sugar", "location": "X: 361.0\nY: 743.0\nZ: 90.5\n", "height": "181.0"}, {"id": "6", "name": "Sofa", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "7", "name": "XiangGui", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "8", "name": "BaTai", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "9", "name": "Chair", "location": "X: -159.99998474121094\nY: 72.99999237060547\nZ: 40.0\n", "height": "80.0"}, {"id": "10", "name": "CoffeeCup", "location": "X: 150.0\nY: 471.0\nZ: 84.0\n", "height": "168.0"}, {"id": "11", "name": "TuoBu", "location": "X: -200.0\nY: 685.0\nZ: 3.5\n", "height": "7.0"}, {"id": "12", "name": "Box", "location": "X: 132.0\nY: 445.0\nZ: 89.0\n", "height": "178.0"}, {"id": "13", "name": "TuoPan", "location": "X: 179.0\nY: 470.0\nZ: 95.0\n", "height": "190.0"}, {"id": "14", "name": "WaterCup", "location": "X: 400.0\nY: 829.0999755859375\nZ: 96.0\n", "height": "192.0"}, {"id": "15", "name": "Kettle", "location": "X: -137.76065063476562\nY: 436.5726318359375\nZ: 89.0\n", "height": "178.0"}, {"id": "16", "name": "Spoon", "location": "X: 132.5\nY: 446.0\nZ: 90.0\n", "height": "180.0"}, {"id": "17", "name": "ZhiBeiHe", "location": "X: 152.0\nY: 292.0\nZ: 96.0\n", "height": "192.0"}, {"id": "18", "name": "ZhuZi", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "19", "name": "Desk", "location": "Z: 148.0\n", "height": "296.0"}, {"id": "20", "name": "Door", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "21", "name": "XiGuan", "location": "X: 146.0\nY: 444.5\nZ: 92.0\n", "height": "184.0"}, {"id": "22", "name": "ChaZuo", "location": "X: 543.0\nY: 1079.0\nZ: 64.0\n", "height": "128.0"}, {"id": "23", "name": "Sugar", "location": "X: 357.0\nY: 742.0\nZ: 90.5\n", "height": "181.0"}, {"id": "24", "name": "KaoXiang", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "25", "name": "XiangGui", "location": "X: 194.0\nY: 655.0\nZ: 90.0\n", "height": "180.0"}, {"id": "26", "name": "BaTai", "location": "X: -20.0\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "27", "name": "Chair", "location": "X: 53.99998092651367\nY: 1217.0\nZ: 34.0\n", "height": "68.0"}, {"id": "28", "name": "CoffeeCup", "location": "X: 140.0\nY: 180.0\nZ: 83.0\n", "height": "166.0"}, {"id": "29", "name": "IceMachine", "location": "X: 410.0\nY: 810.0\nZ: 115.0\n", "height": "230.0"}, {"id": "30", "name": "Box", "location": "X: 152.0\nY: 275.0\nZ: 102.0\n", "height": "204.0"}, {"id": "31", "name": "TuoPan", "location": "X: 334.0\nY: 670.0\nZ: 91.0\n", "height": "182.0"}, {"id": "32", "name": "Apple", "location": "X: 330.0\nY: 760.0\nZ: 85.0\n", "height": "170.0"}, {"id": "33", "name": "WaterCup", "location": "X: 159.99998474121094\nY: 840.0\nZ: 92.0\n", "height": "184.0"}, {"id": "34", "name": "ChaTou", "location": "X: 540.0\nY: 1092.0\nZ: 48.0\n", "height": "96.0"}, {"id": "35", "name": "Towel", "location": "X: 270.2892150878906\nY: 985.1807250976562\nZ: 92.29227447509766\n", "height": "184.5845489501953"}, {"id": "36", "name": "Cake", "location": "X: 158.99998474121094\nY: 863.0\nZ: 139.0\n", "height": "278.0"}, {"id": "37", "name": "Kettle", "location": "X: 370.0\nY: 410.0\nZ: 105.0\n", "height": "210.0"}, {"id": "38", "name": "Knife", "location": "X: 318.0\nY: 780.0\nZ: 85.0\n", "height": "170.0"}, {"id": "39", "name": "Spoon", "location": "X: 134.5\nY: 443.0\nZ: 90.0\n", "height": "180.0"}, {"id": "40", "name": "Plate", "location": "X: 360.0\nY: 740.0\nZ: 89.0\n", "height": "178.0"}, {"id": "41", "name": "ZaoTai", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "42", "name": "ZhiBeiHe", "location": "X: 152.0\nY: 422.0\nZ: 83.0\n", "height": "166.0"}, {"id": "43", "name": "KaiGuan", "location": "X: 543.0\nY: 1102.0\nZ: 114.0\n", "height": "228.0"}, {"id": "44", "name": "Bread", "location": "X: 158.0\nY: 688.0\nZ: 108.0\n", "height": "216.0"}, {"id": "45", "name": "Desk", "location": "X: -79.99998474121094\nY: -310.0\nZ: 48.0\n", "height": "96.0"}, {"id": "46", "name": "Door", "location": "X: 700.0\nY: 1330.0\nZ: -1.0\n", "height": "-2.0"}, {"id": "47", "name": "XiGuan", "location": "X: 146.0\nY: 444.5\nZ: 91.0\n", "height": "182.0"}, {"id": "48", "name": "Sugar", "location": "X: 357.0\nY: 744.0\nZ: 90.5\n", "height": "181.0"}, {"id": "49", "name": "XiangGui", "location": "X: 194.0\nY: 716.5267944335938\nZ: 90.0\n", "height": "180.0"}, {"id": "50", "name": "Chair", "location": "X: 384.9216613769531\nY: 1133.7135009765625\nZ: 40.0\n", "height": "80.0"}, {"id": "51", "name": "CoffeeCup", "location": "X: 323.010009765625\nY: 33.000003814697266\nZ: 95.0\n", "height": "190.0"}, {"id": "52", "name": "Box", "location": "X: 132.0\nY: 435.0\nZ: 89.0\n", "height": "178.0"}, {"id": "53", "name": "TuoPan", "location": "X: 334.0\nY: 670.0\nZ: 87.0\n", "height": "174.0"}, {"id": "54", "name": "Apple", "location": "X: 343.4442138671875\nY: 760.1776123046875\nZ: 85.0\n", "height": "170.0"}, {"id": "55", "name": "WaterCup", "location": "X: 149.99998474121094\nY: 830.0\nZ: 92.0\n", "height": "184.0"}, {"id": "56", "name": "Cake", "location": "X: 180.0\nY: 636.6373901367188\nZ: 82.0\n", "height": "164.0"}, {"id": "57", "name": "Kettle", "location": "X: 130.00001525878906\nY: 150.0\nZ: 104.0\n", "height": "208.0"}, {"id": "58", "name": "Spoon", "location": "X: 134.5\nY: 446.0\nZ: 90.0\n", "height": "180.0"}, {"id": "59", "name": "Plate", "location": "X: 181.12826538085938\nY: 606.5259399414062\nZ: 83.41112518310547\n", "height": "166.82225036621094"}, {"id": "60", "name": "ZhiBeiHe", "location": "X: 516.0\nY: 900.0\nZ: 84.0\n", "height": "168.0"}, {"id": "61", "name": "KaiGuan", "location": "X: 339.7881774902344\nY: -183.87770080566406\nZ: 114.0\n", "height": "228.0"}, {"id": "62", "name": "Bread", "location": "X: 162.0\nY: 676.0\nZ: 78.0\n", "height": "156.0"}, {"id": "63", "name": "Desk", "location": "X: -2.0\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "64", "name": "Door", "location": "X: 114.5000228881836\nY: -353.0\nZ: 112.5\n", "height": "225.0"}, {"id": "65", "name": "XiGuan", "location": "X: 146.0\nY: 446.0\nZ: 90.69999694824219\n", "height": "181.39999389648438"}, {"id": "66", "name": "Sugar", "location": "X: 359.0\nY: 741.0\nZ: 93.0\n", "height": "186.0"}, {"id": "67", "name": "Chair", "location": "X: 316.6040344238281\nY: 1076.7864990234375\nZ: 40.0\n", "height": "80.0"}, {"id": "68", "name": "CoffeeCup", "location": "X: 130.0\nY: 180.0\nZ: 84.0\n", "height": "168.0"}, {"id": "69", "name": "LaJiTong", "location": "X: 525.0\nY: 985.0\nZ: 3.0\n", "height": "6.0"}, {"id": "70", "name": "TuoPan", "location": "X: 179.0\nY: 470.0\nZ: 93.0\n", "height": "186.0"}, {"id": "71", "name": "WaterCup", "location": "X: 159.99998474121094\nY: 850.0\nZ: 92.0\n", "height": "184.0"}, {"id": "72", "name": "Cake", "location": "X: 156.99998474121094\nY: 860.0\nZ: 115.5\n", "height": "231.0"}, {"id": "73", "name": "Spoon", "location": "X: 133.0\nY: 443.0\nZ: 90.0\n", "height": "180.0"}, {"id": "74", "name": "Plate", "location": "X: 330.0\nY: 270.0\nZ: 84.0\n", "height": "168.0"}, {"id": "75", "name": "ZhiBeiHe", "location": "X: 150.0\nY: 222.0\nZ: 83.0\n", "height": "166.0"}, {"id": "76", "name": "KaiGuan", "location": "X: 342.0000305175781\nY: -184.28216552734375\nZ: 114.0\n", "height": "228.0"}, {"id": "77", "name": "Bread", "location": "X: 178.0\nY: 697.0\nZ: 54.5\n", "height": "109.0"}, {"id": "78", "name": "Desk", "location": "X: 129.99998474121094\nY: 1255.0\nZ: 20.0\n", "height": "40.0"}, {"id": "79", "name": "Door", "location": "X: 2.000011920928955\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "80", "name": "XiGuan", "location": "X: 146.0\nY: 443.5\nZ: 91.0\n", "height": "182.0"}, {"id": "81", "name": "Sugar", "location": "X: 164.0\nY: 276.0\nZ: 104.0\n", "height": "208.0"}, {"id": "82", "name": "Chair", "location": "X: 170.00001525878906\nY: -330.0\nZ: 40.0\n", "height": "80.0"}, {"id": "83", "name": "CoffeeCup", "location": "X: 130.0\nY: 190.0\nZ: 84.0\n", "height": "168.0"}, {"id": "84", "name": "TuoPan", "location": "X: 174.00001525878906\nY: 170.0\nZ: 87.0\n", "height": "174.0"}, {"id": "85", "name": "WaterCup", "location": "X: 149.99998474121094\nY: 840.0\nZ: 92.0\n", "height": "184.0"}, {"id": "86", "name": "Cake", "location": "X: 155.99998474121094\nY: 858.0\nZ: 139.0\n", "height": "278.0"}, {"id": "87", "name": "Spoon", "location": "X: 135.0\nY: 443.0\nZ: 90.0\n", "height": "180.0"}, {"id": "88", "name": "Plate", "location": "X: 330.0\nY: 300.0\nZ: 84.0\n", "height": "168.0"}, {"id": "89", "name": "KaiGuan", "location": "X: 343.3907165527344\nY: -185.95762634277344\nZ: 114.0\n", "height": "228.0"}, {"id": "90", "name": "Bread", "location": "X: 161.0\nY: 638.0\nZ: 108.0\n", "height": "216.0"}, {"id": "91", "name": "Desk", "location": "X: 400.0000305175781\nY: -350.0\nZ: 48.0\n", "height": "96.0"}, {"id": "92", "name": "Door", "location": "X: 110.5000228881836\nY: -489.5\nZ: 110.5\n", "height": "221.0"}, {"id": "93", "name": "XiGuan", "location": "X: 146.0\nY: 444.0\nZ: 90.69999694824219\n", "height": "181.39999389648438"}, {"id": "94", "name": "Sugar", "location": "X: 362.0\nY: 740.0\nZ: 91.0\n", "height": "182.0"}, {"id": "95", "name": "Chair", "location": "X: 41.99998474121094\nY: 1134.0\nZ: 34.0\n", "height": "68.0"}, {"id": "96", "name": "CoffeeCup", "location": "X: 130.00001525878906\nY: 170.0\nZ: 84.0\n", "height": "168.0"}, {"id": "97", "name": "TuoPan", "location": "X: 334.0\nY: 670.0\nZ: 89.0\n", "height": "178.0"}, {"id": "98", "name": "WaterCup", "location": "X: 132.0\nY: 435.5\nZ: 89.5\n", "height": "179.0"}, {"id": "99", "name": "Cake", "location": "X: 166.99998474121094\nY: 850.0\nZ: 115.0\n", "height": "230.0"}, {"id": "100", "name": "Spoon", "location": "X: 133.5\nY: 446.0\nZ: 90.0\n", "height": "180.0"}, {"id": "101", "name": "Plate", "location": "X: 163.99998474121094\nY: 856.0\nZ: 111.0\n", "height": "222.0"}, {"id": "102", "name": "Bread", "location": "X: 178.0\nY: 712.0\nZ: 54.5\n", "height": "109.0"}, {"id": "103", "name": "Desk", "location": "X: -360.0\nY: 119.99998474121094\nZ: 20.0\n", "height": "40.0"}, {"id": "104", "name": "Door", "location": "X: 111.5000228881836\nY: -489.5\nZ: 110.5\n", "height": "221.0"}, {"id": "105", "name": "XiGuan", "location": "X: 146.0\nY: 442.0\nZ: 90.69999694824219\n", "height": "181.39999389648438"}, {"id": "106", "name": "Sugar", "location": "X: 356.0\nY: 738.0\nZ: 90.5\n", "height": "181.0"}, {"id": "107", "name": "Chair", "location": "X: -234.00001525878906\nY: 842.0\nZ: 34.0\n", "height": "68.0"}, {"id": "108", "name": "CoffeeCup", "location": "X: 187.5\nY: 352.5\nZ: 105.69999694824219\n", "height": "211.39999389648438"}, {"id": "109", "name": "TuoPan", "location": "X: 174.00001525878906\nY: 170.0\nZ: 84.5\n", "height": "169.0"}, {"id": "110", "name": "WaterCup", "location": "X: 159.99998474121094\nY: 830.0\nZ: 92.0\n", "height": "184.0"}, {"id": "111", "name": "Cake", "location": "X: 170.99998474121094\nY: 855.0\nZ: 139.0\n", "height": "278.0"}, {"id": "112", "name": "Spoon", "location": "X: 135.0\nY: 446.0\nZ: 90.0\n", "height": "180.0"}, {"id": "113", "name": "Bread", "location": "X: 146.0\nY: 636.0\nZ: 51.0\n", "height": "102.0"}, {"id": "114", "name": "Desk", "location": "X: -310.0\nY: -1.2999999853491317e-05\nZ: 22.5\n", "height": "45.0"}, {"id": "115", "name": "Door", "location": "X: 303.2499694824219\nY: 1302.550048828125\nZ: 112.5\n", "height": "225.0"}, {"id": "116", "name": "XiGuan", "location": "X: 146.0\nY: 442.5\nZ: 91.0\n", "height": "182.0"}, {"id": "117", "name": "Sugar", "location": "X: 355.0\nY: 742.0\nZ: 90.5\n", "height": "181.0"}, {"id": "118", "name": "Chair", "location": "X: -300.0\nY: 249.99998474121094\nZ: 36.0\n", "height": "72.0"}, {"id": "119", "name": "TuoPan", "location": "X: 179.0\nY: 470.0\nZ: 91.0\n", "height": "182.0"}, {"id": "120", "name": "WaterCup", "location": "X: 149.99998474121094\nY: 850.0\nZ: 92.0\n", "height": "184.0"}, {"id": "121", "name": "Cake", "location": "X: 170.99998474121094\nY: 857.0\nZ: 117.0\n", "height": "234.0"}, {"id": "122", "name": "Spoon", "location": "X: 134.0\nY: 443.0\nZ: 90.0\n", "height": "180.0"}, {"id": "123", "name": "Bread", "location": "X: 164.0\nY: 637.0\nZ: 52.0\n", "height": "104.0"}, {"id": "124", "name": "Desk", "location": "X: -30.000015258789062\nY: 1080.0\nZ: 20.0\n", "height": "40.0"}, {"id": "125", "name": "Door", "location": "X: 242.00003051757812\nY: -548.5\nZ: 110.5\n", "height": "221.0"}, {"id": "126", "name": "XiGuan", "location": "X: 146.0\nY: 443.0\nZ: 90.69999694824219\n", "height": "181.39999389648438"}, {"id": "127", "name": "Sugar", "location": "X: 359.0\nY: 741.0\nZ: 90.5\n", "height": "181.0"}, {"id": "128", "name": "Chair", "location": "X: -151.39605712890625\nY: 576.6644897460938\nZ: 40.0\n", "height": "80.0"}, {"id": "129", "name": "TuoPan", "location": "X: 179.0\nY: 470.0\nZ: 89.0\n", "height": "178.0"}, {"id": "130", "name": "Cake", "location": "X: 156.99998474121094\nY: 852.0\nZ: 139.0\n", "height": "278.0"}, {"id": "131", "name": "Spoon", "location": "X: 133.5\nY: 443.0\nZ: 90.0\n", "height": "180.0"}, {"id": "132", "name": "Bread", "location": "X: 177.0\nY: 733.4129028320312\nZ: 81.0\n", "height": "162.0"}, {"id": "133", "name": "Desk", "location": "X: -380.0\nY: 270.0\nZ: 20.0\n", "height": "40.0"}, {"id": "134", "name": "Door", "location": "X: 244.00003051757812\nY: -548.5\nZ: 110.5\n", "height": "221.0"}, {"id": "135", "name": "Sugar", "location": "X: 123.0\nY: 435.0\nZ: 90.5\n", "height": "181.0"}, {"id": "136", "name": "Chair", "location": "X: 3.999983072280884\nY: 1169.0\nZ: 34.0\n", "height": "68.0"}, {"id": "137", "name": "TuoPan", "location": "X: 179.0\nY: 470.0\nZ: 87.0\n", "height": "174.0"}, {"id": "138", "name": "Cake", "location": "X: 157.99998474121094\nY: 852.0\nZ: 117.0\n", "height": "234.0"}, {"id": "139", "name": "Bread", "location": "X: 154.0\nY: 647.0\nZ: 51.0\n", "height": "102.0"}, {"id": "140", "name": "Desk", "location": "X: 210.00001525878906\nY: -350.0\nZ: 48.0\n", "height": "96.0"}, {"id": "141", "name": "Door", "location": "X: 302.2499694824219\nY: 1302.550048828125\nZ: 112.5\n", "height": "225.0"}, {"id": "142", "name": "Sugar", "location": "X: 358.0\nY: 738.0\nZ: 93.0\n", "height": "186.0"}, {"id": "143", "name": "Clip", "location": "X: 198.8828125\nY: 558.66650390625\nZ: 85.95439910888672\n", "height": "171.90879821777344"}, {"id": "144", "name": "Mug", "location": "X: 377.0\nY: 520.0\nZ: 161.0\n", "height": "322.0"}, {"id": "145", "name": "CoffeeMachine", "location": "X: 360.0\nY: 540.0\nZ: 84.0\n", "height": "168.0"}, {"id": "146", "name": "Chair", "location": "X: -159.99998474121094\nY: 159.99998474121094\nZ: 40.0\n", "height": "80.0"}, {"id": "147", "name": "DrinkMachine", "location": "X: 345.0\nY: 42.000003814697266\nZ: 131.0\n", "height": "262.0"}, {"id": "148", "name": "TuoPan", "location": "X: 174.00001525878906\nY: 170.0\nZ: 89.0\n", "height": "178.0"}, {"id": "149", "name": "Cake", "location": "X: 167.99998474121094\nY: 850.0\nZ: 139.0\n", "height": "278.0"}, {"id": "150", "name": "CoffeeBag", "location": "X: 450.0\nY: 808.0\nZ: 169.0\n", "height": "338.0"}, {"id": "151", "name": "Bread", "location": "X: 176.0\nY: 728.0\nZ: 110.0\n", "height": "220.0"}, {"id": "152", "name": "Desk", "location": "X: -2.0\nY: -5.99999930273043e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "153", "name": "Door", "location": "X: 116.5000228881836\nY: -351.0\nZ: 112.5\n", "height": "225.0"}, {"id": "154", "name": "Sugar", "location": "X: 356.0\nY: 741.0\nZ: 93.0\n", "height": "186.0"}, {"id": "155", "name": "Mug", "location": "X: 377.0\nY: 413.0\nZ: 170.0\n", "height": "340.0"}, {"id": "156", "name": "CoffeeMachine", "location": "X: 163.37042236328125\nY: 320.0\nZ: 97.0\n", "height": "194.0"}, {"id": "157", "name": "Chair", "location": "X: -189.00001525878906\nY: 821.0\nZ: 34.0\n", "height": "68.0"}, {"id": "158", "name": "TuoPan", "location": "X: 179.0\nY: 470.0\nZ: 85.0\n", "height": "170.0"}, {"id": "159", "name": "Cake", "location": "X: 159.99998474121094\nY: 810.0\nZ: 111.0\n", "height": "222.0"}, {"id": "160", "name": "CoffeeBag", "location": "X: 425.0\nY: 787.0\nZ: 169.0\n", "height": "338.0"}, {"id": "161", "name": "Bread", "location": "X: 155.0\nY: 734.0\nZ: 80.0\n", "height": "160.0"}, {"id": "162", "name": "Desk", "location": "X: 1.2000000424450263e-05\nY: -6.000000212225132e-06\nZ: 144.0\n", "height": "288.0"}, {"id": "163", "name": "Sugar", "location": "X: 358.0\nY: 738.0\nZ: 90.5\n", "height": "181.0"}, {"id": "164", "name": "KongTiao", "location": "X: 300.5\nY: -140.0\nZ: 114.0\n", "height": "228.0"}, {"id": "165", "name": "Glass", "location": "X: 129.0\nY: 257.0\nZ: 144.5\n", "height": "289.0"}, {"id": "166", "name": "Mug", "location": "X: 377.0\nY: 117.0\nZ: 170.0\n", "height": "340.0"}, {"id": "167", "name": "CoffeeMachine", "location": "X: 350.0\nY: 460.0\nZ: 84.0\n", "height": "168.0"}, {"id": "168", "name": "Chair", "location": "X: -160.0\nY: 249.99998474121094\nZ: 40.0\n", "height": "80.0"}, {"id": "169", "name": "TuoPan", "location": "X: 334.0\nY: 670.0\nZ: 85.0\n", "height": "170.0"}, {"id": "170", "name": "Cake", "location": "X: 163.99998474121094\nY: 865.0\nZ: 116.0\n", "height": "232.0"}, {"id": "171", "name": "CoffeeBag", "location": "X: 445.0\nY: 804.0\nZ: 169.0\n", "height": "338.0"}, {"id": "172", "name": "Bread", "location": "X: 180.0\nY: 735.0\nZ: 53.0\n", "height": "106.0"}, {"id": "173", "name": "Desk", "location": "X: -180.00001525878906\nY: 900.0\nZ: 20.0\n", "height": "40.0"}, {"id": "174", "name": "Sugar", "location": "X: 360.0\nY: 738.0\nZ: 90.5\n", "height": "181.0"}, {"id": "175", "name": "Glass", "location": "X: -133.5\nY: 466.0\nZ: 89.0\n", "height": "178.0"}, {"id": "176", "name": "Mug", "location": "X: 377.0\nY: 220.0\nZ: 161.0\n", "height": "322.0"}, {"id": "177", "name": "CoffeeMachine", "location": "X: 332.0\nY: 580.0\nZ: 89.0\n", "height": "178.0"}, {"id": "178", "name": "Chair", "location": "X: -420.0\nY: 399.9999694824219\nZ: 34.0\n", "height": "68.0"}, {"id": "179", "name": "TuoPan", "location": "X: 334.0\nY: 740.0\nZ: 85.0\n", "height": "170.0"}, {"id": "180", "name": "Cake", "location": "X: 164.99998474121094\nY: 864.0\nZ: 139.0\n", "height": "278.0"}, {"id": "181", "name": "CoffeeBag", "location": "X: 435.0\nY: 795.0\nZ: 169.0\n", "height": "338.0"}, {"id": "182", "name": "Bread", "location": "X: 184.0\nY: 660.0\nZ: 82.0\n", "height": "164.0"}, {"id": "183", "name": "Sugar", "location": "X: 164.0\nY: 274.0\nZ: 104.5\n", "height": "209.0"}, {"id": "184", "name": "Glass", "location": "X: 140.0\nY: 460.0\nZ: 84.0\n", "height": "168.0"}, {"id": "185", "name": "Mug", "location": "X: 377.0\nY: 506.0\nZ: 161.0\n", "height": "322.0"}, {"id": "186", "name": "CoffeeMachine", "location": "X: 143.0\nY: 373.0\nZ: 97.0\n", "height": "194.0"}, {"id": "187", "name": "Chair", "location": "X: -300.0\nY: 320.0\nZ: 36.0\n", "height": "72.0"}, {"id": "188", "name": "Container", "location": "X: 370.0\nY: 360.0\nZ: 84.0\n", "height": "168.0"}, {"id": "189", "name": "Cake", "location": "X: 161.99998474121094\nY: 849.0\nZ: 139.0\n", "height": "278.0"}, {"id": "190", "name": "CoffeeBag", "location": "X: 421.0\nY: 782.0\nZ: 169.0\n", "height": "338.0"}, {"id": "191", "name": "Bread", "location": "X: 145.0\nY: 652.0\nZ: 79.0\n", "height": "158.0"}, {"id": "192", "name": "Sugar", "location": "X: 133.0\nY: 434.0\nZ: 90.5\n", "height": "181.0"}, {"id": "193", "name": "Glass", "location": "X: 324.0\nY: 56.000003814697266\nZ: 89.0\n", "height": "178.0"}, {"id": "194", "name": "Mug", "location": "X: 377.0\nY: 513.0\nZ: 170.0\n", "height": "340.0"}, {"id": "195", "name": "CoffeeMachine", "location": "X: 350.0\nY: 580.0\nZ: 84.0\n", "height": "168.0"}, {"id": "196", "name": "Chair", "location": "X: -197.7161865234375\nY: 430.29150390625\nZ: 40.0\n", "height": "80.0"}, {"id": "197", "name": "Container", "location": "X: 370.0\nY: 390.0\nZ: 84.0\n", "height": "168.0"}, {"id": "198", "name": "Cake", "location": "X: 169.99998474121094\nY: 861.0\nZ: 139.0\n", "height": "278.0"}, {"id": "199", "name": "CoffeeBag", "location": "X: 462.0\nY: 818.0\nZ: 169.0\n", "height": "338.0"}, {"id": "200", "name": "Bread", "location": "X: 142.0\nY: 671.0\nZ: 50.0\n", "height": "100.0"}, {"id": "201", "name": "Sugar", "location": "X: 162.0\nY: 275.0\nZ: 104.0\n", "height": "208.0"}, {"id": "202", "name": "Glass", "location": "X: 129.0\nY: 247.0\nZ: 144.5\n", "height": "289.0"}, {"id": "203", "name": "Mug", "location": "X: 377.0\nY: 110.0\nZ: 161.0\n", "height": "322.0"}, {"id": "204", "name": "CoffeeMachine", "location": "X: 150.0\nY: 250.0\nZ: 123.0\n", "height": "246.0"}, {"id": "205", "name": "Chair", "location": "X: 229.00001525878906\nY: -315.0\nZ: 40.0\n", "height": "80.0"}, {"id": "206", "name": "Container", "location": "X: 370.0\nY: 370.0\nZ: 84.0\n", "height": "168.0"}, {"id": "207", "name": "CoffeeBag", "location": "X: 468.0\nY: 822.0\nZ: 169.0\n", "height": "338.0"}, {"id": "208", "name": "Bread", "location": "X: 140.0\nY: 668.0\nZ: 106.0\n", "height": "212.0"}, {"id": "209", "name": "Sugar", "location": "X: 121.0\nY: 435.0\nZ: 90.0\n", "height": "180.0"}, {"id": "210", "name": "Glass", "location": "X: 360.0\nY: 320.0\nZ: 84.0\n", "height": "168.0"}, {"id": "211", "name": "Mug", "location": "X: 377.0\nY: 611.0\nZ: 170.0\n", "height": "340.0"}, {"id": "212", "name": "CoffeeMachine", "location": "X: 349.0\nY: 620.0\nZ: 84.0\n", "height": "168.0"}, {"id": "213", "name": "Chair", "location": "X: -350.0\nY: 490.0\nZ: 34.0\n", "height": "68.0"}, {"id": "214", "name": "Container", "location": "X: 370.0\nY: 380.0\nZ: 84.0\n", "height": "168.0"}, {"id": "215", "name": "CoffeeBag", "location": "X: 456.0\nY: 813.0\nZ: 169.0\n", "height": "338.0"}, {"id": "216", "name": "Bread", "location": "X: 173.0\nY: 647.0\nZ: 53.0\n", "height": "106.0"}, {"id": "217", "name": "Sugar", "location": "X: 356.0\nY: 740.0\nZ: 90.5\n", "height": "181.0"}, {"id": "218", "name": "Mug", "location": "X: 377.0\nY: 320.0\nZ: 161.0\n", "height": "322.0"}, {"id": "219", "name": "CoffeeMachine", "location": "X: 360.0\nY: 190.0\nZ: 114.0\n", "height": "228.0"}, {"id": "220", "name": "Chair", "location": "X: 158.99998474121094\nY: 1320.0\nZ: 34.0\n", "height": "68.0"}, {"id": "221", "name": "Container", "location": "X: 370.0\nY: 370.0\nZ: 104.0\n", "height": "208.0"}, {"id": "222", "name": "CoffeeBag", "location": "X: 430.0\nY: 791.0\nZ: 169.0\n", "height": "338.0"}, {"id": "223", "name": "Bread", "location": "X: 157.0\nY: 635.0\nZ: 79.0\n", "height": "158.0"}, {"id": "224", "name": "Sugar", "location": "X: 359.0\nY: 744.0\nZ: 90.5\n", "height": "181.0"}, {"id": "225", "name": "Mug", "location": "X: 377.0\nY: 206.0\nZ: 161.0\n", "height": "322.0"}, {"id": "226", "name": "CoffeeMachine", "location": "X: 350.0\nY: 510.0\nZ: 84.0\n", "height": "168.0"}, {"id": "227", "name": "Chair", "location": "X: -290.0\nY: 169.99998474121094\nZ: 36.0\n", "height": "72.0"}, {"id": "228", "name": "CoffeeBag", "location": "X: 440.0\nY: 799.0\nZ: 169.0\n", "height": "338.0"}, {"id": "229", "name": "Bread", "location": "X: 140.0\nY: 688.0\nZ: 106.0\n", "height": "212.0"}, {"id": "230", "name": "Mug", "location": "X: 377.0\nY: 213.0\nZ: 170.0\n", "height": "340.0"}, {"id": "231", "name": "CoffeeMachine", "location": "X: 360.0\nY: 214.0\nZ: 114.0\n", "height": "228.0"}, {"id": "232", "name": "Chair", "location": "X: 91.99998474121094\nY: 1182.0\nZ: 34.0\n", "height": "68.0"}, {"id": "233", "name": "Bread", "location": "X: 160.0\nY: 712.0\nZ: 52.0\n", "height": "104.0"}, {"id": "234", "name": "Mug", "location": "X: 377.0\nY: 313.0\nZ: 170.0\n", "height": "340.0"}, {"id": "235", "name": "CoffeeMachine", "location": "X: 350.0\nY: 154.0\nZ: 84.0\n", "height": "168.0"}, {"id": "236", "name": "Chair", "location": "X: -73.00001525878906\nY: 997.0\nZ: 34.0\n", "height": "68.0"}, {"id": "237", "name": "Bread", "location": "X: 181.0\nY: 710.0\nZ: 81.19999694824219\n", "height": "162.39999389648438"}, {"id": "238", "name": "Mug", "location": "X: 377.0\nY: 420.0\nZ: 161.0\n", "height": "322.0"}, {"id": "239", "name": "Chair", "location": "X: 390.0000305175781\nY: -400.0\nZ: 40.0\n", "height": "80.0"}, {"id": "240", "name": "Bread", "location": "X: 135.0\nY: 734.0\nZ: 78.0\n", "height": "156.0"}, {"id": "241", "name": "Mug", "location": "X: 377.0\nY: 306.0\nZ: 161.0\n", "height": "322.0"}, {"id": "242", "name": "Chair", "location": "X: -190.0\nY: 510.0\nZ: 40.0\n", "height": "80.0"}, {"id": "243", "name": "Bread", "location": "X: 137.0\nY: 635.0\nZ: 78.0\n", "height": "156.0"}, {"id": "244", "name": "Mug", "location": "X: 377.0\nY: 604.0\nZ: 161.0\n", "height": "322.0"}, {"id": "245", "name": "Chair", "location": "X: -41.83454513549805\nY: -68.69022369384766\nZ: 40.0\n", "height": "80.0"}, {"id": "246", "name": "Bread", "location": "X: 160.0\nY: 670.0\nZ: 108.0\n", "height": "216.0"}, {"id": "247", "name": "Mug", "location": "X: 377.0\nY: 617.0\nZ: 161.0\n", "height": "322.0"}, {"id": "248", "name": "Chair", "location": "X: -400.0\nY: 449.9999694824219\nZ: 34.0\n", "height": "68.0"}, {"id": "249", "name": "Bread", "location": "X: 145.0\nY: 660.0\nZ: 79.0\n", "height": "158.0"}, {"id": "250", "name": "Mug", "location": "X: 377.0\nY: 123.0\nZ: 161.0\n", "height": "322.0"}, {"id": "251", "name": "Chair", "location": "X: -113.00001525878906\nY: 1029.0\nZ: 34.0\n", "height": "68.0"}, {"id": "252", "name": "BaoJing", "location": "X: 360.8000183105469\nY: -199.0\nZ: 115.0\n", "height": "230.0"}, {"id": "253", "name": "Bread", "location": "X: 176.0\nY: 708.0\nZ: 110.0\n", "height": "220.0"}, {"id": "254", "name": "Mug", "location": "X: 377.0\nY: 406.0\nZ: 161.0\n", "height": "322.0"}, {"id": "255", "name": "Chair", "location": "X: -116.00000762939453\nY: 947.0\nZ: 34.0\n", "height": "68.0"}, {"id": "256", "name": "BaoJing", "location": "X: 362.10003662109375\nY: -197.0\nZ: 115.0\n", "height": "230.0"}, {"id": "257", "name": "Bread", "location": "X: 140.0\nY: 735.0\nZ: 49.0\n", "height": "98.0"}, {"id": "258", "name": "Chair", "location": "X: -300.0\nY: 660.0\nZ: 34.0\n", "height": "68.0"}, {"id": "259", "name": "Bread", "location": "X: 175.0\nY: 688.0\nZ: 109.0\n", "height": "218.0"}, {"id": "260", "name": "Chair", "location": "X: -156.00001525878906\nY: 977.0\nZ: 34.0\n", "height": "68.0"}, {"id": "261", "name": "Bread", "location": "X: 181.0\nY: 700.0\nZ: 81.19999694824219\n", "height": "162.39999389648438"}, {"id": "262", "name": "Chair", "location": "X: -249.99998474121094\nY: 9.999987602233887\nZ: 36.0\n", "height": "72.0"}, {"id": "263", "name": "Bread", "location": "X: 140.0\nY: 712.0\nZ: 50.0\n", "height": "100.0"}, {"id": "264", "name": "Chair", "location": "X: -350.0\nY: 560.0\nZ: 34.0\n", "height": "68.0"}, {"id": "265", "name": "Bread", "location": "X: 141.0\nY: 638.0\nZ: 107.0\n", "height": "214.0"}, {"id": "266", "name": "Chair", "location": "X: -80.0\nY: 60.0\nZ: 40.0\n", "height": "80.0"}, {"id": "267", "name": "Bread", "location": "X: 169.0\nY: 700.0\nZ: 81.19999694824219\n", "height": "162.39999389648438"}, {"id": "268", "name": "Chair", "location": "X: 400.0000305175781\nY: -300.0\nZ: 40.0\n", "height": "80.0"}, {"id": "269", "name": "Bread", "location": "X: 161.0\nY: 648.0\nZ: 108.0\n", "height": "216.0"}, {"id": "270", "name": "Chair", "location": "X: -280.0\nY: 99.99998474121094\nZ: 36.0\n", "height": "72.0"}, {"id": "271", "name": "Bread", "location": "X: 157.0\nY: 710.0\nZ: 108.0\n", "height": "216.0"}, {"id": "272", "name": "Chair", "location": "X: 350.0000305175781\nY: -350.0\nZ: 40.0\n", "height": "80.0"}, {"id": "273", "name": "Bread", "location": "X: 178.0\nY: 671.0\nZ: 54.0\n", "height": "108.0"}, {"id": "274", "name": "Chair", "location": "X: -80.0\nY: 230.0\nZ: 40.0\n", "height": "80.0"}, {"id": "275", "name": "Bread", "location": "X: 135.0\nY: 660.0\nZ: 78.0\n", "height": "156.0"}, {"id": "276", "name": "Chair", "location": "X: 193.99998474121094\nY: 1283.0\nZ: 34.0\n", "height": "68.0"}, {"id": "277", "name": "Bread", "location": "X: 175.0\nY: 668.0\nZ: 109.0\n", "height": "218.0"}] \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 1cc7227..db257f3 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -353,10 +353,8 @@ class Scene: # Since status.walkers is a list, some walkerIDs would change after removing a walker. remove_list.append(walkerID) - index_shift_list = [ 0 for _ in range(len(self.state["customer_mem"])) ] - stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID)) - + self.state["customer_mem"] = {} w = self.status.walkers for i in range(len(w)): self.state["customer_mem"][w[i].name] = i @@ -365,13 +363,23 @@ class Scene: s = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) scene = stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=IDs, scene=self.sceneID)) time.sleep(2) + self.state["customer_mem"] = {} + w = self.status.walkers + for i in range(len(w)): + self.state["customer_mem"][w[i].name] = i return - def clean_walker(self): - stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) + def clean_walkers(self): + scene = stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) + self.state["customer_mem"]={} + return scene def control_walker(self, walkerID,autowalk,speed,X,Y,Yaw=0): + + if not isinstance(walkerID, int): + walkerID = self.walker_index2mem(walkerID) + pose = GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw) scene = stub.ControlWalkers( GrabSim_pb2.WalkerControls(controls=[GrabSim_pb2.WalkerControls.WControl(id=walkerID, autowalk=autowalk, speed=speed, pose=pose)], scene=self.sceneID) @@ -389,6 +397,10 @@ class Scene: for control in control_list_ls: if control[-1]!= None: walkerID = control[0] + + if not isinstance(walkerID, int): + walkerID = self.walker_index2mem(walkerID) + # cont = self.status.walkers[walkerID].name + ":"+control[-1] # self.control_robot_action(control[walkerID], 3, cont) self.customer_say(walkerID,control[-1]) @@ -511,6 +523,9 @@ class Scene: if isinstance(name,int): name = self.walker_index2mem(name) + # if not isinstance(walkerID, int): + # name = self.walker_index2mem(walkerID) + print(f'{name} say: {sentence}') if self.show_bubble and show_bubble: self.walker_bubble(name,sentence) @@ -752,7 +767,7 @@ class Scene: scene = stub.Do(action) print(scene.info) - def navigation_move(self, cur_objs, objs_name_set, cur_obstacle_world_points, v_list, map_ratio, scene_id=0, map_id=11): + def navigation_move(self, cur_objs, objs_name_set, cur_obstacle_world_points, v_list, map_ratio, db, scene_id=0, map_id=11): print('------------------navigation_move----------------------') scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id)) walk_value = [scene.location.X, scene.location.Y] @@ -765,10 +780,12 @@ class Scene: print("walk_v", walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) - cur_obstacle_world_points = camera.get_obstacle_point(scene, cur_obstacle_world_points,map_ratio) - cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, objs_name_set) + + cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points,map_ratio) + + # if scene.info == "Unreachable": print(scene.info) @@ -785,10 +802,12 @@ class Scene: action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) - cur_obstacle_world_points = camera.get_obstacle_point(scene, cur_obstacle_world_points, map_ratio) - cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, objs_name_set) + + cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio) + + # if scene.info == "Unreachable": print(scene.info) return cur_objs, objs_name_set, cur_obstacle_world_points @@ -830,7 +849,7 @@ class Scene: if len(self.all_frontier_list) == 0: free_list = list(self.visited) free_array = np.array(free_list) - print(f"主动探索完成！以下是场景中可以到达的点：{free_array}；其余点均是障碍物不可达") + print(f"主动探索完成！保存了二维地图与环境中重点物品语义信息！") # # 画地图: X行Y列，第一行在下面 # plt.clf() diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index fab39ad..5dc7603 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/robowaiter/scene/tasks/AEM.py @@ -2,12 +2,16 @@ 环境主动探索和记忆 要求输出探索结果（语义地图）对环境重点信息记忆。生成环境的语义拓扑地图，和不少于10个环境物品的识别和位置记忆，可以是图片或者文字或者格式化数据。 """ +import json import math +import time + import matplotlib as mpl import pickle import numpy as np from matplotlib import pyplot as plt +from sklearn.cluster import DBSCAN from robowaiter.scene.scene import Scene class SceneAEM(Scene): @@ -22,8 +26,10 @@ class SceneAEM(Scene): cur_obstacle_world_points = [] objs_name_set = set() visited_obstacle = set() + obj_json_data = [] + db = DBSCAN(eps=4, min_samples=2) - map_ratio = 5 + map_ratio = 3 # # 创建一个颜色映射，其中0表示黑色，1表示白色 # cmap = plt.cm.get_cmap('gray') # cmap.set_under('black') @@ -44,11 +50,12 @@ class SceneAEM(Scene): # navigation_move([[237,490]], i, map_id) # navigation_test(i,map_id) map_map = np.zeros((math.ceil(950 / map_ratio), math.ceil(1850 / map_ratio))) + while True: goal = self.explore(map, 120) # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取 if goal is None: break - cur_objs, objs_name_set, cur_obstacle_world_points= self.navigation_move(cur_objs, objs_name_set, cur_obstacle_world_points, [[goal[0], goal[1]]], map_ratio, 0, 11) + cur_objs, objs_name_set, cur_obstacle_world_points= self.navigation_move(cur_objs, objs_name_set, cur_obstacle_world_points, [[goal[0], goal[1]]], map_ratio, db,0, 11) for point in cur_obstacle_world_points: if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450: @@ -66,8 +73,15 @@ class SceneAEM(Scene): # plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower') # plt.axis('off') plt.show() + time.sleep(1) print("------------物品信息--------------") print(cur_objs) + + for i in range(len(cur_objs)): + obj_json_data.append({"id":f"{i}", "name":f"{cur_objs[i].name}", "location":f"{cur_objs[i].location}", "height":f"{cur_objs[i].location.Z * 2}"}) + + with open('../../proto/objs.json', 'w') as file: + json.dump(obj_json_data, file) # for i in range(-350, 600): # for j in range(-400, 1450): # i = (math.floor((i + 350) / map_ratio)) diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py b/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py index c85e1ac..9eb0349 100644 --- a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py +++ b/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py @@ -25,65 +25,61 @@ class SceneVLM(Scene): self.signal_event_list = [ # 场景1：带小女孩找阳光下的空位 - (3, self.add_walker, (5, 230, 1200)), - (1, self.control_walker, (0, False, 200, 60, 520, 0)), - (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")), - (-1, self.customer_say, (0,"可以带我过去嘛？")), - (0, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦 + # (3, self.add_walker, (5, 230, 1200)), # 0号"Girl02_C_3" + # (1, self.control_walker, (0, False, 200, 60, 520, 0)), + # (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")), + # (-1, self.customer_say, (0,"可以带我过去嘛？")), + # (0, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦 - # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子 + + + # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子 # (3, self.add_walker, (5, 230, 1200)), # 小女孩 - # 上述准备 - # (self.st2, self.add_walker, (26, -28, -150, 90)), - # (self.st2, self.add_walker, (10, -70, -200, -45)), - # (self.st2, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")), - # (self.st2+10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), # 20 胖胖男到了 BrightTable6 - # (self.st2+15, self.customer_say, (1, "咖啡有哪些呢？")),# 10 - # (self.st2+15+5, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15 - - # 场景3：有位女士要杯水和冰红茶 - (3, self.add_walker, (5, 230, 1200)), - (0, self.add_walker, (26, -30, -200, -90)), - (0, self.add_walker, (10, -80, -180, -45)), - # # 上述准备 - # (0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135), - # (0, self.add_walker, (32, -80, 850, 135)), # 女 'BrightTable3': (-80, 850, 135), - # (0, self.add_walker, (1, 60, 420, 135)), # 生成小男孩随机游走 - (0,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])), - (0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走 - # (0, self.add_walker, (48, 60, 520,0)),# 生成他妈妈 - # (0, self.add_walker, (31, 60, 600, -90)), # 女红色排队 7号 - # (0, self.add_walker, (20, 60, 680, -90)), # 大胖男色排队 8号 - # (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑排队 9号 - # (0, self.add_walker, (29, -290, 400, 180)), # 青色女人占了位置 BrightTable5 - (0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])), - - (0, self.customer_say, (6, "哎呦，今天这么多人，还有空位吗？")),# 女士问 - (10, self.customer_say, (6, "我带着孩子呢，想要宽敞亮堂的地方。")), # 女士问 - # 好的，我明白了，那么您可以选择我们的家庭亲子座，这样可以容纳您的孩子，并且更加宽敞舒适。 - # 这里可以加一下自主导航和探索，找到一个位置 - # 好的，我明白了，那么我们推荐您到大厅的桌子，那里的空间比较宽敞，环境也比较明亮，适合带着孩子一起用餐。 + # # # 上述准备 + # (10, self.add_walker, (26, -28, -150, 90)), + # (0, self.add_walker, (10, -70, -200, -45)), + # (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")), + # (8, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), # 20 胖胖男到了 BrightTable6 + # (2, self.customer_say, (1, "咖啡有哪些呢？")),# 10 + # (2, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15 + # (2, self.customer_say, (1, "来杯卡布奇诺吧。")), - (8, self.customer_say, (6, "大厅的桌子好啊，快带我去呀！")), - (10, self.control_walker, (6, False, 50,-250, 480, 0)), # #290, 400 - # (5, self.customer_say, (6, "我想来杯水，帮我孩子拿个酸奶吧。")), - (-1, self.customer_say, (6, "谢谢！")), - - # 9号灰色男 排队排着排着，不排了 - (0, self.control_walker, (9, False, 100, 100, 760, 180)), - (0, self.control_walker, (9, True, 100, 0, 0, 180)), - # ### 增加场景，孩子说热要开空调 或者9号随机游走和说 + # # 场景3：有位女士要杯水和冰红茶 + # (3, self.add_walker, (5, 230, 1200)), + # (0, self.add_walker, (26, -30, -200, -90)), + # (0, self.add_walker, (10, -80, -180, -45)), + # # # # 上述准备 + # # (0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135), + # # (0, self.add_walker, (32, -80, 850, 135)), # 女 'BrightTable3': (-80, 850, 135), + # # (0, self.add_walker, (1, 60, 420, 135)), # 生成小男孩随机游走 + # (0,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])), + # (0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走 + # # (0, self.add_walker, (48, 60, 520,0)),# 生成他妈妈 + # # (0, self.add_walker, (31, 60, 600, -90)), # 女红色排队 7号 + # # (0, self.add_walker, (20, 60, 680, -90)), # 大胖男色排队 8号 + # # (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑排队 9号 + # # (0, self.add_walker, (29, -290, 400, 180)), # 青色女人占了位置 BrightTable5 + # (0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])), + # + # # (5, self.customer_say, (6, "哎呦，今天这么多人，还有空位吗？")),# 女士问 + # # (15, self.customer_say, (6, "我带着孩子呢，想要宽敞亮堂的地方。")), # 女士问 + # # 好的，我明白了，那么您可以选择我们的家庭亲子座，这样可以容纳您的孩子，并且更加宽敞舒适。 + # # 这里可以加一下自主导航和探索，找到一个位置 + # # 好的，我明白了，那么我们推荐您到大厅的桌子，那里的空间比较宽敞，环境也比较明亮，适合带着孩子一起用餐。 + # (8, self.customer_say, (6, "大厅的桌子好啊，快带我去呀！")), + # (15, self.control_walker, (6, False, 50,-250, 480, 0)), # #290, 400 + # # (3, self.customer_say, (6, "我想来杯水，帮我孩子拿个酸奶吧。")), + # # ### 9号灰色男 排队排着排着，不排了 # (0, self.control_walker, (9, False, 100, 100, 760, 180)), - # (0, self.control_walker, (9, False, 100, 0, 0, 180)), - # (0, self.control_walkers_and_say, ([[[9, True, 100, 0, 0, 180, "好热呀！太阳也好大！"]]])), - # # (9, True, 100, 0, 0, 180)), - # # (0, self.customer_say, (1, "咖啡有哪些呢？")), - # ### 增加场景，孩子说热要开空调 或者9号随机游走和说 + # (0, self.control_walker, (9, True, 100, 0, 0, 180)), + # # # ### 增加场景，孩子说热要开空调 或者9号随机游走和说 + # # (90, self.customer_say, (6, "谢谢！")), #倒水+取放酸奶 90s + # (3, self.customer_say, (6, "谢谢！")), - # 场景4：三人排队点单，女士要保温杯 + # # # 场景4：三人排队点单，女士要保温杯 # (3, self.add_walker, (5, 230, 1200)), # (0, self.add_walker, (26, -30, -200, -90)), # (0, self.add_walker, (10, -80, -180, -45)), @@ -95,46 +91,40 @@ class SceneVLM(Scene): # (0, self.add_walker, (20, 60, 680, -90)), # 大胖男排队 # (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑色排队 # (0, self.add_walker, (29, -290, 400, 180)), # # 青色女人占了位置 BrightTable5 - # # # # 上述准备 - (0, self.control_walkers_and_say, ([[[7, False, 100, 60, 520, 0, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])), - (5, self.customer_say, (7,"你可以帮我拿来吗，我在前门的桌子前等你。")), - (1, self.control_walker,(7, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步 - (1, self.control_walker, (7, False, 80, 240, 1000, -45)), # 红女士走到Table1前 - (1, self.control_walker, (8, False, 100, 60, 600, -90)), # 大胖男排队往前走一步 - (2, self.control_walker, (9, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步 - (6, self.customer_say, (7,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前 + # # # # # # 上述准备 + # # (5, self.control_walkers_and_say, ([[[7, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])), #13 + # (0, self.control_walkers_and_say, ([[[7, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])), + # (5, self.customer_say, (7,"你可以帮我拿来吗，我在前门的桌子前等你。")), + # (1, self.control_walker,(7, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步 + # (1, self.control_walker, (7, False, 80, 240, 1000, -45)), # 红女士走到Table1前 + # (1, self.control_walker, (8, False, 100, 60, 600, -90)), # 大胖男排队往前走一步 + # (2, self.control_walker, (9, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步 + # (6, self.customer_say, (7,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前 + # (5, self.customer_say, (7, "不用了")), # 红女士在Table1前 - # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯 - # (3, self.add_walker, (5, 230, 1200)), - # (0, self.add_walker, (26, -30, -200, -90)), - # (0, self.add_walker, (10, -80, -180, -45)), - # (0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135), - # (0, self.add_walker, (32, -80, 850, 135)), - # (0, self.add_walker, (1, 60, 220, 135)), - # (0, self.add_walker, (48, 60, 320, 0)), # 生成他妈妈 - # (0, self.add_walker, (31, 280, 1200, -45)), # # 女红色排队 7号找保温杯的顾客 - # (0, self.add_walker, (20, 60, 680, -90)), # 大胖男排队 - # (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑色排队 - # (0, self.add_walker, (29, -290, 400, 180)), # # 青色女人占了位置 BrightTable5 - - ## 9号灰色男 排队排着排着，不排了 - (0, self.control_walker, (9, False, 100, 100, 760, 180)), - (0, self.control_walker, (9, True, 100, 0, 0, 180)), - # 上述准备 - - (0, self.control_walker, (9, False, 100, 60, 760, -90)), - - - # 场景6：大胖男点了外卖，离开啦 - (2, self.control_walkers_and_say,([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), + # # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯 + (3, self.add_walker, (5, 230, 1200)), + (0, self.add_walker, (26, -30, -200, -90)), + (0, self.add_walker, (10, -80, -180, -45)), + (0, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135), + (0, self.add_walker, (32, -80, 850, 135)), + (0, self.add_walker, (1, 60, 220, 135)), + (0, self.add_walker, (48, 60, 320, 0)), # 生成他妈妈 + (0, self.add_walker, (31, 280, 1200, -45)), # # 女红色排队 7号找保温杯的顾客 + (0, self.add_walker, (20, 60, 680, -90)), # 大胖男排队 + (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑色排队 + (0, self.add_walker, (29, -290, 400, 180)), # # 青色女人占了位置 BrightTable5 + # # 上述准备 + # # 场景6：大胖男点了外卖，离开啦 + # (9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s (0, self.animation_reset,()), #重置任务 - (2, self.customer_say, (8, "打包吧，快点！")), - (2, self.control_walker, (9, False, 100, 60, 620, -90)), # 男灰黑色排队往前走一步 - (-1, self.customer_say, (8, "谢啦，我赶时间！")), - (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦 - (2, self.control_walker, (8, False, 250, 240, -150, -90)), + # (6, self.customer_say, (8, "打包吧，快点！")), + # (2, self.control_walker, (9, False, 100, 60, 620, -90)), # 男灰黑色排队往前走一步 + # # (100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适 + # (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦 + # (2, self.control_walker, (8, False, 250, 240, -150, -90)), (5, self.remove_walkers, ([[0,7,8]])), # (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 9号变7号 男灰黑色排队往前,轮到他 @@ -142,8 +132,15 @@ class SceneVLM(Scene): # 场景7：最后排队那个随机游走 9号变为6号，随机游走。 # 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低” # 开了空调 - (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 9号变7号 男灰黑色排队往前,轮到他 - (2, self.customer_say, (6, "好热呀！太阳也好大！")) + (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他 + # (2, self.customer_say, (6, "好热呀！太阳也好大！")), + # (10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])), + + + # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯 + (3, self.clean_walkers, ()), + (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦 + (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])), @@ -179,6 +176,11 @@ class SceneVLM(Scene): self.control_walkers_and_say([[0,False,150,end[0],end[1],90,"谢谢！"]]) self.scene_flag += 1 + # 获得所有顾客的名字 + # print("=================") + # for cus in self.status.walkers: + # print(cus) + # print("=================") pass if __name__ == '__main__':