Skip to content

Commit 33672f6

Browse files
TetsuKawaTetsuKawa
authored
Merge branch 'main' into feat/remove-cat-command
2 parents 1049d18 + c40da9f commit 33672f6

File tree

3 files changed

+204
-50
lines changed

3 files changed

+204
-50
lines changed

‎planning/path_smoother/docs/eb.md

+50-50
Original file line numberDiff line numberDiff line change
@@ -78,65 +78,65 @@ We formulate a quadratic problem minimizing the diagonal length of the rhombus o
7878

7979
![eb](../media/eb.svg){: style="width:600px"}
8080

81-
Assuming that $k$'th point is $\boldsymbol{p}_k = (x_k, y_k)$, the objective function is as follows.
81+
Assuming that $k$'th point is $\mathbf{p}_k = (x_k, y_k)$, the objective function is as follows.
8282

8383
$$
8484
\begin{align}
85-
\ J & = \min \sum_{k=1}^{n-2} ||(\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}) - (\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1})||^2 \\
86-
\ & = \min \sum_{k=1}^{n-2} ||\boldsymbol{p}_{k+1} - 2 \boldsymbol{p}_{k} + \boldsymbol{p}_{k-1}||^2 \\
87-
\ & = \min \sum_{k=1}^{n-2} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\
85+
\ J & = \min \sum_{k=1}^{n-2} ||(\mathbf{p}_{k+1} - \mathbf{p}_{k}) - (\mathbf{p}_{k} - \mathbf{p}_{k-1})||^2 \\\
86+
\ & = \min \sum_{k=1}^{n-2} ||\mathbf{p}_{k+1} - 2 \mathbf{p}_{k} + \mathbf{p}_{k-1}||^2 \\\
87+
\ & = \min \sum_{k=1}^{n-2} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\\
8888
\ & = \min
8989
\begin{pmatrix}
90-
\ x_0 \\
91-
\ x_1 \\
92-
\ x_2 \\
93-
\vdots \\
94-
\ x_{n-3}\\
95-
\ x_{n-2} \\
96-
\ x_{n-1} \\
97-
\ y_0 \\
98-
\ y_1 \\
99-
\ y_2 \\
100-
\vdots \\
101-
\ y_{n-3}\\
102-
\ y_{n-2} \\
103-
\ y_{n-1} \\
90+
\ x_0 \\\
91+
\ x_1 \\\
92+
\ x_2 \\\
93+
\vdots \\\
94+
\ x_{n-3}\\\
95+
\ x_{n-2} \\\
96+
\ x_{n-1} \\\
97+
\ y_0 \\\
98+
\ y_1 \\\
99+
\ y_2 \\\
100+
\vdots \\\
101+
\ y_{n-3}\\\
102+
\ y_{n-2} \\\
103+
\ y_{n-1} \\\
104104
\end{pmatrix}^T
105105
\begin{pmatrix}
106-
1 & -2 & 1 & 0 & \dots& \\
107-
-2 & 5 & -4 & 1 & 0 &\dots \\
108-
1 & -4 & 6 & -4 & 1 & \\
109-
0 & 1 & -4 & 6 & -4 & \\
110-
\vdots & 0 & \ddots&\ddots& \ddots \\
111-
& \vdots & & & \\
112-
& & & 1 & -4 & 6 & -4 & 1 \\
113-
& & & & 1 & -4 & 5 & -2 \\
114-
& & & & & 1 & -2 & 1& \\
115-
& & & & & & & &1 & -2 & 1 & 0 & \dots& \\
116-
& & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\
117-
& & & & & & & &1 & -4 & 6 & -4 & 1 & \\
118-
& & & & & & & &0 & 1 & -4 & 6 & -4 & \\
119-
& & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\
120-
& & & & & & & & & \vdots & & & \\
121-
& & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\
122-
& & & & & & & & & & & & 1 & -4 & 5 & -2 \\
123-
& & & & & & & & & & & & & 1 & -2 & 1& \\
106+
1 & -2 & 1 & 0 & \dots& \\\
107+
-2 & 5 & -4 & 1 & 0 &\dots \\\
108+
1 & -4 & 6 & -4 & 1 & \\\
109+
0 & 1 & -4 & 6 & -4 & \\\
110+
\vdots & 0 & \ddots&\ddots& \ddots \\\
111+
& \vdots & & & \\\
112+
& & & 1 & -4 & 6 & -4 & 1 \\\
113+
& & & & 1 & -4 & 5 & -2 \\\
114+
& & & & & 1 & -2 & 1& \\\
115+
& & & & & & & &1 & -2 & 1 & 0 & \dots& \\\
116+
& & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\\
117+
& & & & & & & &1 & -4 & 6 & -4 & 1 & \\\
118+
& & & & & & & &0 & 1 & -4 & 6 & -4 & \\\
119+
& & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\\
120+
& & & & & & & & & \vdots & & & \\\
121+
& & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\\
122+
& & & & & & & & & & & & 1 & -4 & 5 & -2 \\\
123+
& & & & & & & & & & & & & 1 & -2 & 1& \\\
124124
\end{pmatrix}
125125
\begin{pmatrix}
126-
\ x_0 \\
127-
\ x_1 \\
128-
\ x_2 \\
129-
\vdots \\
130-
\ x_{n-3}\\
131-
\ x_{n-2} \\
132-
\ x_{n-1} \\
133-
\ y_0 \\
134-
\ y_1 \\
135-
\ y_2 \\
136-
\vdots \\
137-
\ y_{n-3}\\
138-
\ y_{n-2} \\
139-
\ y_{n-1} \\
126+
\ x_0 \\\
127+
\ x_1 \\\
128+
\ x_2 \\\
129+
\vdots \\\
130+
\ x_{n-3}\\\
131+
\ x_{n-2} \\\
132+
\ x_{n-1} \\\
133+
\ y_0 \\\
134+
\ y_1 \\\
135+
\ y_2 \\\
136+
\vdots \\\
137+
\ y_{n-3}\\\
138+
\ y_{n-2} \\\
139+
\ y_{n-1} \\\
140140
\end{pmatrix}
141141
\end{align}
142142
$$

