Added new metrics: actual lookahead distance

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`
+
 
 <img src="csv/lookahead01.svg" width=60% />
 

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

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++)
         {