def _create_map_1(env):
#obs_movement = MovementPattern.StaticMovement((0,0))
#obs = DynamicObstacle(obs_movement)
#obs.fillcolor = (0x55, 0x55, 0x55)
#obs.shape = 4
#obs.polygon = Polygon([
# (640, 80),
# (640, 480),
# (445, 450),
# (408, 300),
# (408, 165),
# (460, 108),
# (490, 104),
# (490, 91),
# (640, 80),
#]);
with open('obs2.json') as f:
obslist = json.load(f)
for obs in obslist:
obs_movement = MovementPattern.StaticMovement((0, 0))
if obs['type'] == 'circle':
obs_movement = MovementPattern.StaticMovement(obs['center'])
obs2 = DynamicObstacle(obs_movement)
obs2.shape = 1
obs2.radius = obs['radius']
obs2.fillcolor = (0x55, 0x55, 0x55)
env.static_obstacles.append(obs2)
continue
polygon = Polygon(np.array(obs['points']))
obs2 = DynamicObstacle(obs_movement)
obs2.shape = 4
obs2.polygon = polygon
obs2.fillcolor = (0x55, 0x55, 0x55)
env.static_obstacles.append(obs2)
def _map_mod_obsmat(env):
H_pix2meter = np.array([[2.8128700e-02, 2.0091900e-03, -4.6693600e+00],
[8.0625700e-04, 2.5195500e-02, -5.0608800e+00],
[3.4555400e-04, 9.2512200e-05, 4.6255300e-01]])
H_meter2pix = np.linalg.inv(H_pix2meter)
with open('obsmat.json', 'r') as f:
obsmat = json.load(f)
time_offset = obsmat[str(
env.cmdargs.ped_id_to_replace
)][0]['time'] if env.cmdargs.ped_id_to_replace > 0 else 0
for ped_id in obsmat:
if ped_id == str(env.cmdargs.ped_id_to_replace):
continue
path_list = []
firstpoint = obsmat[ped_id][0]
path_list.append((-1000, -1000, firstpoint['time'] - time_offset))
for waypoint in obsmat[ped_id]:
#pos = Geometry.apply_homography(H_meter2pix, (waypoint['pos_x'], waypoint['pos_y']))
pos = (waypoint['pos_x'], waypoint['pos_y'])
# Rotate 90 degrees to match video
path_list.append(
(pos[1], pos[0], waypoint['time'] - time_offset))
# Get obstacles offscreen after they finish their path
path_list.append((-1000, -1000, path_list[-1][2] + 0.01))
obs_mover = MovementPattern.PathMovement(path_list, loop=False)
dynobs = DynamicObstacle(obs_mover, obs_id=int(ped_id))
dynobs.shape = 3
dynobs.width = 0.6
dynobs.height = 0.3
env.dynamic_obstacles.append(dynobs)
def make_default_robot(robot_name, path_color):
start_point = Target(
(np.random.randint(50, 350), np.random.randint(350, 550)),
radius=20,
color=0x00FF00)
initial_position = np.array(start_point.position)
target = Target((np.random.randint(600, 760), np.random.randint(50, 250)),
radius=20)
objective = NavigationObjective(target, env)
env.non_interactive_objects += [start_point, target]
robot = Robot(initial_position,
cmdargs,
env,
path_color=path_color,
name=robot_name,
objective=objective)
robot.put_sensor('radar', RadarSensor(env, robot, [], radar))
robot.put_sensor('gps', GpsSensor(robot))
if robot_name in params['robots']:
robot.put_sensor('params', params['robots'][robot_name])
else:
robot.put_sensor('params', {})
robot_obstacle = DynamicObstacle(MovementPattern.RobotBodyMovement(robot))
robot_obstacle.shape = 1
robot_obstacle.radius = 10
robot_obstacle.fillcolor = (0xcc, 0x88, 0x44)
robot.set_obstacle(robot_obstacle)
return robot, target
def _map_mod_7(env):
y_arr = [50, 200, 350, 500]
for x in [200, 440]:
for ind, i in enumerate(y_arr):
origin = np.array([x, i])
speed = _get_speed_for_speedmode(env.get_speed_mode())
obs_mover = MovementPattern.CircleMovement(origin, 30, speed=speed)
dynobs = DynamicObstacle(obs_mover)
dynobs.radius = 50
dynobs.shape = 1
env.dynamic_obstacles.append(dynobs)
for x in [300, 560]:
for ind, i in enumerate(y_arr):
origin = np.array([x, i])
speed = _get_speed_for_speedmode(env.get_speed_mode())
obs_mover = MovementPattern.RandomMovement(initial_pos=origin,
speed=speed)
dynobs = DynamicObstacle(obs_mover)
dynobs.size = [50, 50]
dynobs.shape = 2
env.dynamic_obstacles.append(dynobs)
def _map_mod_3(env):
for j in np.arange(8):
x = int(np.random.uniform(100, 700))
y = int(np.random.uniform(150, 450))
origin = (x, y)
speed = _get_speed_for_speedmode(env.get_speed_mode())
obs_mover = MovementPattern.CircleMovement(origin, 30, speed=speed)
dynobs = DynamicObstacle(obs_mover)
dynobs.radius = int(np.random.uniform(20, 30))
dynobs.shape = 1
env.dynamic_obstacles.append(dynobs)
def _map_mod_8(env):
x = [100, 400, 600, 100, 700, 650]
y = [50, 350, 175, 200, 400, 500]
for ind, i in enumerate(x):
origin = np.array([i, y[ind]])
speed = _get_speed_for_speedmode(env.get_speed_mode())
obs_mover = MovementPattern.CircleMovement(origin, 30, speed=speed)
dynobs = DynamicObstacle(obs_mover)
dynobs.radius = 20
dynobs.shape = 1
env.dynamic_obstacles.append(dynobs)
def _create_obs_from_spec(obs_spec):
obs_movement = MovementPattern.StaticMovement((0, 0))
if obs_spec['type'] == 'circle':
obs_movement = MovementPattern.StaticMovement(obs_spec['center'])
new_obs = DynamicObstacle(obs_movement)
new_obs.shape = 1
new_obs.radius = obs_spec['radius']
new_obs.fillcolor = (0x55, 0x55, 0x55)
return new_obs
polygon = Polygon(np.array(obs_spec['points']))
new_obs = DynamicObstacle(obs_movement)
new_obs.shape = 4
new_obs.polygon = polygon
new_obs.fillcolor = (0x55, 0x55, 0x55)
return new_obs
def _create_map_2(env):
with open('obs2.json') as f:
obslist = json.load(f)
for obs in obslist:
obs_movement = MovementPattern.StaticMovement((0, 0))
if obs['type'] == 'circle':
obs_movement = MovementPattern.StaticMovement(obs['center'])
obs2 = DynamicObstacle(obs_movement)
obs2.shape = 1
obs2.radius = obs['radius']
obs2.fillcolor = (0x55, 0x55, 0x55)
env.static_obstacles.append(obs2)
continue
polygon = Polygon(np.array(obs['points']))
obs2 = DynamicObstacle(obs_movement)
obs2.shape = 4
obs2.polygon = polygon
obs2.fillcolor = (0x55, 0x55, 0x55)
env.static_obstacles.append(obs2)
def _map_mod_2(env):
p_list = [
[100, 50],
[400, 350],
[600, 175],
]
for p in p_list:
x = p[0]
y = p[1]
origin = np.array([x, y])
speed = _get_speed_for_speedmode(env.get_speed_mode())
obs_mover = MovementPattern.CircleMovement(origin, 30, 10)
dynobs = DynamicObstacle(obs_mover)
dynobs.radius = 50
dynobs.shape = 1
env.dynamic_obstacles.append(dynobs)
def make_randompath_dynamic_obstacle(env,
x_low=1,
x_high=None,
y_low=1,
y_high=None,
radius_low=5,
radius_high=40,
shape=1,
speed_low=1.0,
speed_high=9.0,
num_path_points=50,
path_x_low=1,
path_x_high=None,
path_y_low=1,
path_y_high=None):
x_high = env.width if x_high is None else x_high
y_high = env.height if y_high is None else y_high
path_x_high = env.width if path_x_high is None else path_x_high
path_y_high = env.height if path_y_high is None else path_y_high
speed = np.random.uniform(low=speed_low, high=speed_high)
path_list = []
for j in range(num_path_points):
x_coord = int(np.random.uniform(path_x_low, path_x_high))
y_coord = int(np.random.uniform(path_y_low, path_y_high))
path_list.append((x_coord, y_coord))
# For smooth looping, close the path
path_list.append(path_list[0])
obs_mover = MovementPattern.PathMovement(path_list, speed=speed, loop=True)
dynobs = DynamicObstacle(obs_mover)
dynobs.radius = int(np.random.uniform(radius_low, radius_high))
dynobs.size = [dynobs.radius, dynobs.radius]
dynobs.shape = shape
return dynobs
def _map_mod_5(env):
for i in range(1, 20):
speed = _get_speed_for_speedmode(env.get_speed_mode())
speed_args = _fixed_speed_kwargs(speed)
dynobs = make_randompath_dynamic_obstacle(env,
radius_low=10,
radius_high=25,
**speed_args)
env.dynamic_obstacles.append(dynobs)
for j in np.arange(10):
x = int(np.random.uniform(0, 800))
y = int(np.random.uniform(0, 600))
origin = np.array([x, y])
speed = _get_speed_for_speedmode(env.get_speed_mode())
obs_mover = MovementPattern.CircleMovement(origin, 30, speed=speed)
dynobs = DynamicObstacle(obs_mover)
dynobs.radius = int(np.random.uniform(10, 20))
dynobs.shape = 1
env.dynamic_obstacles.append(dynobs)
for j in np.arange(10):
x = int(np.random.uniform(0, 800))
y = int(np.random.uniform(0, 600))
origin = np.array([x, y])
speed = _get_speed_for_speedmode(env.get_speed_mode())
obs_mover = MovementPattern.RandomMovement(initial_pos=origin,
speed=speed)
dynobs = DynamicObstacle(obs_mover)
dynobs.size = [
int(np.random.uniform(10, 20)),
int(np.random.uniform(10, 20))
]
dynobs.shape = 2
env.dynamic_obstacles.append(dynobs)