def main():
"""
:return:
"""
parameters = robot_prob.ParametersProb()
this_goal = robot_prob.Goal(location=(random.randrange(20, 80),
random.randrange(10, 60), math.pi),
radius=3)
true_pose = (random.randrange(10, 50), 90, 0.1)
this_map = mapdef.mapdef()
sonar_params = {'RMAX': 100, 'EXP_LEN': 0.1, 'r_rez': 2}
this_sonar = ogmap.Sonar(num_theta=16, gauss_var=0.1, params=sonar_params)
this_ens = mcl.Ensemble(pose=true_pose,
nn=100,
acc_var=np.array((0.001, 0.001, 0.001)),
meas_var=np.array((0.1, 0.1, 0.1)))
this_robot = RobotProbHA(parameters, this_sonar)
this_robot.situate(this_map, true_pose, this_goal, this_ens)
# print "Robot Running"
this_robot.automate(num_steps=100)
# plt.close()
if robot.check_success(this_goal, this_robot):
print "SUCCESS"
return True
else:
print "FAILURE"
return False
def main():
print """Legend:
Yellow star\t -\t True position of robot
Blue arrows\t -\t Particle cloud
Yellow dots\t -\t Sonar pings
Green boxes\t -\t Obstacles
Red star\t -\t Goal"""
these_parameters = Parameters(vel_max=1
, omega_max=0.1
, displacement_slowdown=5
, avoid_threshold=5
)
true_pose = (20, 90, pi)
this_goal = Goal(location=(50, 50, 0)
, radius=3)
this_map = mapdef()
this_sonar = ogmap.Sonar(num_theta=10
, gauss_var=.01
)
this_robot = Robot(these_parameters
, this_sonar
)
this_robot.situate(this_map
, true_pose
, this_goal
)
this_robot.automate()
if check_success(this_goal, this_robot):
print "SUCCESS"
else:
print "FAILURE"
def setup_module(module):
"""setup state for tests"""
global pose
global th
global rs
global scan
global sonar
pose = (0, 0, 0) #pose
th = np.linspace(0, 2 * pi, 10)
rs = np.linspace(0, 100, 10)
scan = ogmap.Scan(pose, th, rs)
sonar = ogmap.Sonar()
def setup_module(module):
"""setup state for tests"""
global test_robot
parameters = robot.Parameters()
this_sonar = ogmap.Sonar(num_theta=1, gauss_var=0.1**2)
test_robot = robot.Robot(parameters, this_sonar)
this_pose = (0, 0, 0)
this_goal = robot.Goal(location=(10, 10, 0), radius=3)
this_map = ogmap.OGMap(gridsize=1)
test_robot.situate(this_map, this_pose, this_goal)
if self.navigator.current_behavior.name == "Goal-reached":
self.goal_attained = True
control_v = self.vcontroller(policy(**robot_state))
return control_x, control_v
if __name__ == "__main__":
print """Legend:
Yellow star\t -\t True position of robot
Blue arrows\t -\t Particle cloud
Yellow dots\t -\t Sonar pings
Green boxes\t -\t Obstacles
Red star\t -\t Goal"""
parameters = Parameters(vel_max=1,
omega_max=0.1,
displacement_slowdown=25,
avoid_threshold=6)
true_pose = (20, 90, pi)
this_goal = Goal(location=(50, 50, 0), radius=3)
this_map = mapdef()
this_sonar = ogmap.Sonar(num_theta=16, gauss_var=0.01)
this_robot = RobotHA(parameters, this_sonar)
this_robot.situate(this_map, true_pose, this_goal)
plt.ion()
this_robot.automate(500)
plt.draw()
if __name__ == "__main__":
from mapdef import mapdef
control_x = np.array([0, 0, 0]) # stay put
control_v = np.array([0, 0, 0])
measure_x = np.array([0, 0, 0])
meas_rate = 10
true_pose = (50, 50, 0)
this_ens = Ensemble(pose=true_pose)
this_sonar = ogmap.Sonar(num_theta=10, gauss_var=1)
this_map = mapdef()
plt.ion()
fig = plt.figure()
scan = this_sonar.simulate_scan(true_pose, this_map)
for i in range(num_steps):
print this_ens.x_ens.shape
this_ens.pf_update(control_x, control_v)
this_ens.show_map_scan(scan=scan, this_map=this_map, pose=true_pose)
if i % meas_rate == 0:
print this_ens.x_ens.shape
scan = this_sonar.simulate_scan(true_pose, this_map)
this_ens.pf_sonar(scan, this_sonar, this_map)
this_ens.inject_random(true_pose, scan, this_sonar, this_map, 10)
this_ens.show_map_scan(scan=scan,
plt.plot(true_pose[0], true_pose[1], '*', color='y', markersize=30)
plt.plot(this_x0, this_y0, '.', color='b', markersize=20)
plt.plot(
this_x0 + some_scan.rs * np.cos(some_scan.thetas + this_phi_guess),
this_y0 + some_scan.rs * np.sin(some_scan.thetas + this_phi_guess),
'.',
color='r',
markersize=10)
plt.xlim(0, some_map.gridsize)
plt.ylim(0, some_map.gridsize)
plt.draw()
return ll, (xs, ys)
if __name__ == "__main__":
from mapdef import mapdef
N_THETA = 10
true_pose = np.array((50, 50, 0))
x0, y0, phi = true_pose
phi_guess = phi
this_sonar = ogmap.Sonar(num_theta=N_THETA, gauss_var=1)
this_map = mapdef()
scan = this_sonar.simulate_scan(true_pose, this_map)
plt.ion()
loglike_map(true_pose, scan, this_map, this_sonar, plot_on=True)
plt.show(block=True)