测试导航算法

robowaiter/algos/navigate/DstarLite/navigate.py

@@ -59,10 +59,12 @@ class Navigator:
                 next_step = min(self.step_num, len(path))
                 (next_x, next_y) = path[next_step-1]
                 print('plan pos:', (next_x, next_y), end=' ')
-                self.scene.walk_to(next_x, next_y, yaw, velocity=self.v)
+                scene_info = self.scene.walk_to(next_x, next_y, yaw, velocity=self.v)
+                yaw = scene_info.rotation.Yaw
+
                 if animation:
                     self.planner.draw_graph(self.step_num)  # 画出搜索路径
-                time.sleep(self.step_time)
+                # time.sleep(self.step_time)
             pos = np.array((self.scene.status.location.X, self.scene.status.location.Y))
             print('reach pos:', pos)
 

robowaiter/behavior_lib/act/DealChat.py

@@ -39,7 +39,8 @@ class DealChat(Act):
         else:
             answer = ask_llm(chat)
             print(f"机器人回答：{answer}")
-            self.scene.chat_bubble(f"机器人回答：{answer}")
+            if self.scene.show_bubble:
+                self.scene.chat_bubble(f"机器人回答：{answer}")
 
         return ptree.common.Status.RUNNING
 

robowaiter/behavior_lib/act/MoveTo.py

@@ -2,14 +2,20 @@ import py_trees as ptree
 from typing import Any
 from robowaiter.behavior_lib._base.Act import Act
 from robowaiter.algos.navigate.DstarLite.navigate import Navigator
+
 class MoveTo(Act):
 
     def __init__(self, *args):
         super().__init__(*args)
 
     def _update(self) -> ptree.common.Status:
+        # self.scene.test_move()
+
         navigator = Navigator(scene=self.scene, area_range=[-350, 600, -400, 1450], map=self.scene.state["map"]["2d"])
         goal = self.scene.state['map']['obj_pos'][self.args[0]]
         navigator.navigate(goal, animation=False)
+
         self.scene.state['condition_set'].add('At(Robot,Table)')
+
+
         return ptree.common.Status.RUNNING

robowaiter/scene/scene.py

@@ -34,6 +34,7 @@ def image_extract(camera_data):
 class Scene:
     robot = None
     event_list = []
+    show_bubble = False
 
     default_state = {
         "map": {
@@ -60,7 +61,7 @@ class Scene:
 
     def __init__(self,robot=None, sceneID=0):
         self.sceneID = sceneID
-        self.use_offset = True
+        self.use_offset = False
         self.start_time = time.time()
         self.time = 0
         self.sub_task_seq = None
@@ -132,7 +133,8 @@ class Scene:
     def create_chat_event(self,sentence):
         def customer_say():
             print(f'顾客说：{sentence}')
-            self.chat_bubble(f'顾客说：{sentence}')
+            if self.show_bubble:
+                self.chat_bubble(f'顾客说：{sentence}')
             self.state['chat_list'].append(f'{sentence}')
 
         return customer_say
@@ -161,13 +163,17 @@ class Scene:
     def walk_to(self, X, Y, Yaw, velocity=150, dis_limit=100):
         if self.use_offset:
             X, Y = X + loc_offset[0], Y + loc_offset[1]
-        stub.Do(
-            GrabSim_pb2.Action(
-                scene=self.sceneID,
-                action=GrabSim_pb2.Action.ActionType.WalkTo,
-                values=[X, Y, Yaw, velocity, dis_limit],
-            )
+
+        v = [X, Y, Yaw - 90, velocity, dis_limit]
+        print(v)
+        action = GrabSim_pb2.Action(
+            scene=self.sceneID,
+            action=GrabSim_pb2.Action.ActionType.WalkTo,
+            values=v
         )
+        scene_info = stub.Do(action)
+        return scene_info
+
 
     def reachable_check(self, X, Y, Yaw):
         if self.use_offset:
@@ -374,4 +380,14 @@ class Scene:
             )
             scene = stub.Do(action)
 
+    def test_move(self):
+        v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
+        scene = self.status
+        for walk_v in v_list:
+            walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100]
+            print("walk_v", walk_v)
+            action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
+            scene = stub.Do(action)
+            print(scene.info)
+
 

run_robowaiter.py

@@ -1,7 +1,7 @@
 import os
 from robowaiter import Robot, task_map
 
-TASK_NAME = 'VLM'
+TASK_NAME = 'VLN'
 
 # create robot
 project_path = "./robowaiter"

sub_task.ptml

@@ -1,7 +1,7 @@
 selector{
-cond At(Coffee,Bar)
+cond At(Robot,Table)
 selector{
-cond At(Robot,CoffeeMachine)
-act MakeCoffee
+cond At(Robot,Bar)
+act MoveTo(Table)
 }
 }

zoo/demo/导航寻路.py

@@ -1,60 +1,139 @@
 #!/usr/bin/env python3
 # -*- encoding: utf-8 -*-
+# enconding = utf8
 import sys
 import time
 import grpc
 
+sys.path.append('./')
+sys.path.append('../')
+
 import matplotlib.pyplot as plt
 import numpy as np
 from mpl_toolkits.axes_grid1 import make_axes_locatable
 
-from proto import GrabSim_pb2
-from proto import GrabSim_pb2_grpc
+from robowaiter.proto import GrabSim_pb2_grpc,GrabSim_pb2
 
-channel = grpc.insecure_channel('localhost:30001',options=[
-            ('grpc.max_send_message_length', 1024*1024*1024),
-            ('grpc.max_receive_message_length', 1024*1024*1024)
-        ])
+
+channel = grpc.insecure_channel('localhost:30001', options=[
+    ('grpc.max_send_message_length', 1024 * 1024 * 1024),
+    ('grpc.max_receive_message_length', 1024 * 1024 * 1024)
+])
 
 sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
 
-def map_test(map_id=0, scene_num=1):
-    initworld = sim_client.Init(GrabSim_pb2.NUL())
-    print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
-    initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
+'''
+初始化，卸载已经加载的关卡，清除所有机器人
+'''
 
-def walk_test(scene_id=0, map_id=0):
-    """
-    移动底盘
-    GrabSim_pb2.Action(sceneID=0, action=GrabSim_pb2.Action.ActionType.WalkTo, values=[x, y, yaw, velocity, dis])
-    yaw: 机器人朝向; velocity:速度, -1代表瞬移; dis:最终达到的位置距离目标点最远距离, 如果超过此距离则目标位置不可达
-    """
+
+def Init():
+    sim_client.Init(GrabSim_pb2.NUL())
+
+
+'''
+获取当前可加载的地图信息(地图名字、地图尺寸)
+'''
+
+
+def AcquireAvailableMaps():
+    AvailableMaps = sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())
+    print(AvailableMaps)
+
+
+'''
+1、根据mapID加载指定地图
+2、如果scene_num>1,则根据地图尺寸偏移后加载多个相同地图
+3、这样就可以在一个关卡中训练多个地图
+'''
+
+
+def SetWorld(map_id=0, scene_num=1):
+    print('------------------SetWorld----------------------')
+    world = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
+
+
+'''
+返回场景的状态信息
+1、返回机器人的位置和旋转
+2、返回各个关节的名字和旋转
+3、返回场景中标记的物品信息(名字、类型、位置、旋转)
+4、返回场景中行人的信息(名字、位置、旋转、速度)
+5、返回机器人手指和双臂的碰撞信息
+'''
+
+
+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}")
 
