Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improved reloading of OpenDRIVE maps in OSC #752

Merged
merged 1 commit into from
Apr 16, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions Docs/CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
- Added `--openscenarioparams` argument to overwrite global `ParameterDeclaration`
- Added controller using CARLA's autopilot (in replacement for ActivateControllerAction)
- Added support for storyboards with multiple stories
- Eliminated unnecessary reloads of OpenDRIVE maps
* Additional Scenarios:
- Added Construction setup scenario.
### :bug: Bug Fixes
Expand Down
23 changes: 13 additions & 10 deletions scenario_runner.py
Original file line number Diff line number Diff line change
Expand Up @@ -87,12 +87,11 @@ def __init__(self, args):
# requests in the localhost at port 2000.
self.client = carla.Client(args.host, int(args.port))
self.client.set_timeout(self.client_timeout)

self.traffic_manager = self.client.get_trafficmanager(int(self._args.trafficManagerPort))
CarlaDataProvider.set_client(self.client)

dist = pkg_resources.get_distribution("carla")
if LooseVersion(dist.version) < LooseVersion('0.9.10'):
raise ImportError("CARLA version 0.9.10 or newer required. CARLA version found: {}".format(dist))
if LooseVersion(dist.version) < LooseVersion('0.9.11'):
raise ImportError("CARLA version 0.9.11 or newer required. CARLA version found: {}".format(dist))

# Load agent if requested via command line args
# If something goes wrong an exception will be thrown by importlib (ok here)
Expand Down Expand Up @@ -177,6 +176,7 @@ def _cleanup(self):
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.world.apply_settings(settings)
self.client.get_trafficmanager(int(self._args.trafficManagerPort)).set_synchronous_mode(False)
except RuntimeError:
sys.exit(-1)

Expand Down Expand Up @@ -326,19 +326,14 @@ def _load_and_wait_for_world(self, town, ego_vehicles=None):
settings.synchronous_mode = True
settings.fixed_delta_seconds = 1.0 / self.frame_rate
self.world.apply_settings(settings)

self.traffic_manager.set_synchronous_mode(True)
self.traffic_manager.set_random_device_seed(int(self._args.trafficManagerSeed))

CarlaDataProvider.set_client(self.client)
CarlaDataProvider.set_world(self.world)
CarlaDataProvider.set_traffic_manager_port(int(self._args.trafficManagerPort))

# Wait for the world to be ready
if CarlaDataProvider.is_sync_mode():
self.world.tick()
else:
self.world.wait_for_tick()

if CarlaDataProvider.get_map().name != town and CarlaDataProvider.get_map().name != "OpenDriveMap":
print("The CARLA server uses the wrong map: {}".format(CarlaDataProvider.get_map().name))
print("This scenario requires to use map: {}".format(town))
Expand Down Expand Up @@ -366,6 +361,12 @@ def _load_and_run_scenario(self, config):
self._cleanup()
return False

CarlaDataProvider.set_traffic_manager_port(int(self._args.trafficManagerPort))
tm = self.client.get_trafficmanager(int(self._args.trafficManagerPort))
tm.set_random_device_seed(int(self._args.trafficManagerSeed))
if self._args.sync:
tm.set_synchronous_mode(True)

# Prepare scenario
print("Preparing scenario: " + config.name)
try:
Expand Down Expand Up @@ -597,6 +598,8 @@ def main():
try:
scenario_runner = ScenarioRunner(arguments)
result = scenario_runner.run()
except Exception: # pylint: disable=broad-except
traceback.print_exc()

finally:
if scenario_runner is not None:
Expand Down
26 changes: 23 additions & 3 deletions srunner/scenarioconfigs/openscenario_configuration.py
Original file line number Diff line number Diff line change
Expand Up @@ -170,25 +170,45 @@ def _set_carla_town(self):
world = self.client.get_world()
wmap = None
if world:
world.get_settings()
wmap = world.get_map()

if world is None or (wmap is not None and wmap.name != self.town):
if ".xodr" in self.town:
with open(self.town) as od_file:
data = od_file.read()
index = data.find('<OpenDRIVE>')
data = data[index:]

old_map = ""
if wmap is not None:
old_map = wmap.to_opendrive()
index = old_map.find('<OpenDRIVE>')
old_map = old_map[index:]

if data != old_map:
self.logger.warning(" Wrong OpenDRIVE map in use. Forcing reload of CARLA world")
self.client.generate_opendrive_world(str(data))
world = self.client.get_world()

vertex_distance = 2.0 # in meters
wall_height = 1.0 # in meters
extra_width = 0.6 # in meters
world = self.client.generate_opendrive_world(str(data),
carla.OpendriveGenerationParameters(
vertex_distance=vertex_distance,
wall_height=wall_height,
additional_width=extra_width,
smooth_junctions=True,
enable_mesh_visibility=True))
else:
self.logger.warning(" Wrong map in use. Forcing reload of CARLA world")
self.client.load_world(self.town)
world = self.client.get_world()

CarlaDataProvider.set_world(world)
world.wait_for_tick()
if CarlaDataProvider.is_sync_mode():
world.tick()
else:
world.wait_for_tick()
else:
CarlaDataProvider.set_world(world)

Expand Down