Merge branch 'release/1.15.3'

CHANGELOG.md

@@ -4,6 +4,25 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](http://keepachangelog.com/).
 
 ## Unreleased
+## 1.15.3 - 2024-11-18
+### Added
+- Criterions
+    - _Profile2_
+        - Parameters `reference_obtain_start` and `reference_obtain_start_td` to prepare the criterion and whole pipeline to generate overtaking maneuvers. Using this generates following parameters that overwrite values in the configuration: `v_0`, `fixed_points` and `fixed_segments`.
+
+### Changed
+- Selectors
+    - _Uniform_distance_
+        - Parameter `fixed_points` can be now set from `selector_args` as well.
+    - _Uniform_time_
+        - Parameter `fixed_points` can be now set from `selector_args` as well.
+- Initialization (`init()`) of all algorithms may now return an optional dictionary. When returned, it is fused together with the configuration of the current cascade step (except penalizers as they are handled by the optimizers).
+
+### Fixed
+- Criterions
+    - _Profile2_
+        - Properly save npy data without raising ragged array 'VisibleDeprecationWarning'.
+
 ## 1.15.2 - 2024-11-12
 ### Added
 - 'ng_generate_data'

CITATION.cff

@@ -5,8 +5,8 @@ authors:
   given-names: "Jaroslav"
   orcid: "https://orcid.org/0000-0001-8816-2773"
 title: "ng_trajectory"
-version: 1.15.2
-date-released: 2024-11-12
+version: 1.15.3
+date-released: 2024-11-18
 url: "https://github.com/jara001/ng_trajectory"
 license: GPL-3.0
 preferred-citation:

doc/README.md

@@ -334,6 +334,8 @@ reference (str) = None [Name of the file to load (x, y, t, v) reference path tha
 reference_dist (float) = 1.0 [Minimum allowed distance from the reference at given time [m].]
 reference_rotate (int) = 0 [Number of points to rotate the reference trajectory.]
 reference_laptime (float) = 0 [Lap time of the given reference. 0 = estimated from data]
+reference_obtain_start (bool) = False [When given, initial speed and initial position of the vehicle is computed.]
+reference_obtain_start_td (float) = 0.0 [Time distance behind the reference [s].]
 save_solution_csv (str) = $ [When non-empty, save final trajectory to this file as CSV. Use '$' to use log name instead.]
 favor_overtaking (float) = 0 [Penalty value to add to the lap time when overtaking does not occur.]
 friction_map (str) = None [Name of the file to load (x, y, mu*100) with friction map.]
@@ -553,11 +555,13 @@ This selector uniformly samples the input path so that the selected points are e
 
 
 ```html
+Parameters:
+fixed_points (list) = [] [Points to be used in the selection upon calling 'select'.]
+
 Init parameters:
 sampling_distance (float) = 1.0 [[m] Distance of super-sampling before the interpolation, skipped when 0.]
 distance (float) = 0 [[m] Distance between the individual points, ignored when 0, used when requesting negative number of points.]
 rotate (float) = 0 [Parameter for rotating the input path. 0 is not rotated. <0, 1)]
-fixed_points (list) = [] [Points to be used in the selection upon calling 'select'.]
 ```
 
 
@@ -592,6 +596,7 @@ Following algorithms are used:
 Parameters:
 overlap (int) = 0 [Size of the trajectory overlap. 0 disables this.]
 friction_map_yaml (str) = None [(Requires pyyaml) Name of the yaml configuration of the original map that was used to create '.npy' files. Map file specified in the configuration has to exist.]
+fixed_points (list) = [] [Points to be used in the selection upon calling 'select'.]
 
 Init parameters:
 rotate (float) = 0 [Parameter for rotating the input path. 0 is not rotated. <0, 1)]
@@ -620,7 +625,6 @@ friction_map_plot (bool) = False [When True, friction map is plotted.]
 friction_map_save (bool) = False [When True, friction map is saved alongside the log files.]
 sampling_distance (float) = 1.0 [[m] Distance of super-sampling before the interpolation, skipped when 0.]
 distance (float) = 0 [[m] Distance between the individual points, ignored when 0, used when requesting negative number of points.]
-fixed_points (list) = [] [Points to be used in the selection upon calling 'select'.]
 
 Init (viz.) parameters:
 plot (bool) = False [Whether a graphical representation should be created.]

ng_trajectory/criterions/curvature/main.py

@@ -10,12 +10,18 @@
 
 import numpy
 
+from typing import (
+    Any,
+    Dict,
+    Optional,
+)
+
 
 ######################
 # Functions
 ######################
 
-def init(**kwargs) -> None:
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize criterion."""
     pass
 

ng_trajectory/criterions/jazar_model/main.py

@@ -12,6 +12,12 @@
 import numpy
 import math
 
+from typing import (
+    Any,
+    Dict,
+    Optional,
+)
+
 
 # Parameters
 from ng_trajectory.parameter import ParameterList
@@ -383,7 +389,7 @@ def computeProfile(points):
 # General functions
 ######################
 
-def init(**kwargs) -> None:
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize criterion."""
     P.updateAll(kwargs, reset = False)
 

ng_trajectory/criterions/length/main.py

@@ -11,12 +11,18 @@
 
 import numpy
 
+from typing import (
+    Any,
+    Dict,
+    Optional,
+)
+
 
 ######################
 # Functions
 ######################
 
-def init(**kwargs) -> None:
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize criterion."""
     pass
 

ng_trajectory/criterions/manual/main.py

@@ -11,6 +11,12 @@
 
 from threading import Lock
 
+from typing import (
+    Any,
+    Dict,
+    Optional,
+)
+
 
 # Global variables
 INPUT_LOCK = Lock()
@@ -20,7 +26,7 @@
 # Functions
 ######################
 
-def init(**kwargs):
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize criterion."""
     return None
 

ng_trajectory/criterions/profile/main.py

@@ -43,6 +43,12 @@
 
 from itertools import chain  # Join generators
 
+from typing import (
+    Any,
+    Dict,
+    Optional,
+)
+
 
 # Global variables
 CENTERLINE = None
@@ -329,7 +335,7 @@ def perr(*args, **kwargs):
 # Functions
 ######################
 
-def init(**kwargs) -> None:
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize criterion."""
     global REFERENCE, CENTERLINE, OVERTAKING_POINTS
 

ng_trajectory/criterions/profile2/main.py

@@ -33,7 +33,7 @@
 
 from ng_trajectory.log import (
     logfileName,
-    log, logvv,
+    log, logv, logvv,
     print0
 )
 
@@ -53,7 +53,12 @@
 from skimage.segmentation import flood_fill
 import copy
 
-from typing import Tuple
+from typing import (
+    Any,
+    Dict,
+    Tuple,
+    Optional,
+)
 
 
 # Global variables
@@ -83,6 +88,8 @@
 P.createAdd("reference_dist", 1.0, float, "Minimum allowed distance from the reference at given time [m].", "init")
 P.createAdd("reference_rotate", 0, int, "Number of points to rotate the reference trajectory.", "init")
 P.createAdd("reference_laptime", 0, float, "Lap time of the given reference. 0 = estimated from data", "init")
+P.createAdd("reference_obtain_start", False, bool, "When given, initial speed and initial position of the vehicle is computed.", "init")
+P.createAdd("reference_obtain_start_td", 0.0, float, "Time distance behind the reference [s].", "init")
 P.createAdd("save_solution_csv", "$", str, "When non-empty, save final trajectory to this file as CSV. Use '$' to use log name instead.", "init")
 P.createAdd("plot", False, bool, "Whether a graphical representation should be created.", "init (viz.)")
 P.createAdd("plot_reference", False, bool, "Whether the reference trajectory should be plotted.", "init (viz.)")
@@ -465,10 +472,12 @@ def get_rect_points(
     return corners.T  # n x 2
 
 
-def init(**kwargs) -> None:
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize criterion."""
     global REFERENCE, CENTERLINE, OVERTAKING_POINTS, MAP_OUTSIDE, MAP_INSIDE
 
+    cri_dict = {}
+
     profiler.parametersSet(**kwargs)
 
     P.updateAll(kwargs)
@@ -485,6 +494,8 @@ def init(**kwargs) -> None:
             logfileName() + ".csv"
         )
 
