parser.add_argument('--output', default='results')
parser.add_argument('--num_particles', default=500, type=int)
parser.add_argument('--visualize', action='store_true')
args = parser.parse_args()
src_path_map = args.path_to_map
src_path_log = args.path_to_log
os.makedirs(args.output, exist_ok=True)
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
logfile = open(src_path_log, 'r')
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
map_model = ReadTable()
ray_map = map_model.return_Map()
print(ray_map.shape)
# ray_map = 0
bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0)
num_particles = args.num_particles
initial_num = num_particles
# X_bar = init_particles_random(num_particles, occupancy_map)
X_bar = init_particles_freespace(num_particles, occupancy_map)
"""
from matplotlib import pyplot as plt
from matplotlib import figure as fig
import time
src_path_map = '../data/map/wean.dat'
src_path_log = '../data/log/robotdata1.log'
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0)
print(bool_occ_map[500][400])
logfile = open(src_path_log, 'r')
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
def visualize_ray_cast(x_t1, bool_occ_map, tstep, output_path):
walk_stride = 8
angle_stride = 10
x, y, theta = x_t1
laser_x_t1 = x + 25 * math.cos(theta)
laser_y_t1 = y + 25 * math.sin(theta)
xout = laser_x_t1 / 10
yout = laser_y_t1 / 10
for deg in range(-90, 90, angle_stride):
laser_theta = (deg + theta * 180 / math.pi) * (math.pi / 180)
calc_distance, x_end, y_end = ray_cast_visual(laser_x_t1, laser_y_t1,
laser_theta, walk_stride)