Skip to content

Commit 295074f

Browse files
committed
pre-commit
Signed-off-by: t4-adc <grp-rd-1-adc-admin@tier4.jp>
1 parent de78b43 commit 295074f

2 files changed

Lines changed: 18 additions & 15 deletions

File tree

planning/autoware_static_centerline_generator/README.md

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -103,18 +103,18 @@ By default, footprint colors are:
103103

104104
- Distance to lanelet boundary < 0.1 [m]: Red
105105
- Distance to lanelet boundary < 0.2 [m]: Orange
106-
- Distance to lanelet boundary < 0.3 [m]: Yello
106+
- Distance to lanelet boundary < 0.3 [m]: Yellow
107107
- Yaw difference exceeds jitter_deg_threshold (default is 40) [deg]: Blue
108108

109109
## Troubleshooting (VMB embedding)
110110

111111
When embedding a fixed route from VMB, some routes may fail or produce incomplete centerlines. Common causes and remedies:
112112

113-
| Symptom / Error | Cause | What to check |
114-
|-----------------|--------|----------------|
115-
| **LaneletsNotConnected** | The lane IDs in the route are not topologically connected (next lanelet of one is not the following in the list). | In VMB, ensure the selected lane sequence is connected in the map (no skipped lanes, correct direction). |
116-
| **PathNotFound** | Optimization returned no trajectory (e.g. goal_method, very sharp curve, or degenerate lane). | Check `goal_method` and route shape; try a shorter or simpler segment. |
117-
| **InvalidRoute** | Request had an empty route. | Ensure the route selection in VMB sends at least one lane ID. |
118-
| **InvalidLanelet** | Start/goal lanelet has no valid centerline (e.g. centerline has fewer than 2 points). | Fix or regenerate the map so that lanelets have valid centerlines. |
119-
| **Some lanelets with no points** | Optimized path does not pass through every selected lanelet, or points fall outside polygon (e.g. sharp curves). | Check terminal for `PlanPath: lanelet id X has no assigned points`. Adjust route or map; sharp curves may need manual centerline. |
120-
| **Points outside all lanelets** | Smoothed trajectory lies slightly outside lane polygons (boundaries or optimization). | Terminal logs: `X trajectory point(s) were outside all lanelet polygons`. Points are still assigned to the nearest segment; if embedding looks wrong, simplify the route or check map geometry. |
113+
| Symptom / Error | Cause | What to check |
114+
| -------------------------------- | ----------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
115+
| **LaneletsNotConnected** | The lane IDs in the route are not topologically connected (next lanelet of one is not the following in the list). | In VMB, ensure the selected lane sequence is connected in the map (no skipped lanes, correct direction). |
116+
| **PathNotFound** | Optimization returned no trajectory (e.g. goal_method, very sharp curve, or degenerate lane). | Check `goal_method` and route shape; try a shorter or simpler segment. |
117+
| **InvalidRoute** | Request had an empty route. | Ensure the route selection in VMB sends at least one lane ID. |
118+
| **InvalidLanelet** | Start/goal lanelet has no valid centerline (e.g. centerline has fewer than 2 points). | Fix or regenerate the map so that lanelets have valid centerlines. |
119+
| **Some lanelets with no points** | Optimized path does not pass through every selected lanelet, or points fall outside polygon (e.g. sharp curves). | Check terminal for `PlanPath: lanelet id X has no assigned points`. Adjust route or map; sharp curves may need manual centerline. |
120+
| **Points outside all lanelets** | Smoothed trajectory lies slightly outside lane polygons (boundaries or optimization). | Terminal logs: `X trajectory point(s) were outside all lanelet polygons`. Points are still assigned to the nearest segment; if embedding looks wrong, simplify the route or check map geometry. |

planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@
5353

5454
#include <algorithm>
5555
#include <chrono>
56+
#include <cinttypes>
5657
#include <filesystem>
5758
#include <fstream>
5859
#include <iostream>
@@ -158,8 +159,8 @@ std::vector<lanelet::Id> check_lanelet_connection(
158159

159160
void assign_centerline_points_to_lanelets(
160161
const std::vector<TrajectoryPoint> & optimized_traj_points,
161-
const lanelet::ConstLanelets & route_lanelets,
162-
PlanPath::Response::SharedPtr response, const rclcpp::Logger & logger)
162+
const lanelet::ConstLanelets & route_lanelets, PlanPath::Response::SharedPtr response,
163+
const rclcpp::Logger & logger)
163164
{
164165
std::vector<std::vector<geometry_msgs::msg::Point>> points_per_lanelet(route_lanelets.size());
165166
size_t current_lanelet_idx = 0;
@@ -168,8 +169,9 @@ void assign_centerline_points_to_lanelets(
168169
for (const auto & traj_point : optimized_traj_points) {
169170
const auto pt = convert_to_lanelet_point(traj_point.pose.position);
170171

171-
if (current_lanelet_idx < route_lanelets.size() &&
172-
lanelet::geometry::inside(route_lanelets.at(current_lanelet_idx), pt)) {
172+
if (
173+
current_lanelet_idx < route_lanelets.size() &&
174+
lanelet::geometry::inside(route_lanelets.at(current_lanelet_idx), pt)) {
173175
points_per_lanelet.at(current_lanelet_idx).push_back(traj_point.pose.position);
174176
continue;
175177
}
@@ -210,8 +212,9 @@ void assign_centerline_points_to_lanelets(
210212
} else {
211213
RCLCPP_WARN(
212214
logger,
213-
"PlanPath: lanelet id %ld has no assigned points (trajectory may skip this segment).",
214-
static_cast<long>(route_lanelets.at(i).id()));
215+
"PlanPath: lanelet id %" PRId64
216+
" has no assigned points (trajectory may skip this segment).",
217+
static_cast<int64_t>(route_lanelets.at(i).id()));
215218
}
216219
}
217220
}

0 commit comments

Comments
 (0)