Skip to content

Commit 4cd1310

Browse files
committed
add patch
Signed-off-by: wep21 <[email protected]>
1 parent 1ee7eeb commit 4cd1310

File tree

1 file changed

+22
-0
lines changed

1 file changed

+22
-0
lines changed
Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
diff --git a/src/route_handler.cpp b/src/route_handler.cpp
2+
index 878f1aa..d3604b4 100644
3+
--- a/src/route_handler.cpp
4+
+++ b/src/route_handler.cpp
5+
@@ -2073,7 +2073,7 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
6+
RCLCPP_WARN(
7+
logger_,
8+
"Failed to find reroute on previous preferred lanelets %s, but on previous route "
9+
- "segment %ld still",
10+
+ "segment %lld still",
11+
preferred_lanelets_str.str().c_str(), closest_lanelet.id());
12+
return closest_lanelet;
13+
}
14+
@@ -2134,7 +2134,7 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
15+
const double optional_route_length = optional_route->length2d();
16+
const double optional_route_cost = optional_route_length + angle_diff_weight * angle_diff;
17+
RCLCPP_DEBUG(
18+
- logger_, "Lanelet ID %ld: Route length = %.1f, Angle Diff = %.4f rad, Route cost = %.2f",
19+
+ logger_, "Lanelet ID %lld: Route length = %.1f, Angle Diff = %.4f rad, Route cost = %.2f",
20+
st_llt.id(), optional_route_length, angle_diff, optional_route_cost);
21+
if (optional_route_cost < min_route_cost) {
22+
min_route_cost = optional_route_cost;

0 commit comments

Comments
 (0)