diff --git a/bag2lanelet/README.md b/bag2lanelet/README.md
new file mode 100644
index 000000000..f1a5ffc5b
--- /dev/null
+++ b/bag2lanelet/README.md
@@ -0,0 +1,95 @@
+# bag2lanelet
+
+This package generates a lanelet map necessary for Autoware's autonomous driving from rosbag data containing information about Localization (`/tf`). This enables autonomous driving based on manual driving information.
+
+The provided functionalities are as follows:
+
+- bag2lanelet.py: Generates lanelet (.osm) from a rosbag based on the position of `base_link`.
+- bag2trajectory.py: Generates trajectory information (.csv) for vector_map_builder from a rosbag.
+
+## Example
+
+
+As an example, the process of lanelet generation based on driving trajectories from the planning simulator is performed as follows. Typically, the expectation is to use rosbag data from manual driving, rather than from the planning simulator.
+
+
+Firstly, you need to run the planning_simulator following the [planning_simulator tutorial in Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/). The process would be, install Autoawre, download the maps, run the planning_simulator, and start autonomous driving. Make sure to save the rosbag during this driving session using the following command:
+
+```sh
+ros2 bag record /tf -o /tmp/bag2lanelet_sample.bag
+```
+
+
+
+
+ 
+
+
+
+
+After completing the drive, you can run the `bag2lanelet.py` script. This requires specifying the output directory, lane width and MGRS coordinates:
+
+```sh
+./bag2lanelet.py /tmp/bag2lanelet_sample.bag /tmp/bag2lanelet_sample -l 3.0 -m 54SUE
+```
+
+The map will be saved in the specified directory, following the naming convention `-lanelet2_map.osm`. The map generated will appear like this. You can see the example result in [./example/lanelet2_map.osm](./example/lanelet2_map.osm).
+
+When you relaunch the planning_simulator with the new lanelet2 map, you will see the following.
+
+
+ 
+
+
+
+Please note that at this stage, although this map works with Autoware, the shape of the lanes will appear jagged. (Refer to the 'Limitations' section for more details.) While this is an issue that should be addressed in the future, it can currently be resolved by loading it in [Vector Map Builder](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-maps/creating-vector-map/#vector-map-builder) as follows.
+
+
+Following the documentation of the [Vector Map Builder](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-maps/creating-vector-map/#vector-map-builder), import the generated Lanelet2 map. You can see the refined lane on the application.
+
+
+ 
+
+
+Then, Export the map. You can run the planning_simulator with the refined lanelet2 map and see how it goes on the Rviz.
+
+
+ 
+
+
+
+## Requirements
+
+```
+sudo apt update
+sudo apt install ros-humble-tf-transformations ros-humble-tf-transformations
+pip install -r requirements.txt
+```
+
+## Usage
+
+Check `./bag2lanelet.py --help`
+
+### generate lanelet2 file
+
+```
+./bag2lanelet.py /home/autoware/rosbag/sample . --width=3.0
+```
+or with MGRS code at Monza Track
+
+```
+./bag2lanelet.py /home/autoware/rosbag/sample . --width=3.0 --mgrs 32TNR219517
+```
+
+### generate trajectory file for Vector Map Builder
+
+```
+./bag2trajectory.py /home/autoware/rosbag/sample sample.csv
+```
+
+
+## Limitations
+
+Here is the limitations of this package. Contributions to further improvements are more than welcome.
+
+- Due to the low conversion accuracy from MGRS to latitude and longitude in this script, the lanes in the output lanelet.osm appear jagged. Importing and then exporting through vector_map_builder corrects these values with high accuracy.
\ No newline at end of file
diff --git a/bag2lanelet/bag2lanelet.py b/bag2lanelet/bag2lanelet.py
new file mode 100755
index 000000000..3576c31b4
--- /dev/null
+++ b/bag2lanelet/bag2lanelet.py
@@ -0,0 +1,56 @@
+#!/bin/env python3
+import argparse
+import os
+import pathlib
+from datetime import datetime
+
+from bag2way import bag2pose, pose2line
+from lanelet_xml import LaneletMap
+
+
+def genarate(input_path, output, width, mgrs, interval, offset, use_centerline=False):
+ pose_array = bag2pose(input_path, interval)
+ pose_array = pose_array[::10]
+ left, right, center = pose2line(pose_array, width=width, offset=offset)
+
+ m = LaneletMap(mgrs=mgrs)
+ left_nodes = [m.add_node(*node) for node in left]
+ right_nodes = [m.add_node(*node) for node in right]
+ center_nodes = [m.add_node(*node) for node in center]
+
+ left_line = m.add_way(left_nodes)
+ right_line = m.add_way(right_nodes)
+ center_line = m.add_way(center_nodes) if use_centerline else None
+
+ m.add_relation(left_line, right_line, center_line)
+ os.makedirs(output, exist_ok=True) if not os.path.isdir(output) else None
+ m.save(
+ str(output) + "/" + datetime.now().strftime("%y-%m-%d-%H-%M-%S") + "-" + "lanelet2_map.osm"
+ )
+
+
+def main():
+ parser = argparse.ArgumentParser(description="Create lanelet2 file from rosbag2")
+ parser.add_argument("input_bag", help="input bag stored path")
+ parser.add_argument("output_lanelet", help="output lanelet2 save path")
+ parser.add_argument("-l", "--width", type=float, default=2.0, help="lane width[m]")
+ parser.add_argument("-m", "--mgrs", default="54SUE", help="MGRS code")
+ parser.add_argument(
+ "--interval", type=float, nargs=2, default=[0.1, 2.0], help="min and max interval between tf position")
+ parser.add_argument(
+ "--offset", type=float, nargs=3, default=[0.0, 0.0, 0.0], help="offset[m] from base_link"
+ )
+ parser.add_argument("--center", action="store_true", help="add centerline to lanelet")
+
+ args = parser.parse_args()
+ input_path = pathlib.Path(args.input_bag)
+ if not input_path.exists():
+ raise FileNotFoundError("Input bag folder '{}' is not found.".format(input_path))
+ output_path = pathlib.Path(args.output_lanelet)
+
+ print(args)
+ genarate(input_path, output_path, args.width, args.mgrs, args.interval, args.offset, args.center)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/bag2lanelet/bag2map.py b/bag2lanelet/bag2map.py
new file mode 100755
index 000000000..8ca3d29d4
--- /dev/null
+++ b/bag2lanelet/bag2map.py
@@ -0,0 +1,47 @@
+#!/bin/env python3
+import argparse
+import os
+import pathlib
+from datetime import datetime
+
+import folium
+from bag2way import bag2point_stamped, bag2pose, pose2line
+from lanelet_xml import LaneletMap
+
+
+def genarate(input_path, output, mgrs):
+ point_array = bag2point_stamped(input_path, 40.0, 500.0)
+ m = LaneletMap(mgrs=mgrs)
+ latlon = [{'lat': lat, 'lon': lon} for lat, lon in [m.get_latlon(*node) for node in point_array[:, 1:4]]]
+ t = point_array[:, 0]
+
+ f = folium.Figure(width=800, height=500)
+ map = folium.Map(location=[latlon[0]["lat"], latlon[0]["lon"]], zoom_start=20).add_to(f)
+ idx = 0
+ # Note: latlon and t should be same index size
+ for ll in latlon:
+ print(ll["lat"], ll["lon"], t[idx])
+ folium.Marker(location=[ll["lat"], ll["lon"]], popup=t[idx]).add_to(map)
+ idx += 1
+ os.makedirs(output, exist_ok=True) if not os.path.isdir(output) else None
+ f.save(str(output) + "/" + datetime.now().strftime("%y-%m-%d-%H-%M-%S") + "-" + "map.html")
+ map.show_in_browser()
+
+
+def main():
+ parser = argparse.ArgumentParser(description="Create lanelet2 file from rosbag2")
+ parser.add_argument("input_bag", help="input bag stored path")
+ parser.add_argument("output_dir", default="", help="output html store dir")
+ parser.add_argument("-m", "--mgrs", default="54SUE", help="MGRS code")
+
+ args = parser.parse_args()
+ input_path = pathlib.Path(args.input_bag)
+ if not input_path.exists():
+ raise FileNotFoundError("Input bag folder '{}' is not found.".format(input_path))
+
+ print(input_path)
+ genarate(input_path, args.output_dir, args.mgrs)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/bag2lanelet/bag2trajectory.py b/bag2lanelet/bag2trajectory.py
new file mode 100755
index 000000000..fb4e043f1
--- /dev/null
+++ b/bag2lanelet/bag2trajectory.py
@@ -0,0 +1,39 @@
+#!/bin/env python3
+import argparse
+import pathlib
+
+import numpy as np
+import tf_transformations
+from bag2way import bag2pose
+
+
+def genarate(input_path, output_path):
+ pose_array = bag2pose(input_path)
+ pose_array = pose_array[::50]
+
+ trajectory = []
+ for pose in pose_array:
+ euler = tf_transformations.euler_from_quaternion(pose[3:])
+ trajectory.append([*pose[:3], euler[2]])
+ np.savetxt(
+ str(output_path), trajectory, fmt="%.3f", delimiter=",", header="x,y,z,yaw", comments=""
+ )
+
+
+def main():
+ parser = argparse.ArgumentParser(description="Create lanelet2 file from rosbag2")
+ parser.add_argument("input_bag", help="input bag stored path")
+ parser.add_argument("output_csv", help="output trajectory csv path")
+
+ args = parser.parse_args()
+ input_path = pathlib.Path(args.input_bag)
+ if not input_path.exists():
+ raise FileNotFoundError("Input bag folder '{}' is not found.".format(input_path))
+ output_path = pathlib.Path(args.output_csv)
+
+ print(input_path, output_path)
+ genarate(input_path, output_path)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/bag2lanelet/bag2way.py b/bag2lanelet/bag2way.py
new file mode 100755
index 000000000..e77f69322
--- /dev/null
+++ b/bag2lanelet/bag2way.py
@@ -0,0 +1,119 @@
+from datetime import datetime
+from typing import Text
+
+import numpy as np
+import tf_transformations
+from rclpy.serialization import deserialize_message
+from rosbag2_py import ConverterOptions, SequentialReader, StorageOptions
+from rosidl_runtime_py.utilities import get_message
+
+
+def create_reader(bag_dir: str) -> SequentialReader:
+ storage_options = StorageOptions(
+ uri=bag_dir,
+ storage_id="sqlite3",
+ )
+ converter_options = ConverterOptions(
+ input_serialization_format="cdr",
+ output_serialization_format="cdr",
+ )
+
+ reader = SequentialReader()
+ reader.open(storage_options, converter_options)
+ return reader
+
+# too close & outlier pose filter
+def is_close_pose(p0, p1, eps, thresh):
+ dist = ((p0.x - p1.x) ** 2 + (p0.y - p1.y) ** 2 + (p0.z - p1.z) ** 2) ** 0.5
+ if dist < eps or thresh < dist:
+ return True
+ else:
+ return False
+
+
+def bag2pose(input_path, interval=[0.1, 10000.0]):
+ reader = create_reader(str(input_path))
+ type_map = {}
+ for topic_type in reader.get_all_topics_and_types():
+ type_map[topic_type.name] = topic_type.type
+
+ pose_list = []
+ is_initial_pose = True
+ prev_trans = None
+ # read topic and fix timestamp if lidar, and write
+ while reader.has_next():
+ (topic, data, stamp) = reader.read_next()
+ if topic == "/tf":
+ msg_type = get_message(type_map[topic])
+ msg = deserialize_message(data, msg_type)
+ for transform in msg.transforms:
+ if transform.child_frame_id != "base_link":
+ continue
+ trans = transform.transform.translation
+ rot = transform.transform.rotation
+ if is_initial_pose:
+ is_initial_pose = False
+ prev_trans = trans
+ elif is_close_pose(prev_trans, trans, interval[0], interval[1]):
+ # print("too close or too far")
+ continue
+ pose_list.append(np.r_[trans.x, trans.y, trans.z, rot.x, rot.y, rot.z, rot.w])
+ prev_trans = trans
+
+ return np.array(pose_list)
+
+
+def bag2point_stamped(input_path, too_close, too_far):
+ reader = create_reader(str(input_path))
+ type_map = {}
+ for topic_type in reader.get_all_topics_and_types():
+ type_map[topic_type.name] = topic_type.type
+
+ pose_list = []
+ is_initial_pose = True
+ prev_trans = None
+ # read topic and fix timestamp if lidar, and write
+ while reader.has_next():
+ (topic, data, stamp) = reader.read_next()
+ if topic == "/tf":
+ msg_type = get_message(type_map[topic])
+ msg = deserialize_message(data, msg_type)
+ for transform in msg.transforms:
+ if transform.child_frame_id != "base_link":
+ continue
+ trans = transform.transform.translation
+ rot = transform.transform.rotation
+ date_time = datetime.fromtimestamp(stamp * 1e-9)
+ if is_initial_pose:
+ is_initial_pose = False
+ prev_trans = trans
+ elif is_close_pose(prev_trans, trans, too_close, too_far):
+ # print("too close or too far")
+ continue
+ pose_list.append(np.r_[date_time, trans.x, trans.y, trans.z])
+ prev_trans = trans
+
+ return np.array(pose_list)
+
+
+def pose2line(pose_array, width=3.0, offset=[0, 0, 0]):
+ pos = pose_array[:, 0:3]
+ rot = pose_array[:, 3:7]
+ left = []
+ right = []
+ center = []
+ for p, r in zip(pos, rot):
+ lat = tf_transformations.quaternion_multiply(
+ tf_transformations.quaternion_multiply(r, [0, width / 2.0, 0, 0]),
+ tf_transformations.quaternion_conjugate(r),
+ )[:3]
+
+ off = tf_transformations.quaternion_multiply(
+ tf_transformations.quaternion_multiply(r, [*offset, 0]),
+ tf_transformations.quaternion_conjugate(r),
+ )[:3]
+
+ left.append(p + off + lat)
+ right.append(p + off - lat)
+ center.append(p + off)
+ return left, right, center
diff --git a/bag2lanelet/example/lanelet2_map.osm b/bag2lanelet/example/lanelet2_map.osm
new file mode 100644
index 000000000..d8a0761db
--- /dev/null
+++ b/bag2lanelet/example/lanelet2_map.osm
@@ -0,0 +1,1634 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/bag2lanelet/image/psim-kashiwanoha-bag2lanelet-original-map.png b/bag2lanelet/image/psim-kashiwanoha-bag2lanelet-original-map.png
new file mode 100644
index 000000000..2a1bcfc30
Binary files /dev/null and b/bag2lanelet/image/psim-kashiwanoha-bag2lanelet-original-map.png differ
diff --git a/bag2lanelet/image/psim-kashiwanoha-bag2lanelet-vmb-map.png b/bag2lanelet/image/psim-kashiwanoha-bag2lanelet-vmb-map.png
new file mode 100644
index 000000000..29ab5f2a9
Binary files /dev/null and b/bag2lanelet/image/psim-kashiwanoha-bag2lanelet-vmb-map.png differ
diff --git a/bag2lanelet/image/psim-kashiwanoha-sample-map.png b/bag2lanelet/image/psim-kashiwanoha-sample-map.png
new file mode 100644
index 000000000..6d8e510a6
Binary files /dev/null and b/bag2lanelet/image/psim-kashiwanoha-sample-map.png differ
diff --git a/bag2lanelet/image/vmb-kashiwanoha-bag2lanelet-map.png b/bag2lanelet/image/vmb-kashiwanoha-bag2lanelet-map.png
new file mode 100644
index 000000000..4dfe6bc7d
Binary files /dev/null and b/bag2lanelet/image/vmb-kashiwanoha-bag2lanelet-map.png differ
diff --git a/bag2lanelet/lanelet_xml.py b/bag2lanelet/lanelet_xml.py
new file mode 100755
index 000000000..eff949630
--- /dev/null
+++ b/bag2lanelet/lanelet_xml.py
@@ -0,0 +1,89 @@
+import xml.etree.ElementTree as ET
+from xml.dom.minidom import parseString
+from mgrspy import mgrs
+
+
+class LaneletMap:
+ def __init__(self, mgrs="53SPU"):
+ self.mgrs = mgrs
+ self.element_num = 0
+ self.node_list = []
+ self.way_list = []
+ self.relation_list = []
+
+ self.root = ET.Element("osm", {"generator": "bag2lanelet"})
+ self.meta = ET.SubElement(
+ self.root, "MetaInfo", {"format_version": "1", "map_version": "2"}
+ )
+
+ def get_latlon(self, x, y, z):
+ mgrs_code = self.mgrs + "%05d%05d" % (x, y)
+ latlon = mgrs.toWgs(mgrs_code)
+ return latlon
+
+ def add_node(self, x, y, z):
+ self.element_num += 1
+ mgrs_code = self.mgrs + "%05d%05d" % (x, y)
+ print(mgrs_code)
+ latlon = mgrs.toWgs(mgrs_code)
+ mgrs_code = self.mgrs + ("%05d" % x)[:3] + ("%05d" % y)[:3]
+ node = ET.SubElement(self.root, 'node', {'id': str(self.element_num), 'lat': str(latlon[0]), 'lon': str(latlon[1])})
+ tag = [
+ {"k": "type", "v": ""},
+ {"k": "subtype", "v": ""},
+ {"k": "mgrs_code", "v": mgrs_code},
+ {"k": "local_x", "v": str(x)},
+ {"k": "local_y", "v": str(y)},
+ {"k": "ele", "v": str(z)},
+ ]
+ for t in tag:
+ ET.SubElement(node, "tag", t)
+
+ return self.element_num
+
+ def add_way(self, node_list):
+ self.element_num += 1
+
+ way = ET.SubElement(self.root, "way", {"id": str(self.element_num)})
+ tag = [
+ {"k": "type", "v": "line_thin"},
+ {"k": "subtype", "v": "solid"},
+ ]
+
+ for nd in node_list:
+ ET.SubElement(way, "nd", {"ref": str(nd)})
+
+ for t in tag:
+ ET.SubElement(way, "tag", t)
+
+ return self.element_num
+
+ def add_relation(self, left_id, right_id, center_id=None):
+ self.element_num += 1
+ relation = ET.SubElement(self.root, "relation", {"id": str(self.element_num)})
+
+ ET.SubElement(relation, "member", {"type": "way", "ref": str(left_id), "role": "left"})
+ ET.SubElement(relation, "member", {"type": "way", "ref": str(right_id), "role": "right"})
+
+ if center_id:
+ ET.SubElement(
+ relation, "member", {"type": "way", "ref": str(center_id), "role": "centerline"}
+ )
+
+ tag = [
+ {"k": "type", "v": "lanelet"},
+ {"k": "subtype", "v": "road"},
+ {"k": "location", "v": "urban"},
+ {"k": "participant:vehicle", "v": "yes"},
+ {"k": "one_way", "v": "yes"},
+ {"k": "speed_limit", "v": str(20)},
+ ]
+ for t in tag:
+ ET.SubElement(relation, "tag", t)
+ return self.element_num
+
+ def save(self, filename):
+ tree = ET.ElementTree(self.root)
+ parsed = parseString(ET.tostring(self.root, "utf-8")).toprettyxml(indent=" ")
+ with open(filename, "w") as f:
+ f.write(parsed)
diff --git a/bag2lanelet/requirements.txt b/bag2lanelet/requirements.txt
new file mode 100644
index 000000000..c96433458
--- /dev/null
+++ b/bag2lanelet/requirements.txt
@@ -0,0 +1,2 @@
+transforms3d
+mgrspy
\ No newline at end of file