‎planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py

+139
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
#!/bin/env python3
2+
3+
# Copyright 2024 TIER IV, Inc.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
17+
import argparse
18+
from decimal import Decimal
19+
import os
20+
import subprocess
21+
import xml.etree.ElementTree as ET
22+
23+
24+
def sort_attributes(root):
25+
for shallow_element in root:
26+
# sort nodes
27+
attrib = shallow_element.attrib
28+
if len(attrib) > 1:
29+
attributes = sorted(attrib.items())
30+
attrib.clear()
31+
attrib.update(attributes)
32+
if shallow_element.tag == "relation":
33+
pass
34+
35+
# sort the element in the tag
36+
for deep_element in shallow_element:
37+
attrib = deep_element.attrib
38+
if len(attrib) > 1:
39+
# adjust attribute order, e.g. by sorting
40+
attributes = sorted(attrib.items())
41+
attrib.clear()
42+
attrib.update(attributes)
43+
44+
# sort tags
45+
sorted_shallow_element = sorted(shallow_element, key=lambda x: x.items())
46+
if len(shallow_element) != 0:
47+
# NOTE: This "tail" is related to the indent of the next tag
48+
first_tail = shallow_element[0].tail
49+
last_tail = shallow_element[-1].tail
50+
for idx, val_shallow_element in enumerate(sorted_shallow_element):
51+
shallow_element[idx] = val_shallow_element
52+
if idx == len(shallow_element) - 1:
53+
shallow_element[idx].tail = last_tail
54+
else:
55+
shallow_element[idx].tail = first_tail
56+
57+
58+
def remove_diff_to_ignore(osm_root):
59+
decimal_precision = 11 # for lat/lon values
60+
61+
# remove attributes of the osm tag
62+
osm_root.attrib.clear()
63+
64+
# remove the MetaInfo tag generated by Vector Map Builder
65+
if osm_root[0].tag == "MetaInfo":
66+
osm_root.remove(osm_root[0])
67+
68+
# remove unnecessary attributes for diff
69+
for osm_child in osm_root:
70+
if osm_child.tag == "osm":
71+
osm_child.attrib.pop("osm")
72+
if "visible" in osm_child.attrib and osm_child.attrib["visible"]:
73+
osm_child.attrib.pop("visible")
74+
if "version" in osm_child.attrib and osm_child.attrib["version"]:
75+
osm_child.attrib.pop("version")
76+
if "action" in osm_child.attrib and osm_child.attrib["action"] == "modify":
77+
osm_child.attrib.pop("action")
78+
if "lat" in osm_child.attrib:
79+
osm_child.attrib["lat"] = str(
80+
Decimal(float(osm_child.attrib["lat"])).quantize(Decimal(f"1e-{decimal_precision}"))
81+
)
82+
if "lon" in osm_child.attrib:
83+
osm_child.attrib["lon"] = str(
84+
Decimal(float(osm_child.attrib["lon"])).quantize(Decimal(f"1e-{decimal_precision}"))
85+
)
86+
87+
88+
if __name__ == "__main__":
89+
parser = argparse.ArgumentParser()
90+
parser.add_argument(
91+
"-s", "--sort-attributes", action="store_true", help="Sort attributes of LL2 maps"
92+
)
93+
parser.add_argument(
94+
"-i",
95+
"--ignore-minor-attributes",
96+
action="store_true",
97+
help="Ignore minor attributes of LL2 maps which does not change the map's behavior",
98+
)
99+
args = parser.parse_args()
100+
101+
original_osm_file_name = "/tmp/static_centerline_generator/input/lanelet2_map.osm"
102+
modified_osm_file_name = "/tmp/static_centerline_generator/output/lanelet2_map.osm"
103+
104+
# load LL2 maps
105+
original_osm_tree = ET.parse(original_osm_file_name)
106+
original_osm_root = original_osm_tree.getroot()
107+
modified_osm_tree = ET.parse(modified_osm_file_name)
108+
modified_osm_root = modified_osm_tree.getroot()
109+
110+
# sort attributes
111+
if args.sort_attributes:
112+
sort_attributes(modified_osm_root)
113+
sort_attributes(original_osm_root)
114+
115+
# ignore minor attributes
116+
if args.ignore_minor_attributes:
117+
remove_diff_to_ignore(original_osm_root)
118+
remove_diff_to_ignore(modified_osm_root)
119+
120+
# write LL2 maps
121+
output_dir_path = "/tmp/static_centerline_generator/show_lanelet2_map_diff/"
122+
os.makedirs(output_dir_path + "original/", exist_ok=True)
123+
os.makedirs(output_dir_path + "modified/", exist_ok=True)
124+
125+
original_osm_tree.write(output_dir_path + "original/lanelet2_map.osm")
126+
modified_osm_tree.write(output_dir_path + "modified/lanelet2_map.osm")
127+
128+
# show diff
129+
print("[INFO] Show diff of following LL2 maps")
130+
print(f" {output_dir_path + 'original/lanelet2_map.osm'}")
131+
print(f" {output_dir_path + 'modified/lanelet2_map.osm'}")
132+
subprocess.run(
133+
[
134+
"diff",
135+
output_dir_path + "original/lanelet2_map.osm",
136+
output_dir_path + "modified/lanelet2_map.osm",
137+
"--color",
138+
]
139+
)

