diff --git a/README.md b/README.md index 035ec3d..481ef02 100644 --- a/README.md +++ b/README.md @@ -71,7 +71,9 @@ It also provides a `/metrics_wayp` array topic with the following elements: |`[3]` | maximum lateral distance over time | `MAX_LAT_DISTANCE` |`[4]` | current waypoint ID | `CUR_WAYPOINT_ID` |`[5]` | target waypoint ID | `TRG_WAYPOINT_ID` -|`[6]` | target waypoint longitudinal distance | `TRG_WAY_LON_DIST` +|`[6]` | target waypoint longitudinal distance (similar to lookahed distance, but at a waypoint) | `TRG_WAY_LON_DIST` +|`[7]` | actual lookahead distance | `ACT_LOOK_DIST` + diff --git a/include/wayp_plan_tools/common.hpp b/include/wayp_plan_tools/common.hpp index c270a0c..2a003d2 100644 --- a/include/wayp_plan_tools/common.hpp +++ b/include/wayp_plan_tools/common.hpp @@ -18,6 +18,7 @@ namespace common_wpt CUR_WAYPOINT_ID, // current waypoint ID TRG_WAYPOINT_ID, // target waypoint ID TRG_WAY_LON_DIST, // target waypoint longitudinal distance + ACT_LOOK_DIST, // actual lookahead distance NOT_USED_YET }; } // namespace common_wpt diff --git a/src/waypoint_to_target.cpp b/src/waypoint_to_target.cpp index 135a84d..bd90f52 100755 --- a/src/waypoint_to_target.cpp +++ b/src/waypoint_to_target.cpp @@ -311,6 +311,7 @@ class WaypointToTarget : public rclcpp::Node metrics_arr.data[common_wpt::MAX_LAT_DISTANCE] = maximum_distance; // calculate the adaptive lookahead distance double lookahead_actual = calcLookahead(speed_msg.data); + metrics_arr.data[common_wpt::ACT_LOOK_DIST] = lookahead_actual; for (int i = closest_waypoint; i <= last_wp; i++) {