import sys import os sys.path.append(os.pardir) import matplotlib.pyplot as plt # Grid Map 2D Module from grid_map.grid_map_2d import GridMap2DConfigParams from grid_map.grid_map_2d import GridMap2D # Sample Data from tests.test_data import create_sample_robot_path from tests.test_data import create_sample_room if __name__ == "__main__": grid_conf = GridMap2DConfigParams(x_min=-50.0, y_min=-25.0, x_max=50.0, y_max=25.0, reso=0.2, init_val=0.0) grid2d = GridMap2D(gridmap_config=grid_conf) create_sample_room(grid2d) create_sample_robot_path(grid2d) fig1, ax1 = plt.subplots(nrows=1,ncols=1,figsize=(14, 9),dpi=100) fig2, ax2 = plt.subplots(nrows=1,ncols=1,figsize=(14, 9),dpi=100) grid2d.show_heatmap(ax1) grid2d.show_heatmap(ax2) plt.show()
if (map2d.get_val_via_map_coord(x_map=x_map, y_map=y_map) >
0.3):
scans.ranges[i] = \
math.sqrt((x_map - x_ray_src_map)**2 + (y_map - y_ray_src_map)**2) * conf.reso
break
return scans
if __name__ == "__main__":
import matplotlib.pyplot as plt
config_src = GridMap2DConfigParams(\
x_min=-50.0, y_min=-25.0, \
x_max=50.0, y_max=25.0, \
reso=0.2, init_val=0.0)
map2d_src = GridMap2D(gridmap_config=config_src)
config_dst = GridMap2DConfigParams(\
x_min=-50.0, y_min=-25.0, \
x_max=50.0, y_max=25.0, \
reso=0.2, init_val=0.5)
map2d_dst = GridMap2D(gridmap_config=config_dst)
for i in range(int(50 / 0.2)):
y_coor = i * config_src.reso - 25.0
if (map2d_src.is_valid_global_coord(10.0, y_coor)):
map2d_src.update_val_via_global_coord(x=10.0, y=y_coor, value=1.0)
from tests.test_data import create_sample_robot_path
from tests.test_data import create_sample_room
if __name__ == "__main__":
print(__file__ + " Started!")
particle_num = 10
err_conf = MotionErrorModel2dConfigParams(
std_rot_per_rot=0.1, std_rot_per_trans=0.5, \
std_trans_per_trans=0.1, std_trans_per_rot=0.01)
err_model = MotionErrorModel2D(conf=err_conf)
grid_conf_src = GridMap2DConfigParams(x_min=-50.0,
y_min=-25.0,
x_max=50.0,
y_max=25.0,
reso=0.2,
init_val=0.0)
grid2d_gt = GridMap2D(gridmap_config=grid_conf_src)
grid2d_odom_disp = GridMap2D(gridmap_config=grid_conf_src)
grid2d_pose = GridMap2D(gridmap_config=grid_conf_src)
create_sample_room(grid2d_gt)
create_sample_room(grid2d_odom_disp)
create_sample_room(grid2d_pose)
path = create_sample_robot_path(grid2d_odom_disp)
# Lidar Property
lidar_config = VirtualLidar2DConfigParams(range_max=10.0, \
min_angle=-math.pi/2.0, \
max_angle=math.pi/2.0, \
if __name__ == '__main__':
print(__file__ + " Started!")
particle_num = 50
# Error Config
err_conf = MotionErrorModel2dConfigParams(std_rot_per_rot=0.01,
std_rot_per_trans=0.01,
std_trans_per_trans=0.01,
std_trans_per_rot=0.01)
err_model = MotionErrorModel2D(conf=err_conf)
# Grid Map Configuration
grid_conf_src = GridMap2DConfigParams(
x_min=-50.0,y_min=-25.0, \
x_max=50.0, y_max=25.0, \
reso=0.2, init_val=0.0)
# Grid Generation
grid2d_gt = GridMap2D(gridmap_config=grid_conf_src)
grid2d_odom_disp = GridMap2D(gridmap_config=grid_conf_src)
grid2d_pose = GridMap2D(gridmap_config=grid_conf_src)
create_sample_room(grid2d_gt)
create_sample_room(grid2d_odom_disp)
create_sample_room(grid2d_pose)
path = create_sample_robot_path(grid2d_odom_disp)
# Lidar Property
lidar_config = VirtualLidar2DConfigParams(range_max=15.0, \
min_angle=-math.pi * 3.0 / 4.0, \
for idx, pole in enumerate(poles):
print(meas_idx)
if (idx in meas_idx):
plt.scatter(pole.x, pole.y, s=160, c='y')
else:
plt.scatter(pole.x, pole.y, s=40, c='k')
plt.legend(bbox_to_anchor=(0, 0.6), loc='upper left')
if __name__ == "__main__":
print(__file__ + "Started!")
# GridMap Preparation
grid_conf_fld = GridMap2DConfigParams(
x_min=-2.70,y_min=-1.95,x_max=2.7,y_max=1.95,reso=0.02, init_val=0.0)
grid_conf_loc = GridMap2DConfigParams(
x_min=-2.70,y_min=-1.95,x_max=2.7,y_max=1.95,reso=0.02, init_val=0.0)
grid2d_gt = GridMap2D(gridmap_config=grid_conf_fld)
grid2d_loc = GridMap2D(gridmap_config=grid_conf_fld)
dT = 0.05
poles = create_robocup_field(grid2d_gt)
poles = create_robocup_field(grid2d_loc)
ellipse_path, controls = create_robocup_ellipse_path(dT)
draw_path(ellipse_path, grid2d_gt)
# Virutual Detector
vir_lm_det_conf = VirtualLMDetectorConfigParams(\
range_max=2.0,
fov=math.pi)