‎planning/static_centerline_generator/src/static_centerline_generator_node.cpp

+15
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@
4747
#include <lanelet2_projection/UTM.h>
4848

4949
#include <chrono>
50+
#include <filesystem>
5051
#include <fstream>
5152
#include <limits>
5253
#include <memory>
@@ -336,6 +337,13 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout
336337

337338
void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path)
338339
{
340+
// copy the input LL2 map to the temporary file for debugging
341+
const std::string debug_input_file_dir{"/tmp/static_centerline_generator/input/"};
342+
std::filesystem::create_directories(debug_input_file_dir);
343+
std::filesystem::copy(
344+
lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm",
345+
std::filesystem::copy_options::overwrite_existing);
346+
339347
// load map by the map_loader package
340348
map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr {
341349
// load map
@@ -635,5 +643,12 @@ void StaticCenterlineGeneratorNode::save_map(
635643
// save map with modified center line
636644
lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_);
637645
RCLCPP_INFO(get_logger(), "Saved map.");
646+
647+
// copy the output LL2 map to the temporary file for debugging
648+
const std::string debug_output_file_dir{"/tmp/static_centerline_generator/output/"};
649+
std::filesystem::create_directories(debug_output_file_dir);
650+
std::filesystem::copy(
651+
lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm",
652+
std::filesystem::copy_options::overwrite_existing);
638653
}
639654
} // namespace static_centerline_generator

0 commit comments

Comments
 (0)