165 lines
7.1 KiB
Python
165 lines
7.1 KiB
Python
|
#!/usr/bin/env python3
|
|||
|
|
# -*- encoding: utf-8 -*-
|
|||
|
|
import sys
|
|||
|
|
import time
|
|||
|
|
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
import numpy as np
|
|||
|
|
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
|||
|
|
from scene_utils import control
|
|||
|
|
|
|||
|
|
from rrt import RRT
|
|||
|
|
from rrt_star import RRTStar
|
|||
|
|
from apf import APF
|
|||
|
|
|
|||
|
|
|
|||
|
|
class Navigator:
|
|||
|
|
'''
|
|||
|
|
导航类
|
|||
|
|
'''
|
|||
|
|
|
|||
|
|
def __init__(self, scene, area_range, map, scale_ratio=5):
|
|||
|
|
self.scene = scene
|
|||
|
|
self.area_range = area_range # 地图实际坐标范围 xmin, xmax, ymin, ymax
|
|||
|
|
self.map = map # 缩放并离散化的地图 array(X,Y)
|
|||
|
|
self.scale_ratio = scale_ratio # 地图缩放率
|
|||
|
|
self.step_length = 50 # 步长(单次移动)
|
|||
|
|
self.v = 100 # 速度
|
|||
|
|
self.step_time = self.step_length/self.v # 单步移动时长
|
|||
|
|
|
|||
|
|
self.planner = RRTStar(rand_area=area_range, map=map, scale_ratio=scale_ratio, max_iter=400, search_until_max_iter=True)
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def is_reached(pos: np.array((float, float)), goal: np.array((float, float)), dis_limit=25):
|
|||
|
|
'''
|
|||
|
|
判断是否到达目标
|
|||
|
|
'''
|
|||
|
|
dis = np.linalg.norm(pos - goal)
|
|||
|
|
return dis < dis_limit
|
|||
|
|
|
|||
|
|
def reset_goal(self, goal:(float, float)):
|
|||
|
|
# TODO: 使目标可达
|
|||
|
|
# 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点
|
|||
|
|
# 目标在地图外面：起点和目标连线最靠近目标的可行点
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
def navigate(self, goal: (float, float), path_smoothing=True, animation=True):
|
|||
|
|
pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) # 机器人当前: 位置 和 朝向
|
|||
|
|
yaw = self.scene.status.rotation.Yaw
|
|||
|
|
print('------------------navigation_start----------------------')
|
|||
|
|
|
|||
|
|
path = self.planner.planning(pos, goal, path_smoothing, animation)
|
|||
|
|
if path:
|
|||
|
|
self.planner.draw_graph(final_path=path) # 画出探索过程
|
|||
|
|
for (x, y) in path:
|
|||
|
|
self.scene.walk_to(x, y, yaw, velocity=self.v)
|
|||
|
|
time.sleep(self.step_time)
|
|||
|
|
pos = np.array((self.scene.status.location.X, self.scene.status.location.Y))
|
|||
|
|
|
|||
|
|
self.planner.reset()
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
'''APF势场法暂不可用'''
|
|||
|
|
# while not self.is_reached(pos, goal):
|
|||
|
|
# # 1. 路径规划
|
|||
|
|
# path = self.planner.planning(pos, goal, path_smoothing, animation)
|
|||
|
|
# self.planner.draw_graph(final_path=path) # 画出探索过程
|
|||
|
|
#
|
|||
|
|
# # 2. 使用APF导航到路径中的每个waypoint
|
|||
|
|
# traj = [(pos[0], pos[1])]
|
|||
|
|
# #self.planner.draw_graph(final_path=traj) # 画出探索过程
|
|||
|
|
# for i, waypoint in enumerate(path[1:]):
|
|||
|
|
# print('waypoint [', i, ']:', waypoint)
|
|||
|
|
# # if (not self.scene.reachable_check(waypoint[0], waypoint[1], yaw)) and self.map[self.planner.real2map(waypoint[0], waypoint[1])] == 0:
|
|||
|
|
# # print('error')
|
|||
|
|
# while not self.is_reached(pos, waypoint):
|
|||
|
|
# # 2.1 计算next_step
|
|||
|
|
# pos = np.array((self.scene.status.location.X, self.scene.status.location.Y))
|
|||
|
|
# Pobs = [] # 障碍物(顾客)位置数组
|
|||
|
|
# for walker in self.scene.status.walkers:
|
|||
|
|
# Pobs.append((walker.pose.X, walker.pose.Y))
|
|||
|
|
# next_step, _ = APF(Pi=pos, Pg=waypoint, Pobs=Pobs, step_length=self.step_length)
|
|||
|
|
# traj.append((next_step[0], next_step[1]))
|
|||
|
|
# #self.planner.draw_graph(final_path=traj) # 画出探索过程
|
|||
|
|
# while not self.scene.reachable_check(next_step[0], next_step[1], yaw): # 取中点直到next_step可达
|
|||
|
|
# traj.pop()
|
|||
|
|
# next_step = (pos + next_step) / 2
|
|||
|
|
# traj.append((next_step[0], next_step[1]))
|
|||
|
|
# #self.planner.draw_graph(final_path=traj) # 画出探索过程
|
|||
|
|
# # 2.2 移动robot
|
|||
|
|
# self.scene.walk_to(next_step[0], next_step[1], yaw, velocity=self.v)
|
|||
|
|
# # print(self.scene.status.info) # print navigation info
|
|||
|
|
# # print(self.scene.status.collision)
|
|||
|
|
# time.sleep(self.step_time)
|
|||
|
|
# # print(self.scene.status.info) # print navigation info
|
|||
|
|
# # print(self.scene.status.collision)
|
|||
|
|
# self.planner.reset()
|
|||
|
|
if self.is_reached(pos, goal):
|
|||
|
|
print('The robot has achieved goal !!')
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
# class Walker:
|
|||
|
|
# def __int__(self, scene):
|
|||
|
|
# self.scene = scene
|
|||
|
|
#
|
|||
|
|
# def add_walkers(self, walker_loc, scene_id=0):
|
|||
|
|
# """
|
|||
|
|
# 批量添加行人
|
|||
|
|
# Args:
|
|||
|
|
# walker_loc: 行人的初始位置列表( 列表元素数量对应行人数量 )
|
|||
|
|
# """
|
|||
|
|
# print('------------------add walker----------------------')
|
|||
|
|
# walker_list = []
|
|||
|
|
# for i in range(len(walker_loc)):
|
|||
|
|
# # 只有可达的位置才能成功初始化行人，显示unreachable的地方不能初始化行人
|
|||
|
|
# walker_list.append(
|
|||
|
|
# GrabSim_pb2.WalkerList.Walker(id=i, pose=GrabSim_pb2.Pose(X=walker_loc[0], Y=walker_loc[1], Yaw=90)))
|
|||
|
|
# scene = self.client.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=scene_id)) # 生成环境中行人
|
|||
|
|
# # print(self.client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) # 打印行人信息
|
|||
|
|
# return scene
|
|||
|
|
#
|
|||
|
|
# def control_walkers(self, walker_loc, scene_id=0):
|
|||
|
|
# """
|
|||
|
|
# 批量移动行人
|
|||
|
|
# Args:
|
|||
|
|
# walker_loc: 行人的终止位置列表
|
|||
|
|
# """
|
|||
|
|
# scene = self.client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
|||
|
|
# controls = []
|
|||
|
|
# for i in range(len(scene.walkers)):
|
|||
|
|
# loc = walker_loc[i]
|
|||
|
|
# is_autowalk = False # True: 随机移动; False: 移动到目标点
|
|||
|
|
# pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
|
|||
|
|
# controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=200, pose=pose))
|
|||
|
|
# scene = self.client.ControlWalkers(
|
|||
|
|
# GrabSim_pb2.WalkerControls(controls=controls, scene=scene_id)) # 设置行人移动速度和目标点
|
|||
|
|
# # print(self.client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) # 打印行人信息
|
|||
|
|
# time.sleep(10)
|
|||
|
|
# return scene
|
|||
|
|
#
|
|||
|
|
# def remove_walkers(self, ids, scene_id=0):
|
|||
|
|
# '''
|
|||
|
|
# 按编号移除行人
|
|||
|
|
# Args:
|
|||
|
|
# ids: 待移除的行人编号列表
|
|||
|
|
# '''
|
|||
|
|
# scene = self.client.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=ids, scene=scene_id)) # 按编号移除行人
|
|||
|
|
# # print(self.client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) # 打印行人信息
|
|||
|
|
# time.sleep(2)
|
|||
|
|
# return scene
|
|||
|
|
#
|
|||
|
|
# def clean_walkers(self, scene_id=0):
|
|||
|
|
# '''
|
|||
|
|
# 删除环境中所有行人
|
|||
|
|
# '''
|
|||
|
|
# scene = self.client.CleanWalkers(GrabSim_pb2.SceneID(value=scene_id))
|
|||
|
|
# return scene
|