+    lap_time = 0.0
+
     if P.getValue("reference") is not None:
         REFERENCE = numpy.load(P.getValue("reference"))[:, :4]
 
@@ -560,6 +571,58 @@ def init(**kwargs) -> None:
     else:
         REFERENCE = None
 
+    if P.getValue("reference_obtain_start") and REFERENCE is not None:
+        """
+        This section is used to compute starting location of the ego car,
+        to properly set-up the environment for overtaking maneuvers.
+        """
+        # 1. Obtain the starting position.
+        _start_time = lap_time - P.getValue("reference_obtain_start_td")
+        _start_time %= lap_time
+
+        _start_id = numpy.argmin(
+            numpy.sqrt(
+                numpy.power(REFERENCE[:, 2] - _start_time, 2)
+            )
+        )
+
+        # 2. Obtain the initial speed.
+        profiler.parametersSet(v_0 = REFERENCE[_start_id, 3])
+        cri_dict["v_0"] = REFERENCE[_start_id, 3]
+
+        # 3. Obtain the positions of fixed points / segments.
+        #    This will enforce starting position of the ego car, as well as
+        #    its initial heading.
+        # Note: In the original work, the distance between the two points
+        #       was actually 1 -- X -- 2 from the reference. So we use that.
+        # Side note: There is a limit how close the points can be. If they
+        #            are too close, then the selector/segmentator/optimizer
+        #            will fuse them together/associate with the same segment.
+        #            And that is something we don't want.
+        _second_id = (_start_id + 2) % len(REFERENCE)
+
+        # Note: As a workaround, we have to push this into 'selector_args'.
+        # Currently, optimizers do not pass the whole configuration, but only
+        # selected parts.
+        cri_dict["selector_args"] = {
+            "fixed_points": [
+                [REFERENCE[_start_id, 0], REFERENCE[_start_id, 1]],
+                [REFERENCE[_second_id, 0], REFERENCE[_second_id, 1]]
+            ]
+        }
+        cri_dict["fixed_segments"] = [
+            [REFERENCE[_start_id, 0], REFERENCE[_start_id, 1]],
+            [REFERENCE[_second_id, 0], REFERENCE[_second_id, 1]]
+        ]
+
+        log (
+            "Computed ego start location, td = %fs (requested %fs)"
+            % (
+                lap_time - REFERENCE[_start_id, 2],
+                P.getValue("reference_obtain_start_td")
+            )
+        )
+
     if P.getValue("friction_map") is not None:
         fmap = numpy.load(P.getValue("friction_map"))
 
