class ImpactGenerator:
parts = [
'etk800_bumper_F',
'etk800_bumperbar_F',
'etk800_bumper_R',
'etk800_fender_L',
'etk800_fender_R',
'etk800_hood',
'etk800_towhitch',
'etk800_steer',
'etk800_radiator',
'etk800_roof_wagon',
'wheel_F_5',
]
emptyable = {
'etk800_bumperbar_F',
'etk800_towhitch',
}
wca = {
'level': 'west_coast_usa',
'a_spawn': (-270.75, 678, 74.9),
'b_spawn': (-260.25, 678, 74.9),
'pole_pos': (-677.15, 848, 75.1),
'linear_pos_a': (-630, 65, 103.4),
'linear_pos_b': (-619, 77, 102.65),
'linear_rot_b': (0, 0, 45.5),
't_pos_a': (-440, 688, 75.1),
't_pos_b': (-453, 700, 75.1),
't_rot_b': (0, 0, 315),
'ref_pos': (-18, 610, 75),
}
smallgrid = {
'level': 'smallgrid',
'a_spawn': (-270.75, 678, 0.1),
'b_spawn': (-260.25, 678, 0.1),
'pole_pos': (-677.15, 848, 0.1),
'pole': (-682, 842, 0),
'linear_pos_a': (-630, 65, 0.1),
'linear_pos_b': (-619, 77, 0.1),
'linear_rot_b': (0, 0, 45.5),
't_pos_a': (-440, 688, 0.1),
't_pos_b': (-453, 700, 0.1),
't_rot_b': (0, 0, 315),
'ref_pos': (321, 321, 0.1),
}
def __init__(self,
bng_home,
output,
config,
poolsize=2,
smallgrid=False,
sim_mtx=None,
similarity=0.5,
random_select=False,
single=False):
self.bng_home = bng_home
self.output = output
self.config = config
self.smallgrid = smallgrid
self.single = single
self.impactgen_mats = None
if smallgrid:
mats = materialmngr.get_impactgen_materials(bng_home)
self.impactgen_mats = sorted(list(mats))
self.poolsize = poolsize
self.similarity = similarity
self.sim_mtx = sim_mtx
self.random_select = random_select
self.pole_space = None
self.t_bone_space = None
self.linear_space = None
self.nocrash_space = None
self.post_space = None
self.total_possibilities = 0
self.bng = BeamNGpy('localhost', 64256, home=bng_home)
self.scenario = None
scenario_props = ImpactGenerator.wca
if smallgrid:
scenario_props = ImpactGenerator.smallgrid
self.vehicle_a = Vehicle('vehicle_a', model='etk800')
self.vehicle_b = Vehicle('vehicle_b', model='etk800')
self.scenario = Scenario(scenario_props['level'], 'impactgen')
self.scenario.add_vehicle(self.vehicle_a,
pos=scenario_props['a_spawn'],
rot=(0, 0, 0))
self.scenario.add_vehicle(self.vehicle_b,
pos=scenario_props['b_spawn'],
rot=(0, 0, 0))
self.vehicle_a_parts = defaultdict(set)
self.vehicle_a_config = None
self.vehicle_b_config = None
def generate_colors(self):
return copy.deepcopy(self.config['colors'])
def generate_nocrash_space(self, props):
nocrash_options = []
for part in ImpactGenerator.parts: # Vary each configurable part
nocrash_options.append(self.vehicle_a_parts[part])
self.nocrash_space = OptionSpace(nocrash_options)
def generate_pole_space(self, props):
pole_options = [(False, True)] # Vehicle facing forward/backward
pole_options.append(np.linspace(-0.75, 0.75, 5)) # Position offset
pole_options.append(np.linspace(0.15, 0.5, 4)) # Throttle intensity
for part in ImpactGenerator.parts: # Vary each configurable part
pole_options.append(self.vehicle_a_parts[part])
self.pole_space = OptionSpace(pole_options)
def generate_t_bone_space(self, props):
t_options = [(False, True)] # Vehicle hit left/right
t_options.append(np.linspace(-30, 30, 11)) # A rotation offset
t_options.append(np.linspace(-1.5, 1.5, 5)) # B pos. offset
t_options.append(np.linspace(0.2, 0.5, 4)) # B throttle
for part in ImpactGenerator.parts:
t_options.append(self.vehicle_a_parts[part])
self.t_bone_space = OptionSpace(t_options)
def generate_linear_space(self, props):
linear_options = [(False, True)] # Vehicle hit front/back
linear_options.append(np.linspace(-15, 15, 5)) # A rot. offset
linear_options.append(np.linspace(-1.33, 1.33, 5)) # B pos. offset
linear_options.append(np.linspace(0.25, 0.5, 4)) # B throttle
for part in ImpactGenerator.parts:
linear_options.append(self.vehicle_a_parts[part])
self.linear_space = OptionSpace(linear_options)
def get_material_options(self):
if not self.random_select:
selected = materialmngr.pick_materials(self.impactgen_mats,
self.sim_mtx,
poolsize=self.poolsize,
similarity=self.similarity)
if selected is None:
log.info('Could not find material pool through similarity. '
'Falling back to random select.')
else:
selected = random.sample(self.impactgen_mats, self.poolsize)
return selected
def generate_post_space(self):
colors = self.generate_colors()
post_options = []
post_options.append(self.config['times'])
if self.smallgrid:
post_options.append([0])
post_options.append([0])
else:
post_options.append(self.config['clouds'])
post_options.append(self.config['fogs'])
post_options.append(colors)
if self.smallgrid:
mats = self.get_material_options()
if mats is not None:
post_options.append(list(mats))
post_options.append(list(mats))
return OptionSpace(post_options)
def generate_spaces(self):
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
self.generate_nocrash_space(props)
self.generate_t_bone_space(props)
self.generate_linear_space(props)
self.generate_pole_space(props)
def scan_parts(self, parts, known=set()):
with open('out.json', 'w') as outfile:
outfile.write(json.dumps(parts, indent=4, sort_keys=True))
for part_type in ImpactGenerator.parts:
options = parts[part_type]
self.vehicle_a_parts[part_type].update(options)
def init_parts(self):
self.vehicle_a_config = self.vehicle_a.get_part_config()
self.vehicle_b_config = self.vehicle_b.get_part_config()
b_parts = self.vehicle_b_config['parts']
b_parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU'
b_parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU'
b_parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1'
options = self.vehicle_a.get_part_options()
self.scan_parts(options)
for k in self.vehicle_a_parts.keys():
self.vehicle_a_parts[k] = list(self.vehicle_a_parts[k])
if k in ImpactGenerator.emptyable:
self.vehicle_a_parts[k].append('')
def init_settings(self):
self.bng.set_particles_enabled(False)
self.generate_spaces()
log.info('%s pole crash possibilities.', self.pole_space.count)
log.info('%s T-Bone crash possibilities.', self.t_bone_space.count)
log.info('%s parallel crash possibilities.', self.linear_space.count)
log.info('%s no crash possibilities.', self.nocrash_space.count)
self.total_possibilities = \
self.pole_space.count + \
self.t_bone_space.count + \
self.linear_space.count + \
self.nocrash_space.count
log.info('%s total incidents possible.', self.total_possibilities)
def get_vehicle_config(self, setting):
parts = dict()
for idx, part in enumerate(ImpactGenerator.parts):
parts[part] = setting[idx]
refwheel = parts['wheel_F_5']
parts['wheel_R_5'] = refwheel.replace('_F', '_R')
# Force licence plate to always be German
parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU'
parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU'
parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1'
config = copy.deepcopy(self.vehicle_a_config)
config['parts'] = parts
return config
def set_annotation_paths(self):
part_path = os.path.join(self.bng_home, PART_ANNOTATIONS)
part_path = os.path.abspath(part_path)
obj_path = os.path.join(self.bng_home, OBJ_ANNOTATIONS)
obj_path = os.path.abspath(obj_path)
req = dict(type='ImpactGenSetAnnotationPaths')
req['partPath'] = part_path
req['objPath'] = obj_path
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenAnnotationPathsSet'
def set_image_properties(self):
req = dict(type='ImpactGenSetImageProperties')
req['imageWidth'] = self.config['imageWidth']
req['imageHeight'] = self.config['imageHeight']
req['colorFmt'] = self.config['colorFormat']
req['annotFmt'] = self.config['annotFormat']
req['radius'] = self.config['cameraRadius']
req['height'] = self.config['cameraHeight']
req['fov'] = self.config['fov']
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenImagePropertiesSet'
def setup(self):
self.scenario.make(self.bng)
log.debug('Loading scenario...')
self.bng.load_scenario(self.scenario)
log.debug('Setting steps per second...')
self.bng.set_steps_per_second(50)
log.debug('Enabling deterministic mode...')
self.bng.set_deterministic()
log.debug('Starting scenario...')
self.bng.start_scenario()
log.debug('Scenario started. Sleeping 20s.')
time.sleep(20)
self.init_parts()
self.init_settings()
log.debug('Setting annotation properties.')
self.set_annotation_paths()
self.set_image_properties()
def settings_exhausted(self):
return self.t_bone_space.exhausted() and \
self.linear_space.exhausted() and \
self.pole_space.exhausted() and \
self.nocrash_space.exhausted()
def set_post_settings(self, vid, settings):
req = dict(type='ImpactGenPostSettings')
req['ego'] = vid
req['time'] = settings[0]
req['clouds'] = settings[1]
req['fog'] = settings[2]
req['color'] = settings[3]
if len(settings) > 4:
req['skybox'] = settings[4]
if len(settings) > 5:
req['ground'] = settings[5]
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenPostSet'
def finished_producing(self):
req = dict(type='ImpactGenOutputGenerated')
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenZipGenerated'
return resp['state']
def produce_output(self, color_name, annot_name):
while not self.finished_producing():
time.sleep(0.2)
req = dict(type='ImpactGenGenerateOutput')
req['colorName'] = color_name
req['annotName'] = annot_name
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenZipStarted'
'ImpactGenZipStarted'
def capture_post(self, crash_setting):
log.info('Enumerating post-crash settings and capturing output.')
self.bng.switch_vehicle(self.vehicle_a)
ref_pos = ImpactGenerator.wca['ref_pos']
if self.smallgrid:
ref_pos = ImpactGenerator.smallgrid['ref_pos']
self.bng.teleport_vehicle(self.vehicle_a, ref_pos)
self.bng.teleport_vehicle(self.vehicle_b, (10000, 10000, 10000))
self.bng.step(50, wait=True)
self.bng.pause()
self.post_space = self.generate_post_space()
while not self.post_space.exhausted():
post_setting = self.post_space.sample_new()
scenario = [[str(s) for s in crash_setting]]
scenario.append([str(s) for s in post_setting])
key = str(scenario).encode('ascii')
key = hashlib.sha512(key).hexdigest()[:30]
t = int(time.time())
color_name = '{}_{}_0_image.zip'.format(t, key)
annot_name = '{}_{}_0_annotation.zip'.format(t, key)
color_name = os.path.join(self.output, color_name)
annot_name = os.path.join(self.output, annot_name)
log.info('Setting post settings.')
self.set_post_settings(self.vehicle_a.vid, post_setting)
log.info('Producing output.')
self.produce_output(color_name, annot_name)
if self.single:
break
self.bng.resume()
def run_t_bone_crash(self):
log.info('Running t-bone crash setting.')
if self.t_bone_space.exhausted():
log.debug('T-Bone crash setting exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.t_bone_space.sample_new()
side, angle, offset, throttle = setting[:4]
config = setting[4:]
config = self.get_vehicle_config(config)
if side:
angle += 225
else:
angle += 45
pos_a = props['t_pos_a']
rot_b = props['t_rot_b']
pos_b = list(props['t_pos_b'])
pos_b[0] += offset
req = dict(type='ImpactGenRunTBone')
req['ego'] = self.vehicle_a.vid
req['other'] = self.vehicle_b.vid
req['config'] = config
req['aPosition'] = pos_a
req['angle'] = angle
req['bPosition'] = pos_b
req['bRotation'] = rot_b
req['throttle'] = throttle
log.debug('Sending t-bone crash config.')
self.bng.send(req)
log.debug('T-Bone crash response received.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenTBoneRan'
return setting
def run_linear_crash(self):
log.info('Running linear crash setting.')
if self.linear_space.exhausted():
log.debug('Linear crash settings exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.linear_space.sample_new()
back, angle, offset, throttle = setting[:4]
config = setting[4:]
config = self.get_vehicle_config(config)
if back:
angle += 225
else:
offset += 1.3
angle += 45
pos_a = props['linear_pos_a']
rot_b = props['linear_rot_b']
pos_b = list(props['linear_pos_b'])
pos_b[0] += offset
req = dict(type='ImpactGenRunLinear')
req['ego'] = self.vehicle_a.vid
req['other'] = self.vehicle_b.vid
req['config'] = config
req['aPosition'] = pos_a
req['angle'] = angle
req['bPosition'] = pos_b
req['bRotation'] = rot_b
req['throttle'] = throttle
log.debug('Sending linear crash config.')
self.bng.send(req)
log.debug('Linear crash response received.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenLinearRan'
return setting
def run_pole_crash(self):
log.info('Running pole crash setting.')
if self.pole_space.exhausted():
log.debug('Pole crash settings exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.pole_space.sample_new()
back, offset, throttle = setting[:3]
config = setting[3:]
config = self.get_vehicle_config(config)
angle = 45
if back:
angle = 225
offset += 0.85
throttle = -throttle
pos = list(props['pole_pos'])
pos[0] += offset
req = dict(type='ImpactGenRunPole')
req['ego'] = self.vehicle_a.vid
req['config'] = config
req['position'] = pos
req['angle'] = angle
req['throttle'] = throttle
if self.smallgrid:
req['polePosition'] = props['pole']
log.debug('Sending pole crash config.')
self.bng.send(req)
log.debug('Got pole crash response.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenPoleRan'
return setting
def run_no_crash(self):
log.info('Running non-crash scenario.')
if self.nocrash_space.exhausted():
return None
setting = self.nocrash_space.sample_new()
log.info('Got new setting: %s', setting)
vehicle_config = self.get_vehicle_config(setting)
log.info('Got new vehicle config: %s', vehicle_config)
req = dict(type='ImpactGenRunNonCrash')
req['ego'] = self.vehicle_a.vid
req['config'] = vehicle_config
log.info('Sending Non-Crash request: %s', req)
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenNonCrashRan'
log.info('Non-crash finished.')
return setting
def run_incident(self, incident):
log.info('Setting up next incident.')
self.bng.display_gui_message('Setting up next incident...')
setting = incident()
self.capture_post(setting)
return setting
def run_incidents(self):
log.info('Enumerating possible incidents.')
count = 1
incidents = [
self.run_t_bone_crash,
self.run_linear_crash,
self.run_pole_crash,
self.run_no_crash,
]
while not self.settings_exhausted():
log.info('Running incident %s of %s...', count,
self.total_possibilities)
self.bng.restart_scenario()
log.info('Scenario restarted.')
time.sleep(5.0)
self.vehicle_b.set_part_config(self.vehicle_b_config)
log.info('Vehicle B config set.')
incident = incidents[count % len(incidents)]
if self.run_incident(incident) is None:
log.info('Ran out of options for: %s', incident)
incidents.remove(incident) # Possibility space exhausted
count += 1
def run(self):
log.info('Starting up BeamNG instance.')
self.bng.open(['impactgen/crashOutput'])
self.bng.skt.settimeout(1000)
try:
log.info('Setting up BeamNG instance.')
self.setup()
self.run_incidents()
finally:
log.info('Closing BeamNG instance.')
self.bng.close()
def main() -> None:
# Read CommonRoad scenario
cr_scenario, cr_planning_problem_set = CommonRoadFileReader(cr_scenario_path) \
.open()
if cr_planning_problem_set:
for _, pp in cr_planning_problem_set.planning_problem_dict.items():
cr_planning_problem = pp
break # Read only the first one
else:
raise Exception(
"The given CommonRoad scenario does not define a planning problem."
)
# Setup BeamNG
bng = BeamNGpy('localhost', 64256, home=home_path, user=user_path)
bng_scenario = Scenario(
bng_scenario_environment,
bng_scenario_name,
authors='Stefan Huber',
description='Simple visualization of the CommonRoad scenario ' +
cr_scenario_name)
# Add lane network
lanes = cr_scenario.lanelet_network.lanelets
for lane in lanes:
lane_nodes = []
for center in lane.center_vertices:
lane_nodes.append((center[0], center[1], 0,
4)) # FIXME Determine appropriate width
road = Road(cr_road_material)
road.nodes.extend(lane_nodes)
bng_scenario.add_road(road)
# Add ego vehicle
ego_vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='EGO',
color='Cornflowerblue')
ego_init_state = cr_planning_problem.initial_state
ego_init_state.position[0] = 82.8235
ego_init_state.position[1] = 31.5786
add_vehicle_to_bng_scenario(bng_scenario, ego_vehicle, ego_init_state,
etk800_z_offset)
obstacles_to_move = dict()
# Add truck
semi = Vehicle('truck', model='semi', color='Red')
semi_init_state = cr_scenario.obstacle_by_id(206).initial_state
add_vehicle_to_bng_scenario(bng_scenario, semi, semi_init_state,
semi_z_offset)
obstacles_to_move[206] = semi
# Add truck trailer
tanker_init_state = copy(semi_init_state)
tanker_init_state.position += [6, 3]
add_vehicle_to_bng_scenario(
bng_scenario, Vehicle('truck_trailer', model='tanker', color='Red'),
tanker_init_state, tanker_z_offset)
# Add other traffic participant
opponent = Vehicle('opponent',
model='etk800',
licence='VS',
color='Cornflowerblue')
add_vehicle_to_bng_scenario(bng_scenario, opponent,
cr_scenario.obstacle_by_id(207).initial_state,
etk800_z_offset)
obstacles_to_move[207] = opponent
# Add static obstacle
obstacle_shape: Polygon = cr_scenario.obstacle_by_id(399).obstacle_shape
obstacle_pos = obstacle_shape.center
obstacle_rot_deg = rad2deg(semi_init_state.orientation) + rot_offset
obstacle_rot_rad = semi_init_state.orientation + deg2rad(rot_offset)
for w in range(3):
for h in range(3):
for d in range(2):
obstacle = Vehicle('obstacle' + str(w) + str(h) + str(d),
model='haybale',
color='Red')
haybale_pos_diff = obstacle_pos \
+ pol2cart(1.3 * d, obstacle_rot_rad + pi / 4) \
+ pol2cart(2.2 * w, pi / 2 - obstacle_rot_rad)
bng_scenario.add_vehicle(obstacle,
pos=(haybale_pos_diff[0],
haybale_pos_diff[1], h * 1),
rot=(0, 0, obstacle_rot_deg))
bng_scenario.make(bng)
# Manually set the overObjects attribute of all roads to True (Setting this option is currently not supported)
prefab_path = os.path.join(user_path, 'levels', bng_scenario_environment,
'scenarios', bng_scenario_name + '.prefab')
lines = open(prefab_path, 'r').readlines()
for i in range(len(lines)):
if 'overObjects' in lines[i]:
lines[i] = lines[i].replace('0', '1')
open(prefab_path, 'w').writelines(lines)
# Start simulation
bng.open(launch=True)
try:
bng.load_scenario(bng_scenario)
bng.start_scenario()
bng.pause()
bng.display_gui_message(
"The scenario is fully prepared and paused. You may like to position the camera first."
)
delay_to_resume = 15
input("Press enter to continue the simulation... You have " +
str(delay_to_resume) +
" seconds to switch back to the BeamNG window.")
sleep(delay_to_resume)
bng.resume()
for id, obstacle in obstacles_to_move.items():
obstacle.ai_drive_in_lane(False)
obstacle.ai_set_line(
generate_node_list(cr_scenario.obstacle_by_id(id)))
ego_vehicle.ai_drive_in_lane(False)
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='limit')
speed = 65 / 3.6
ego_vehicle.ai_set_line([{
'pos': ego_vehicle.state['pos']
}, {
'pos': (129.739, 56.907, etk800_z_offset),
'speed': speed
}, {
'pos': (139.4, 62.3211, etk800_z_offset),
'speed': speed
}, {
'pos': (149.442, 64.3655, etk800_z_offset),
'speed': speed
}, {
'pos': (150.168, 63.3065, etk800_z_offset),
'speed': speed
}, {
'pos': (188.495, 78.8517, etk800_z_offset),
'speed': speed
}])
# FIXME Setting the speed does not work as expected
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='set')
input("Press enter to end simulation...")
finally:
bng.close()
class Driver:
vehicle = None
bng = None
labeler = None
left_path = None
right_path = None
driving_path = None
road_profiler = None
car_model = None
road_model = None
def __init__(self, car_model: dict, road_model: dict, control_model: dict):
self.car_model = car_model
self.road_model = road_model
self.control_model = control_model
# Note: Speed limit must be m/s
self.road_profiler = RoadProfiler(
road_model['mu'], road_model['speed_limit'] / 3.6,
control_model['discretization_factor'])
def _compute_driving_path(self, car_state, road_name):
road_geometry = self.bng.get_road_edges(road_name)
left_edge_x = np.array([e['left'][0] for e in road_geometry])
left_edge_y = np.array([e['left'][1] for e in road_geometry])
right_edge_x = np.array([e['right'][0] for e in road_geometry])
right_edge_y = np.array([e['right'][1] for e in road_geometry])
road_edges = dict()
road_edges['left_edge_x'] = left_edge_x
road_edges['left_edge_y'] = left_edge_y
road_edges['right_edge_x'] = right_edge_x
road_edges['right_edge_y'] = right_edge_y
self.right_edge = LineString(
zip(road_edges['right_edge_x'][::-1],
road_edges['right_edge_y'][::-1]))
self.left_edge = LineString(
zip(road_edges['left_edge_x'], road_edges['left_edge_y']))
current_position = Point(car_state['pos'][0], car_state['pos'][1])
from shapely.ops import nearest_points
from shapely.affinity import rotate
projection_point_on_right = nearest_points(self.right_edge,
current_position)[0]
projection_point_on_left = nearest_points(self.left_edge,
current_position)[0]
# If the car is closest to the left, then we need to switch the direction of the road...
if current_position.distance(
projection_point_on_right) > current_position.distance(
projection_point_on_left):
# Swap the axis and recompute the projection points
l.debug("Reverse traffic direction")
temp = self.right_edge
self.right_edge = self.left_edge
self.left_edge = temp
del temp
projection_point_on_right = nearest_points(self.right_edge,
current_position)[0]
projection_point_on_left = nearest_points(self.left_edge,
current_position)[0]
# Traffic direction is always 90-deg counter clockwise from right
# Now rotate right point 90-deg counter clockwise from left and we obtain the traffic direction
rotated_right = rotate(projection_point_on_right,
90.0,
origin=projection_point_on_left)
# Vector defining the direction of the road
traffic_direction = np.array([
rotated_right.x - projection_point_on_left.x,
rotated_right.y - projection_point_on_left.y
])
# Find the segment containing the projection of current location
# Starting point on right edge
start_point = None
for pair in pairs(list(self.right_edge.coords[:])):
segment = LineString([pair[0], pair[1]])
# xs, ys = segment.coords.xy
# plt.plot(xs, ys, color='green')
if segment.distance(projection_point_on_right) < 1.8e-5:
road_direction = np.array(
[pair[1][0] - pair[0][0], pair[1][1] - pair[0][1]])
if dot(traffic_direction, road_direction) < 0:
l.debug("Reverse order !")
self.right_edge = LineString([
Point(p[0], p[1]) for p in self.right_edge.coords[::-1]
])
start_point = Point(pair[0][0], pair[0][1])
break
else:
l.debug("Original order !")
start_point = Point(pair[1][0], pair[1][1])
break
assert start_point is not None
# At this point compute the driving path of the car (x, y, t)
self.driving_path = [current_position]
# plt.plot(current_position.x, current_position.y, color='black', marker="x")
# # This might not be robust we need to get somethign close by
# plt.plot([pair[0][0], pair[1][0]], [pair[0][1], pair[1][1]], marker="o")
# plt.plot(projection_point_on_right.x, projection_point_on_right.y, color='b', marker="*")
#
started = False
for right_position in [
Point(p[0], p[1]) for p in list(self.right_edge.coords)
]:
if right_position.distance(start_point) < 1.8e-5:
# print("Start to log positions")
# plt.plot(right_position.x, right_position.y, color='blue', marker="o")
started = True
if not started:
# print("Skip point")
# plt.plot(right_position.x, right_position.y, color='red', marker="*")
continue
else:
# print("Consider point")
# plt.plot(right_position.x, right_position.y, color='green', marker="o")
pass
# Project right_position to left_edge
projected_point = self.left_edge.interpolate(
self.left_edge.project(right_position))
# Translate the right_position 2m toward the center
line = LineString([(right_position.x, right_position.y),
(projected_point.x, projected_point.y)])
self.driving_path.append(line.interpolate(2.0))
def plot_all(self, car_state):
current_position = Point(car_state['pos'][0], car_state['pos'][1])
plt.figure(1, figsize=(5, 5))
plt.clf()
ax = plt.gca()
x, y = self.left_edge.coords.xy
ax.plot(x, y, 'r-')
x, y = self.right_edge.coords.xy
ax.plot(x, y, 'b-')
driving_lane = LineString([p for p in self.driving_path])
x, y = driving_lane.coords.xy
ax.plot(x, y, 'g-')
# node = {
# 'x': target_position.x,
# 'y': target_position.y,
# 'z': 0.3,
# 't': target_time,
# }
xs = [node['x'] for node in self.script]
ys = [node['y'] for node in self.script]
# print("{:.2f}".format(3.1415926));
vs = ['{:.2f}'.format(node['avg_speed'] * 3.6) for node in self.script]
# plt.plot(xs, ys, marker='.')
ax = plt.gca()
for i, txt in enumerate(vs):
ax.annotate(txt, (xs[i], ys[i]))
plt.plot(current_position.x,
current_position.y,
marker="o",
color="green")
# Center around current_positions
ax.set_xlim([current_position.x - 50, current_position.x + 50])
ax.set_ylim([current_position.y - 50, current_position.y + 50])
plt.draw()
plt.pause(0.01)
def run(self, debug=False):
try:
self.vehicle = Vehicle(car_model['id'])
electrics = Electrics()
self.vehicle.attach_sensor('electrics', electrics)
# Connect to running beamng
self.bng = BeamNGpy(
'localhost', 64256
) #, home='C://Users//Alessio//BeamNG.research_unlimited//trunk')
self.bng = self.bng.open(launch=False)
# Put simulator in pause awaiting while planning the driving
self.bng.pause()
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
self.bng.set_deterministic() # Set simulator to be deterministic
self.bng.connect_vehicle(self.vehicle)
assert self.vehicle.skt
# Get Initial state of the car. This assumes that the script is invoked after the scenario is started
self.bng.poll_sensors(self.vehicle)
# Compute the "optimal" driving path and program the ai_script
self._compute_driving_path(self.vehicle.state,
self.road_model['street'])
self.script = self.road_profiler.compute_ai_script(
LineString(self.driving_path), self.car_model)
# Enforce initial car direction nad up
start_dir = (self.vehicle.state['dir'][0],
self.vehicle.state['dir'][1],
self.vehicle.state['dir'][2])
up_dir = (0, 0, 1)
# Configure the ego car
self.vehicle.ai_set_mode('disabled')
# Note that set script teleports the car by default
self.vehicle.ai_set_script(self.script,
start_dir=start_dir,
up_dir=up_dir)
# Resume the simulation
self.bng.resume()
# At this point the controller can stop ? or wait till it is killed
while True:
if debug:
self.bng.pause()
self.bng.poll_sensors(self.vehicle)
self.plot_all(self.vehicle.state)
self.bng.resume()
# Progress the simulation for some time...
# self.bng.step(50)
sleep(2)
except Exception:
# When we brutally kill this process there's no need to log an exception
l.error("Fatal Error", exc_info=True)
finally:
self.bng.close()
