diff --git a/README.md b/README.md index cf43cbe2..62152e5d 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ # autoware_tools -This is a repository for keeping packages that are not needed at runtime, including packages for benchmarking, debugging, tuning, calibrating, etc. \ No newline at end of file + +This is a repository for keeping packages that are not needed at runtime, including packages for benchmarking, debugging, tuning, calibrating, etc. diff --git a/bag2lanelet/README.md b/bag2lanelet/README.md index f1a5ffc5..8ddeefb7 100644 --- a/bag2lanelet/README.md +++ b/bag2lanelet/README.md @@ -9,24 +9,18 @@ The provided functionalities are as follows: ## 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 @@ -35,16 +29,14 @@ After completing the drive, you can run the `bag2lanelet.py` script. This requir The map will be saved in the specified directory, following the naming convention `
- 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.
@@ -57,7 +49,6 @@ Then, Export the map. You can run the planning_simulator with the refined lanele
- ## Requirements ``` @@ -75,10 +66,11 @@ Check `./bag2lanelet.py --help` ``` ./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 +./bag2lanelet.py /home/autoware/rosbag/sample . --width=3.0 --mgrs 32TNR219517 ``` ### generate trajectory file for Vector Map Builder @@ -87,9 +79,8 @@ or with MGRS code at Monza Track ./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 +- 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. diff --git a/bag2lanelet/bag2lanelet.py b/bag2lanelet/bag2lanelet.py index 3576c31b..cdd617dd 100755 --- a/bag2lanelet/bag2lanelet.py +++ b/bag2lanelet/bag2lanelet.py @@ -1,10 +1,11 @@ #!/bin/env python3 import argparse +from datetime import datetime import os import pathlib -from datetime import datetime -from bag2way import bag2pose, pose2line +from bag2way import bag2pose +from bag2way import pose2line from lanelet_xml import LaneletMap @@ -36,7 +37,12 @@ def main(): 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") + "--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" ) @@ -49,7 +55,9 @@ def main(): output_path = pathlib.Path(args.output_lanelet) print(args) - genarate(input_path, output_path, args.width, args.mgrs, args.interval, args.offset, args.center) + genarate( + input_path, output_path, args.width, args.mgrs, args.interval, args.offset, args.center + ) if __name__ == "__main__": diff --git a/bag2lanelet/bag2map.py b/bag2lanelet/bag2map.py index 8ca3d29d..d47c7e2a 100755 --- a/bag2lanelet/bag2map.py +++ b/bag2lanelet/bag2map.py @@ -1,18 +1,23 @@ #!/bin/env python3 import argparse +from datetime import datetime import os import pathlib -from datetime import datetime +from bag2way import bag2point_stamped +from bag2way import bag2pose +from bag2way import pose2line 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]]] + 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) diff --git a/bag2lanelet/bag2trajectory.py b/bag2lanelet/bag2trajectory.py index fb4e043f..d6257d7a 100755 --- a/bag2lanelet/bag2trajectory.py +++ b/bag2lanelet/bag2trajectory.py @@ -2,9 +2,9 @@ import argparse import pathlib +from bag2way import bag2pose import numpy as np import tf_transformations -from bag2way import bag2pose def genarate(input_path, output_path): diff --git a/bag2lanelet/bag2way.py b/bag2lanelet/bag2way.py index e77f6932..9defcb5d 100755 --- a/bag2lanelet/bag2way.py +++ b/bag2lanelet/bag2way.py @@ -2,10 +2,12 @@ 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 rosbag2_py import ConverterOptions +from rosbag2_py import SequentialReader +from rosbag2_py import StorageOptions from rosidl_runtime_py.utilities import get_message +import tf_transformations def create_reader(bag_dir: str) -> SequentialReader: @@ -22,6 +24,7 @@ def create_reader(bag_dir: str) -> 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 diff --git a/bag2lanelet/lanelet_xml.py b/bag2lanelet/lanelet_xml.py index eff94963..485ac74b 100755 --- a/bag2lanelet/lanelet_xml.py +++ b/bag2lanelet/lanelet_xml.py @@ -1,5 +1,6 @@ -import xml.etree.ElementTree as ET from xml.dom.minidom import parseString +import xml.etree.ElementTree as ET + from mgrspy import mgrs @@ -27,7 +28,11 @@ def add_node(self, x, y, z): 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])}) + 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": ""}, diff --git a/bag2lanelet/requirements.txt b/bag2lanelet/requirements.txt index c9643345..227213b7 100644 --- a/bag2lanelet/requirements.txt +++ b/bag2lanelet/requirements.txt @@ -1,2 +1,2 @@ transforms3d -mgrspy \ No newline at end of file +mgrspy diff --git a/build_depends.repos b/build_depends.repos index 6e4fc677..620cba58 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -20,4 +20,3 @@ repositories: type: git url: https://github.com/tier4/autoware_auto_msgs.git version: tier4/main -