-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy path__main__.py
90 lines (77 loc) · 3.62 KB
/
__main__.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
import argparse
from pathlib import Path
from ads_scenario_transformer.transformer.scenario_transformer import ScenarioTransformerConfiguration, ScenarioTransformer
from ads_scenario_transformer.openscenario.openscenario_coder import OpenScenarioEncoder
def main():
parser = argparse.ArgumentParser(description="ADS Scenario Transformer")
parser.add_argument("--apollo-scenario-path",
required=True,
help="Path to Apollo scenario files.")
parser.add_argument("--apollo-map-path",
required=True,
help="Path to Apollo HD map data.")
parser.add_argument("--vector-map-path",
required=True,
help="Path to vector map data.")
parser.add_argument(
"--road-network-pcd-map-path",
required=False,
default="point_cloud.pcd",
help="RoadNetwork point cloud map path marked in scenario file.")
parser.add_argument(
"--road-network-lanelet-map-path",
required=False,
help="RoadNetwork Lanelet map path marked in scenario file.")
parser.add_argument("--output-scenario-path",
required=False,
default="./",
help="Output scenario path.")
parser.add_argument("--source-name",
required=False,
help="Input scenario source")
parser.add_argument("--obstacle-waypoint-frequency",
required=False,
default=2,
help="Intervals of obstacle waypoint")
parser.add_argument(
"--obstacle-threshold",
type=float,
default=60.0,
help="Threshold for obstacle direction change detection (in degrees).")
parser.add_argument("--disable-traffic-signal",
action="store_true",
default=False,
help="Disable processing of traffic signals.")
parser.add_argument("--use-last-position-destination",
action="store_true",
default=False,
help="Use the last known position as the destination.")
args = parser.parse_args()
configuration = ScenarioTransformerConfiguration(
apollo_scenario_path=args.apollo_scenario_path,
apollo_hd_map_path=args.apollo_map_path,
vector_map_path=args.vector_map_path,
road_network_pcd_map_path=args.road_network_pcd_map_path,
road_network_lanelet_map_path=args.road_network_lanelet_map_path,
obstacle_direction_change_detection_threshold=args.obstacle_threshold,
obstacle_waypoint_frequency_in_sec=float(
args.obstacle_waypoint_frequency),
disable_traffic_signal=args.disable_traffic_signal,
use_last_position_as_destination=args.use_last_position_destination)
transformer = ScenarioTransformer(configuration=configuration)
scenario = transformer.transform()
scenario_yaml = OpenScenarioEncoder.encode_proto_pyobject_to_yaml(
proto_pyobject=scenario, wrap_result_with_typename=False)
filename = ""
if args.source_name:
filename = args.source_name
filename = filename + "-" + Path(args.apollo_scenario_path).stem
else:
filename = Path(args.apollo_scenario_path).stem
output_path = Path(args.output_scenario_path) / (filename + ".yaml")
output_path.parent.mkdir(parents=True, exist_ok=True)
with open(output_path, 'w') as file:
file.write(scenario_yaml)
print(f"File written to {output_path}")
if __name__ == "__main__":
main()