+
+'''
+重置场景
+1、重置桌子的宽度和高度
+2、清除生成的行人和物品
+3、重置关节角度、位置旋转
+4、清除碰撞信息
+5、重置场景中标记的物品
+'''
+
+
+def Reset(scene_id=0):
+    print('------------------Reset----------------------')
+    scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id))
+    print(scene)
+
+    # 如果场景支持调整桌子
+    # sim_client.Reset(GrabSim_pb2.ResetParams(scene = scene_id, adjust = True, height = 100.0, width = 100.0))"
+
+
+"""
+导航移动 
+yaw:机器人朝向;
+velocity:速度,>0代表移动,<0代表瞬移,=0代表只查询;
+dis:最终达到的位置距离目标点最远距离,如果超过此距离则目标位置不可达
+"""
+
+
+def navigation_move(scene_id=0, map_id=0):
+    print('------------------navigation_move----------------------')
+    scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
     walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw]
-    print('------------------walk_test----------------------')
     print("position:", walk_value)
 
-    if map_id == 3:  # coffee
-        v_list = [[scene.location.X + 20, scene.location.Y - 500], [scene.location.X - 200, scene.location.Y - 300],
-                  [scene.location.X - 200, scene.location.Y + 20], [0, 880], [250, 1200], [-55, 750], [70, -200]]
+    if map_id == 11:  # coffee
+        v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
     else:
-        v_list = [[scene.location.X - 10, scene.location.Y - 20]]
+        v_list = [[0.0, 0.0]]
 
     for walk_v in v_list:
         walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100]
         print("walk_v", walk_v)
         action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
         scene = sim_client.Do(action)
-        print(scene.info)  # print navigation info
-        time.sleep(2)
+        print(scene.info)
+
 
 if __name__ == '__main__':
-    map_id = 3                      # 地图编号: 3: 咖啡厅
-    scene_num = 1                   # 场景数量
-    map_test(map_id, scene_num)     # 场景加载测试
-    time.sleep(5)
+    map_id = 11  # 地图编号
+    scene_num = 1  # 场景数量
+
+    print('------------ 初始化加载场景 ------------')
+    Init()
+    AcquireAvailableMaps()
+    SetWorld(map_id, scene_num)
+    time.sleep(5.0)
 
     for i in range(scene_num):
-        print("------------------", i, "----------------------")
-        walk_test(i, map_id)        # 导航寻路测试
+        print('------------ 场景操作 ------------')
+        Observe(i)
+        Reset(i)
+
+        print('------------ 导航移动 ------------')
+        navigation_move(i, map_id)