@@ -653,6 +716,8 @@ def init(**kwargs) -> None:
             ])
         )
 
+    return cri_dict if len(cri_dict) > 0 else None
+
 
 def compute(
         points: numpy.ndarray,
@@ -1285,6 +1350,9 @@ def compute(
         if not overflown.get("optimization", True):
             # FIXME: This probably fails at some occasions.
             file_name = logfileName().replace("matryoshka.log", "save.npy")
-            numpy.save(file_name, data_to_save)
+            numpy.save(
+                file_name,
+                numpy.asarray(data_to_save, dtype = object)
+            )
 
     return float(criterion)

ng_trajectory/interpolators/cubic_spline/main.py

@@ -15,6 +15,12 @@
 
 from ng_trajectory.parameter import ParameterList
 
+from typing import (
+    Any,
+    Dict,
+    Optional,
+)
+
 
 # Parameters
 P = ParameterList()
@@ -26,7 +32,7 @@
 # Functions
 ######################
 
-def init(**kwargs) -> None:
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize interpolator."""
     P.updateAll(kwargs)
 

ng_trajectory/interpolators/none/main.py

@@ -12,6 +12,12 @@
 
 from ng_trajectory.parameter import ParameterList
 
+from typing import (
+    Any,
+    Dict,
+    Optional,
+)
+
 
 # Parameters
 P = ParameterList()
@@ -25,7 +31,7 @@
 # Functions
 ######################
 
-def init(**kwargs) -> None:
+def init(**kwargs) -> Optional[Dict[str, Any]]:
     """Initialize interpolator."""
     P.updateAll(kwargs)