class MapManager:
PUB_RATE = 100000000 # 0.1 sec
def __init__(self):
rospy.init_node("MapManager")
rospy.loginfo("Node Map manager initialized")
self._map = MyMap()
self.map_getter = rospy.Service("my_map/get", GetMap,
self.get_occupancy_grid)
self.map_setter = rospy.Service("my_map/set", SetMap,
self.set_occupancy_grid)
self.is_map_loaded = False
self.map_loader = rospy.Service("my_map/load", LoadMap,
self.load_occupancy_grid)
self.binary_srv = rospy.Service("my_map/binary/get", GetMap,
self.get_binary_map)
self.occup_grid_pub = rospy.Publisher("/my_map",
OccupancyGrid,
queue_size=1)
self.grid_timer = rospy.Timer(rospy.Duration(nsecs=self.PUB_RATE),
self.send_occupancy_grid)
def get_occupancy_grid(self, req):
return self._map.to_msg()
def set_occupancy_grid(self, req):
if not self.is_map_loaded:
self._map.from_msg(req.map)
return True
else:
rospy.logwarn_once("Map not set. Already loaded one.")
return False
def load_occupancy_grid(self, req):
url = req.map_url
rel_path = rospy.get_param('output_path', os.getcwd())
url = os.path.join(rel_path, url)
# Load explored map
try:
explored_map = np.genfromtxt(url, delimiter=',')
self.is_map_loaded = True
except ValueError:
print("There is no map to load")
return OccupancyGrid(), 1
expanded = MyMap.expand(explored_map, 20)
self._map = MyMap(MyMap.binary_to_occupancy(expanded), resolution=0.05)
return self._map.to_msg(), 0
def get_binary_map(self, req):
return self._map.binary_msg
def send_occupancy_grid(self, event):
"""Publish OccupancyGrid map"""
self.occup_grid_pub.publish(self._map.to_msg())
def __init__(self, headless=False):
if not headless:
rospy.init_node("Laser2Grid")
rospy.loginfo("Node initialized")
self.global_x = 0
self.global_y = 0
self.global_yaw = 0
self.global_ang_z = 0
# if dimensions are not squared, it doesn't work properly
self.width = int(25 / self.RESOLUTION)
self.height = int(25 / self.RESOLUTION)
self.grid_map = MyMap(grid=np.ones(
(self.width, self.height), dtype=np.int) * -1,
resolution=self.RESOLUTION)
self.laserSub = rospy.Subscriber("/scan", LaserScan, self.laser_cb)
self.laserSub = rospy.Subscriber("/odom", Odometry, self.position_cb)
# Waiting for Map Manager
rospy.wait_for_service("/my_map/set")
self.set_map_client = rospy.ServiceProxy("/my_map/set", SetMap)
self.set_map_timer = rospy.Timer(
rospy.Duration(nsecs=self.SET_MAP_RATE), self.set_map)
def map_connectivity():
rospy.wait_for_service("/my_map/get")
map_client = rospy.ServiceProxy("/my_map/get", GetMap)
try:
resp = map_client()
print('service get_map successfully called')
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
# print('shape is', np.shape(resp))
map_gen = MyMap()
# print('print map_ ',map_)
grid = map_gen.from_msg(resp.map)
finished = True
adyacency_count = 0
threshold = 20
len_x, len_y = np.shape(grid)
list_of_zeros = np.transpose(np.where(grid == 0))
print('list of possible points has dimensions: ', np.shape(list_of_zeros))
for a in list_of_zeros:
# list of zeros is a tuple
# print('adjacency count is: ',adjacency_count)
x = a[0]
y = a[1]
# print('checking position ',x,y)
# check adyacent positions
if x > 0:
if grid[x - 1, y] == -1:
adyacency_count += 1
# if adyacency_count >= threshold:
# finished = False
# return finished
if x < len_x - 1:
if grid[x + 1, y] == -1:
adyacency_count += 1
# if adyacency_count >= threshold:
# finished = False
# return finished
if y > 0:
if grid[x, y - 1] == -1:
adyacency_count += 1
# if adyacency_count >= threshold:
# finished = False
# return finished
if y < len_y - 1:
if grid[x, y + 1] == -1:
adyacency_count += 1
# if adyacency_count >= threshold:
# finished = False
# return finished
print('the total number of adjacent points to unexplored ones is ',
adyacency_count)
if adyacency_count > threshold:
finished = False
print('map is incomplete')
return finished
def load_occupancy_grid(self, req):
url = req.map_url
rel_path = rospy.get_param('output_path', os.getcwd())
url = os.path.join(rel_path, url)
# Load explored map
try:
explored_map = np.genfromtxt(url, delimiter=',')
self.is_map_loaded = True
except ValueError:
print("There is no map to load")
return OccupancyGrid(), 1
expanded = MyMap.expand(explored_map, 20)
self._map = MyMap(MyMap.binary_to_occupancy(expanded), resolution=0.05)
return self._map.to_msg(), 0
def check_in_map(self, points):
valid = True
rospy.wait_for_service("my_map/get")
map_client = rospy.ServiceProxy("my_map/get", GetMap)
grid = map_client()
rospy.wait_for_service("/my_map/binary/get")
binarymap = rospy.ServiceProxy("/my_map/binary/get", GetMap)
resp = binarymap()
map = MyMap()
map.from_msg(grid.map)
binary = MyMap.from_msg(map, resp.map)
for p in points: #check positions in the map
if binary[p[0]][p[1]] == 1:
valid = False
print('trajectory is going through obstacles, incorrect')
break
return valid
def __init__(self):
rospy.init_node("MapManager")
rospy.loginfo("Node Map manager initialized")
self._map = MyMap()
self.map_getter = rospy.Service("my_map/get", GetMap,
self.get_occupancy_grid)
self.map_setter = rospy.Service("my_map/set", SetMap,
self.set_occupancy_grid)
self.is_map_loaded = False
self.map_loader = rospy.Service("my_map/load", LoadMap,
self.load_occupancy_grid)
self.binary_srv = rospy.Service("my_map/binary/get", GetMap,
self.get_binary_map)
self.occup_grid_pub = rospy.Publisher("/my_map",
OccupancyGrid,
queue_size=1)
self.grid_timer = rospy.Timer(rospy.Duration(nsecs=self.PUB_RATE),
self.send_occupancy_grid)
def map_connectivity(self):
resp = self.map_client()
map_gen = MyMap()
grid = map_gen.from_msg(resp.map)
finished = True
adjacency_count = 0
threshold = 1
len_x, len_y = np.shape(grid)
list_of_zeros = np.transpose(np.where(grid == 0))
print('list of possible points has dimensions: ',
np.shape(list_of_zeros))
for a in list_of_zeros:
x, y = a
if x > 0:
if grid[x - 1, y] == -1:
adjacency_count += 1
if x < len_x - 1:
if grid[x + 1, y] == -1:
adjacency_count += 1
if y > 0:
if grid[x, y - 1] == -1:
adjacency_count += 1
if y < len_y - 1:
if grid[x, y + 1] == -1:
adjacency_count += 1
if adjacency_count > threshold:
finished = False
print('map is incomplete')
return finished
if finished:
print('the total number of adjacent points to unexplored ones is ',
adjacency_count)
return finished
def check_obstacles(self, path):
valid = True
rospy.wait_for_service("my_map/get")
map_client = rospy.ServiceProxy("my_map/get", GetMap)
grid = map_client()
rospy.wait_for_service("/my_map/binary/get")
binarymap = rospy.ServiceProxy("/my_map/binary/get", GetMap)
resp = binarymap()
map = MyMap()
map.from_msg(grid.map)
binary = MyMap.from_msg(map, resp.map)
# plt.imshow(binary, cmap='Greys', interpolation='nearest')
# plt.pause(5)
path_to_map = np.around(path/0.05, 0).astype(int) #to achieve points in the scale of the map
#now we create points in trajectory with linspace to check
for i in range(1,len(path_to_map)):
dist = abs(path_to_map[i-1][0]-path_to_map[i][0] + path_to_map[i-1][1]-path_to_map[i][1])
print('distance between initial point ',path_to_map[i-1],' and next point ',path_to_map[i], 'is ',dist)
if dist > 1:
elements = int(round(dist/2, 0))
print('number of points to check is ',elements)
else:
elements = 1
tray_points = get_middle_points(path_to_map[i-1], path_to_map[i], num=elements)
#print('possible trajectory is: ',list(tray_points))
corrected_points = np.around(tray_points, 0).astype(int)
#print('corrected trajectory is: ',list(corrected_points))
for p in tray_points: #check positions in the map
if binary[p[0]][p[1]] == 1:
valid = False
print('trajectory is going through obstacles, incorrect')
break
if not valid:
break
return valid
def get_path(self, req):
start = req.start
end = req.goal
tolerance = req.tolerance
resp = self.map_client()
map_ = MyMap()
map_.from_msg(resp.map)
my_down = MyMap(grid=map_.downscale(map_.grid, 20), resolution=1 / 20)
explored_map = my_down.binary
cv2.imshow("Map BW", map_.to_img(True))
voronoi_graph = generate_voronoi(explored_map)
cv2.imshow("Voronoi", voronoi_graph)
cv2.waitKey(0)
cv2.destroyAllWindows()
free_points = np.transpose(np.where(voronoi_graph == 0)) # Points in white in the voronoi
min_start = 100
min_end = 100
# closest_start = min(free_points, key=lambda p: abs(p[0] - start.pose.position.x) + abs(p[1] - start.pose.position.y))
# closest_end = min(free_points, key=lambda p: abs(p[0] - end.pose.position.x) + abs(p[1] - end.pose.position.y))
# Get the closest point in the graph for start and end
for point in free_points:
tmp_start = (abs(point[0] - start.pose.position.x*10) + abs(point[1] - start.pose.position.y*10))
tmp_end = (abs(point[0] - end.pose.position.x*10) + abs(point[1] - end.pose.position.y*10))
if tmp_start < min_start:
min_start = tmp_start
start_point = point
if tmp_end < min_end:
min_end = tmp_end
end_point = point
path_list = best_first_search(voronoi_graph,
(start_point[0], start_point[1]),
(end_point[0], end_point[1]), 0)
path_list = list(map(lambda i: (float(i[0])/10, float(i[1])/10), path_list))
path_list.append((end.pose.position.x, end.pose.position.y))
path_list.insert(0, (start.pose.position.x, start.pose.position.y))
self.is_path = True
self.path = Path()
self.path.header.frame_id = "map"
print('route starts in ',path_list[0],' and ends in ',path_list[-1])
filtered_path = []
filtered_path = np.copy(path_list[::2])
if path_list[-1][0] != filtered_path[-1][0] or path_list[-1][1] != filtered_path[-1][1]:
filtered_path = np.vstack([filtered_path, path_list[-1]])
filtered_path = np.reshape(filtered_path, (-1,2))
final_path = []
n = 25 #initial tolerance is n * 0.2
while final_path == [] and n >= 1:
print('Tolerance is ',0.2*n,'m')
try_path = self.interpolate_path(filtered_path,0.2*n)
print('new path has number of steps reduced to ',len(try_path))
final_path = self.check_obstacles(try_path)
if len(final_path) <= 1:
final_path = []
n -= 1
# if no_obstacles and len(try_path) >= 2:
# smooth_path = np.copy(try_path)
# print('smoothened trajectory is valid')
# elif len(try_path) < 2:
# break
if path_list[-1][0] != final_path[-1][0] or path_list[-1][1] != final_path[-1][1]:
smooth_path = np.vstack([final_path, path_list[-1]])
for p in final_path:
point = PoseStamped()
point.pose.position.x = p[0]
point.pose.position.y = p[1]
self.path.poses.append(point)
return self.path
# dis = distance.DistanceBetweenTwoPoint(random.randint(1, 50), 1, i + 1)
# listkk.append(dis)
#
# # for i in range(40):
# # print(listkk[i].UP_id, "到", listkk[i].BS_id, "的距离是：", listkk[i].dis)
#
# minDistance.quick_sort(listkk, 0, len(listkk)-1)
#
# for i in range(40):
# print(listkk[i].UP_id, "到", listkk[i].BS_id, "的距离是：", listkk[i].dis)
#
# print("到最近的基站的ID是", listkk[0].BS_id)
#
# print(minDistance.computeMinDistance_BS_ID(listkk))
start_time = datetime.datetime.now()
myMap = MyMap()
myMap.generateUsersPoint()
myMap.generateBaseStationPoint()
myMap.userPoint_set_A()
myMap.userPoint_set_R()
myMap.userPoint_set_U()
# de = DirectEstimate(myMap)
# real_density = de.realDensity()
# estimate_density = de.DirectStatistics_densityEstimation()
# print("用户点的个数是：", myMap.userNum, "\n",
# "基站的个数是：", myMap.baseStationNum, "\n", "直接统计法错误率是：", de.Mean_value_of_difference(real_density, estimate_density))
# print("程序运行时间为：", end_time - start_time)
EMde = EM_densityEstimation(myMap)
real_density = EMde.realDensity()
class Laser2Grid:
RESOLUTION = 0.05
SET_MAP_RATE = 100000000 # 0.1 sec
def __init__(self, headless=False):
if not headless:
rospy.init_node("Laser2Grid")
rospy.loginfo("Node initialized")
self.global_x = 0
self.global_y = 0
self.global_yaw = 0
self.global_ang_z = 0
# if dimensions are not squared, it doesn't work properly
self.width = int(25 / self.RESOLUTION)
self.height = int(25 / self.RESOLUTION)
self.grid_map = MyMap(grid=np.ones(
(self.width, self.height), dtype=np.int) * -1,
resolution=self.RESOLUTION)
self.laserSub = rospy.Subscriber("/scan", LaserScan, self.laser_cb)
self.laserSub = rospy.Subscriber("/odom", Odometry, self.position_cb)
# Waiting for Map Manager
rospy.wait_for_service("/my_map/set")
self.set_map_client = rospy.ServiceProxy("/my_map/set", SetMap)
self.set_map_timer = rospy.Timer(
rospy.Duration(nsecs=self.SET_MAP_RATE), self.set_map)
def position_2_grid(self, x, y):
"""Translates real pose to grid position"""
return [
int(round(x / self.RESOLUTION, 0) - 1),
int(round(y / self.RESOLUTION, 0) - 1)
]
def laser_cb(self, data):
"""Fills occupancy grid with the detected laser info"""
if abs(self.global_ang_z) != 0.0:
return
robot_pos = self.position_2_grid(self.global_x, self.global_y)
points = []
for i, rng in enumerate(data.ranges):
is_obs = False if rng == float('inf') else True
rng = data.range_max if not is_obs else rng
end_pos = list(
map(
lambda x, y: int(x + y), robot_pos,
polar_to_geom(i + degrees(self.global_yaw), rng /
self.RESOLUTION))) # translation 2 robot_pos
points += get_middle_points(robot_pos, end_pos)
if is_obs:
self.grid_map.mark_as_occupied(end_pos[0], end_pos[1])
for p in set(points):
self.grid_map.mark_as_free(p[0], p[1])
def position_cb(self, data):
"""Updates robot status"""
self.global_x = data.pose.pose.position.x
self.global_y = data.pose.pose.position.y
_, _, self.global_yaw = quat_to_euler(data.pose.pose.orientation)
self.global_ang_z = round(data.twist.twist.angular.z, 2)
def set_map(self, event):
"""Send map to map manager via setter"""
self.set_map_client(self.grid_map.to_msg(),
PoseWithCovarianceStamped())
def run_game():
#初始化循环并创建一个对象
prtips()
pygame.init()
#设置背景音效
pygame.mixer_music.load('Lullatone - outside sandwiches.mp3')
pygame.mixer_music.play(10, 0)
#加载动作音效
di_settings = Settings()
screen = pygame.display.set_mode(
(di_settings.screen_width, di_settings.screen_height))
pygame.display.set_caption('dinosaur run!')
#生成对象图片列表
image_d = ['images/long1.png', 'images/long2.png', 'images/long3.png']
image_c = [
'images/cactus01.png', 'images/cactus02.png', 'images/cactus03.png'
]
image_b = ['images/bird1.png', 'images/bird2.png', 'images/bird3.png']
#初始化障碍物-鸟
bird = Bird(800, 150, screen)
dino = Dino(screen)
dino.load('images/long1_2.png', 40, 43, 2)
group = pygame.sprite.Group()
group.add(dino)
#初始化地图
aa = 0
cc = 0
dd = 0
bg1 = MyMap(0, 0, screen)
bg2 = MyMap(734, 0, screen)
ca1 = obstacle.Cactus(800, 205, screen)
ca2 = obstacle.Cactus(1200, 205, screen)
ca3 = obstacle.Cactus(800, 205, screen)
#设置一些标志变量
player_jump = 0
jump_vel = -4.95
flag_bird = False
flag_cactus1 = -1
flag_cactus2 = -1
flag_cactus3 = -1
a_rate = 10000
rate = 2
#载入结束图像
image_over = pygame.image.load('images/game_over.png').convert_alpha()
image_overd = pygame.image.load('images/over.png').convert_alpha()
tick = pygame.time.Clock()
#开始游戏主循环
while True:
tick.tick(100)
ticks = pygame.time.get_ticks()
#计数器
dd += 1
scoreboard = Scoreboard(di_settings, screen, dd / 10)
cc += 1
if int(cc / 10) == 3:
cc = 0
# 监视键盘和鼠标事件
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE:
player_jump = 1
elif event.key == pygame.K_p:
while not event.key == 27:
for event in pygame.event.get():
print()
elif event.key == 27:
sys.exit()
else:
prtips()
if dd == 1050:
a_rate = 2100
#绘制背景
bg1.map_update()
bg2.map_update()
bg1.map_rolling(rate * (1 + dd / a_rate))
bg2.map_rolling(rate * (1 + dd / a_rate))
#是否生成仙人掌
flag_cactus1 = ca1.randcactus(flag_cactus1)
flag_cactus2 = ca2.randcactus(flag_cactus2)
if (not flag_cactus1 == -1) and dd > 1300:
# 绘制仙人掌
ca1.blitme(image_c[flag_cactus1])
flag_cactus1 = ca1.cactus_rolling(flag_cactus1,
rate * (1 + dd / a_rate))
if (not flag_cactus2 == -1) and dd > 1300:
ca2.blitme(image_c[flag_cactus2])
flag_cactus2 = ca2.cactus_rolling(flag_cactus2,
rate * (1 + dd / a_rate))
if dd <= 1000:
flag_cactus3 = 2
if flag_cactus3 == 2:
ca3.blitme(image_c[flag_cactus3])
flag_cactus3 = ca3.cactus_rolling(flag_cactus3,
rate * (1 + dd / a_rate))
#更新恐龙位置
jump_vel, player_jump = dino.jump(player_jump, jump_vel,
1 + dd / a_rate)
if player_jump == 0:
group.update(ticks)
group.draw(screen)
else:
dino.blitme(image_d[2])
#是否生成飞鸟
if dd == 800:
flag_bird = True
# 更新飞鸟
if flag_bird:
flag_bird = bird.bird_rolling()
bird.blitme(image_b[int(cc / 15)])
#更新计分板
scoreboard.show_score()
# 更新画面
pygame.display.update()
#检测碰撞
hit = hit_find(dino.x, ca1.x, ca2.x, bird.x, dino.y)
if hit:
break
screen.blit(image_over, (270, 100))
screen.blit(image_overd, (220, 140))
pygame.display.update()
time.sleep(1)