def load_elements_by_random(self):
map = Map((int(self.cfg['MAP']['grid_row_dimension']),
int(self.cfg['MAP']['grid_column_dimension'])))
count = 0
while count <= int(self.cfg['MAP']['random_init_blocks_num']):
x = random.randint(0,
int(self.cfg['MAP']['grid_row_dimension']) - 1)
y = random.randint(
0,
int(self.cfg['MAP']['grid_column_dimension']) - 1)
if (x, y) not in map.blocks:
count += 1
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
count = 0
while count <= int(self.cfg['MAP']['random_init_robots_num']):
x = random.randint(0,
int(self.cfg['MAP']['grid_row_dimension']) - 1)
y = random.randint(
0,
int(self.cfg['MAP']['grid_column_dimension']) - 1)
if (x, y) not in map.blocks:
count += 1
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
def main():
# Instantiate Map
src_path_map = "data/two_obs.dat"
map1 = Map(src_path_map)
# Instantiate dense lidar for evaluation
dense_lidar = LidarSimulator(map1, angles=np.arange(90) * 4)
# Instantiate Robot to be evaluated
safe_robbie = Robot(map1, use_safe=True)
unsafe_robbie = Robot(map1, use_safe=False)
# Instantiate list to store closest distance over time
safe_closest_list = []
unsafe_closest_list = []
for i in range(100):
plt.cla()
# Move robot
safe_robbie.update()
unsafe_robbie.update()
# Evaluation: Get distance to closest obstacle w/ dense lidar
safe_closest = distance_to_closest_obstacle(dense_lidar, safe_robbie)
safe_closest_list.append(safe_closest)
unsafe_closest = distance_to_closest_obstacle(dense_lidar,
unsafe_robbie)
unsafe_closest_list.append(unsafe_closest)
# TODO: write to text file
# # Visualize
# map1.visualize_map()
# safe_robbie.visualize()
# plt.pause(0.1)
# Visualize history
map1.visualize_map()
safe_robbie.visualize_robot()
unsafe_robbie.visualize_robot()
custom_legend = [
Line2D([0], [0], color=color_cycle[0], lw=4),
Line2D([0], [0], color=color_cycle[1], lw=4)
]
plt.legend(custom_legend, ["Safe Control", "Unsafe Control"])
# Plot Closest Distance over Time
plt.figure()
plt.plot(range(len(safe_closest_list)), safe_closest_list, label="Safe")
plt.plot(range(len(unsafe_closest_list)),
unsafe_closest_list,
label="Unsafe")
plt.legend()
plt.xlabel("Time")
plt.ylabel("Distance to Closest Obstacle (m)")
plt.show()
class Sensor(Device):
def __init__(self, env, parent, radius=10):
self.radius = radius
kp=np.array([[-radius, 0], [radius, 0]])
Device.__init__(self, env, parent, kp=kp, color=(0, 1, 0, 0.5), filled=True)
def read(self):
'''
return true if detect something.
'''
for a in self.env.agents:
if a is not self.parent and dist(self.parent, a) < self.radius:
self.geom.set_color_rgba(np.array([1, 0, 0, 0.5]))
return True
if dist(self.parent, self.env.map) < self.radius:
self.geom.set_color_rgba(np.array([1, 0, 0, 0.5]))
return True
self.geom.set_color_rgba(np.array([0, 1, 0, 0.5]))
return False
env = Simulator(config=config)
my_map = Map()
my_map.get_map_from_geom2d(env, kp=np.array([[-100, 0], [100, 0]]))
robots = [Robot(env) for i in range(5)]
while True:
t = time()
[r.update(t) for r in robots]
env._render()
def main():
print("start!!")
# load map
src_path_map = "data/two_obs.dat"
map1 = Map(src_path_map)
# initialize robot (initializes lidar with map)
robbie = Robot(map1)
for i in range(100):
print("Time " + str(i))
plt.cla()
robbie.update()
map1.visualize_map()
robbie.visualize()
plt.pause(0.1)
print("done!!")
def load_elements_from_file(self, robot_init_filename, block_init_filename):
map = Map((int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension'])))
for x, y in map.get_blocks_init_place(block_init_filename):
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init block (x, y) not in map!')
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
for x, y in map.get_robot_init_place(robot_init_filename):
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init robot (x, y) not in map!')
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
def load_elements_by_click(self, robots_sequences, blocks_sequences):
map = Map((int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension'])))
for x, y in blocks_sequences:
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init block (x, y) not in map!')
map.grid[x][y] = BLOCK_AREA
map.blocks.append((x, y))
for x, y in robots_sequences:
if not map.is_location_in_environment(x, y, (
int(self.cfg['MAP']['grid_row_dimension']), int(self.cfg['MAP']['grid_column_dimension']))):
raise OutsideBoundryError('init robot (x, y) not in map!')
map.grid[x][y] = ROBOT_AREA
map.robots.append(Robot(x, y)) # add robots
return map
from simulator import Simulator, Map, Agent
from devices import Device
import numpy as np
import simulator_config
env = Simulator(simulator_config)
map = Map()
map.get_map_from_geom2d(env,
kp=np.array([[-100, 100], [-100, -100], [100, -100],
[100, 100]]))
robot = Agent(env,
kp=np.array([[-2, 0], [2, 0]]),
color=(1, 0, 0, 0.5),
v_max=5)
robot.reset(init_state=np.array([0, 40, 0]))
device = Device(env,
parent=robot,
kp=np.array([[-10, 0], [10, 0]]),
color=[0, 1, 0, 1],
filled=False)
while True:
robot.update(v=np.array([5, 0]))
env._render()
from simulator import Simulator, Map, Agent
import numpy as np
from time import time
import simulator_config
env = Simulator(config=simulator_config)
map = Map()
map.get_map_from_geom2d(env, np.array([[-100, 0], [100, 0]]))
n_targets = 10
n_robots = 10
targets = [
Agent(env, kp=np.array([[-2, 0], [2, 0]]), color=(1, 0, 0, 0.5), v_max=1.5)
for i in range(n_targets)
]
robots = [
Agent(env, kp=np.array([[-2, 0], [2, 0]]), color=(0, 0, 1, 0.5), v_max=2)
for i in range(n_targets, n_robots + n_targets)
]
tic = time()
# set speed betwee -2 to 2 in both direction
vs = (np.random.rand(n_targets + n_robots, 2) - 0.5) * 4
while True:
env._render()
for i in range(n_targets + n_robots):
if np.random.rand() < 0.02:
vs[i] = (np.random.rand(2) - 0.5) * 4
else:
vs[i] = vs[i]
from simulator import Map, Robot
import math
def program_robot(robot, targets):
# Start of robot logic
robot.rotate_and_wait(1.0)
robot.translate_and_wait(1.0)
# End of robot logic
map = Map(1)
chacha = map.create_robot('R2D2', program_robot, Robot.COLOR_BLUE)
map.start()