Initial commit

src/road_gen/generators/base_road_generator.py

@@ -0,0 +1,50 @@
+import numpy as np
+import secrets
+
+class BaseRoadGenerator:
+    """A base generator object for generating a road of a specified length."""
+    def __init__(
+            self,
+            length: int | float,
+            ds: int | float,
+            velocity: int | float,
+            mu: float = 0.7,
+            g: float = 9.81,
+            seed: int | None = None
+            ):
+        """Initialize a BaseGenerator with a given or random seed.
+
+        Args:
+            length (int | float): The total length of the road in meters.
+            ds (int | float): The step size in meters.
+            velocity (int | float): Velocity in meters per second.
+            mu (float): Coefficient of friction. Defaults to 0.7 (dry asphalt).
+            g (float): Acceleration due to gravity (m/s^2). Defaults to 9.81.
+            seed (int | None, optional): Set a seed for the generator. Default is a random seed.
+        """        
+        if seed == None:
+            seed = secrets.randbits(32)
+        
+        if not isinstance(seed, int):
+            raise TypeError("seed must be an integer or None.")
+        
+        if not isinstance(length, int | float):
+            raise TypeError("Length must be an integer or float in meters.")
+        
+        if not isinstance(ds, int | float):
+            raise TypeError("Step size must be integer or float in meters.")
+        
+        if not isinstance(velocity, int | float):
+            raise TypeError("Velocity must be integer or float in meters per second.")
+        
+        self.length = length
+        self.ds = ds
+
+        self.velocity = velocity
+        self.min_radius = (velocity ** 2) / (g * mu)
+
+        self.seed = seed
+        self._rng = np.random.default_rng(seed)
+
+    def generate(self):
+        pass

src/road_gen/generators/random_road_generator.py

@@ -0,0 +1,70 @@
+from typing import Tuple
+import numpy as np
+
+from .base_road_generator import BaseRoadGenerator
+from ..integrator.integrator import integrate_road
+
+class RandomRoadGenerator(BaseRoadGenerator):
+    def __init__(
+            self,
+            length: int | float,
+            ds: int | float,
+            velocity: int | float,
+            mu: float = 0.7,
+            g: float = 9.81,
+            seed: int | None = None
+            ):
+        
+        """Initialize a RandomRoadGenerator with a given or random seed.
+
+        Args:
+            length (int | float): The total length of the road in meters.
+            ds (int | float): The step size in meters.
+            velocity (int | float): Velocity in meters per second.
+            mu (float): Coefficient of friction. Defaults to 0.7 (dry asphalt).
+            g (float): Acceleration due to gravity (m/s^2). Defaults to 9.81.
+            seed (int | None, optional): Set a seed for the generator. Default is a random seed.
+        """
+
+        super().__init__(length, ds, velocity, mu, g, seed)
+
+    def generate(
+            self,
+            straight_section_prob: float = 0.05,
+            straight_section_max_rel_size: float = 0.1
+    ) -> Tuple[np.ndarray, np.ndarray]:
+        """Generate a random road according to specified parameters.
+
+        Args:
+            straight_section_prob (float, optional): Probability at every step i that a straight section will start. Defaults to 0.05.
+            straight_section_max_rel_size (float, optional): The maximum size that straight section(s) can have relative to the total length of the path. Defaults to 0.1.
+
+        Returns:
+            Tuple[np.ndarray, np.ndarray]: x and y coordinates of the waypoints describing the random road.
+        """
+        
+        self.straight_section_prob = straight_section_prob
+        self.straight_section_max_rel_size = straight_section_max_rel_size
+        
+        num_points = int(self.length / (self.ds))
+        max_curvature = 1 / self.min_radius
+        
+        white_noise = self._rng.standard_normal(num_points)
+        curvature = np.clip(white_noise, -max_curvature, max_curvature)
+
+        # Randomly add multiple straight sections
+        i = 0
+        while i < num_points:
+            if np.random.rand() < straight_section_prob:
+                # Random straight segment length
+                max_len = int(num_points * straight_section_max_rel_size)
+                seg_len = np.random.randint(1, max_len + 1)
+                curvature[i:i+seg_len] = 0  # set curvature to zero for straight
+                i += seg_len  # skip over straight segment
+            else:
+                i += 1
+
+        x, y = integrate_road(curvature)
+
+        return x * self.ds, y * self.ds
+       

src/road_gen/generators/segmented_road_generator.py

@@ -0,0 +1,99 @@
+from typing import Tuple
+
+import numpy as np
+from matplotlib import pyplot as plt
+
+from .base_road_generator import BaseRoadGenerator
+from ..prefabs import prefabs
+from ..integrator.integrator import integrate_road
+
+class SegmentedRoadGenerator(BaseRoadGenerator):
+    def __init__(
+        self,
+        length: int | float,
+        ds: int | float,
+        velocity: int | float,
+        mu: float = 0.7,
+        g: float = 9.81,
+        seed: int | None = None
+        ):
+        """Initialize a SegmentedRoadGenerator with given or random seed.
+
+        Args:
+            length (int | float): The total length of the road in meters.
+            ds (int | float): The step size in meters.
+            velocity (int | float): Velocity in meters per second.
+            mu (float): Coefficient of friction. Defaults to 0.7 (dry asphalt).
+            g (float): Acceleration due to gravity (m/s^2). Defaults to 9.81.
+            seed (int | None, optional): Set a seed for the generator. Default is a random seed.
+        """
+
+        super().__init__(length, ds, velocity, mu, g, seed)
+
+    def generate(
+            self,
+            segments: list[str],
+            alpha: float = 1.0
+    ) -> Tuple[np.ndarray, np.ndarray]:
+        """Generate a curvature profile from a list of segments.
+
+        Args:
+            segments (list[str]): List of segments.
+            length (int): Total length of the path in meters.
+            velocity (float): Velocity of the car in km/h.
+            ds (float, optional): Step size. Defaults to 1.0.
+            alpha (float, optional): Dirichlet concentration parameter. A higher value leads to more uniform apportionment of the length amongst the segments, while a lower value allows more random apportionment. Defaults to 1.0.
+
+        Raises:
+            ValueError: _description_
+            ValueError: _description_
+
+        Returns:
+            np.ndarray: _description_
+        """
+        if not all(segment in prefabs.PREFABS.keys() for segment in segments):
+            raise ValueError(f"Invalid segment type provided. Available choices: {prefabs.SEGMENTS.keys()}")
+        
+        num_points = int(self.length / self.ds)
+
+        # divide num_points into len(segments) randomly sized parts.
+        parts = self._rng.dirichlet(np.full(len(segments), alpha), size=1)[0]
+        parts = parts * num_points
+        parts = np.round(parts).astype(int)
+
+        # correct round off so the sum of parts is still total length L.
+        if sum(parts) != num_points:
+            parts[0] += num_points - sum(parts)
+
+
+        curvature = np.zeros(num_points)
+        current_index = 0
+
+        for seg_name, seg_length in zip(segments, parts):
+            seg_function = prefabs.PREFABS[seg_name]
+
+            if seg_name == 'straight':
+                curvature_s = seg_function(seg_length)
+            else:
+                R_min_angle = seg_length / (np.pi / 2)
+                R_max_angle = seg_length / (np.pi / 6)
+
+                # physics limit
+                R_min = max(self.min_radius, R_min_angle)
+
+                if R_min > R_max_angle:
+                    raise ValueError("No valid radius for this turn segment")
+
+                rand_radius = np.random.uniform(R_min, R_max_angle)
+
+                if seg_name.startswith("u_turn"):
+                    curvature_s = seg_function(rand_radius)
+                else:
+                    curvature_s = seg_function(seg_length, rand_radius)
+
+            curvature[current_index:(current_index + seg_length)] = curvature_s
+            current_index += seg_length
+
+            x, y = integrate_road(curvature)
+
+        return x * self.ds, y * self.ds

src/road_gen/integrator/integrator.py

@@ -0,0 +1,21 @@
+import numpy as np
+
+
+def integrate_road(curvature: np.ndarray, ds: float = 1.0):
+    """Integrate along the curvature field to obtain X and Y coordinates of the road.
+
+    Args:
+        curvature (np.ndarray): The curvature field.
+        ds (float, optional): _description_. Defaults to 1.0.
+
+    Returns:
+        _type_: _description_
+    """    
+
+    theta = np.zeros(len(curvature))
+    theta[1:] = np.cumsum(curvature[:-1] * ds)
+
+    x = np.cumsum(np.cos(theta) * ds)
+    y = np.cumsum(np.sin(theta) * ds)
+
+    return x, y

src/road_gen/main.py

@@ -0,0 +1,77 @@
+import argparse
+
+from matplotlib import pyplot as plt
+
+from .generators.random_road_generator import RandomRoadGenerator
+from .generators.segmented_road_generator import SegmentedRoadGenerator
+
+from .plotting.plot_road import plot_road
+
+from .utils import export
+
+def add_common_args(parser):
+    """Add common arguments to a subparser."""
+    parser.add_argument("--length", "-l", type=int, required=True, help="Length of road in meters.")
+    parser.add_argument("--ds", "-s", type=int, required=True, help="Step size in meters.")
+    parser.add_argument("--velocity", "-v", type=float, required=True, help="Velocity in meters per second. Needed to determine the minimum radius of turns.")
+
+    parser.add_argument("--mu", type=float, required=False, help="Friction coefficient. Defaults to 0.7, which represents dry asphalt.")
+    parser.add_argument("--g", type=float, required=False, help="Acceleration due to gravitation. Defaults to 9.81 meters per second.")
+    
+    parser.add_argument("--seed", type=int, required=False, help="Fix a seed for reproducibility")
+    parser.add_argument("--save", action="store_true", required=False, help="Save all outputs. If false, just plots.")
+    
+def main():
+    parser = argparse.ArgumentParser(description="Generate stuff with two methods.")
+    subparsers = parser.add_subparsers(dest="method", required=True)
+
+    random_parser = subparsers.add_parser("random", help="Generate a random road according to parameters.")
+    random_parser.add_argument("--straight_section_prob", type=float, required=False, help="Probability at every step i that a straight section will start. Defaults to 0.05.")
+    random_parser.add_argument("--straight_section_max_rel_size", type=float, required=False, help="The maximum size that straight section(s) can have relative to the total length of the path. Defaults to 0.1.")
+    add_common_args(random_parser)
+    
+    args = parser.parse_args()
+
+    if args.method == "random":
+        try:
+            if not all(v > 0 for v in (args.length, args.ds, args.velocity)):
+                raise ValueError("Length, step size, and velocity must be positive values.")
+
+            init_args = {
+                "length": args.length,
+                "ds": args.ds,
+                "velocity": args.velocity,
+            }
+
+            if args.mu:
+                init_args["mu"] = args.mu
+            if args.g:
+                init_args["g"] = args.g
+            if args.seed:
+                init_args["seed"] = args.seed
+
+            generator = RandomRoadGenerator(**init_args)
+
+            generate_args = {}
+
+            if args.straight_section_prob:
+                generate_args["straight_section_prob"] = float(args.straight_section_prob)
+            if args.straight_section_max_rel_size:
+                generate_args["straight_section_max_rel_size"] = float(args.straight_section_max_rel_size)
+
+            x, y = generator.generate(**generate_args)
+            
+            if args.save:
+                basename = str(generator.seed)
+                plot_road(x, y, generator, save = True, filename = basename+".jpg")
+                export.coords_to_json(x, y, filename = basename+".json")
+                export.params_to_json(generator, filename = basename+".params.json")
+            else:
+                plot_road(x, y, generator)
+
+        except ValueError as e:
+            print(f"Error: {e}")
+            exit(1)
+
+if __name__ == "__main__":
+    main()

src/road_gen/plotting/plot_road.py

@@ -0,0 +1,44 @@
+from matplotlib import pyplot as plt
+
+from ..generators.random_road_generator import RandomRoadGenerator
+from ..generators.segmented_road_generator import SegmentedRoadGenerator
+
+def plot_road(
+    x,
+    y,
+    generator: RandomRoadGenerator | SegmentedRoadGenerator,
+    save: bool = False,
+    filename: str = "out.jpg"
+    ):
+
+    plt.plot(x, y, '-b')
+    plt.xlabel("X (m)")
+    plt.ylabel("Y (m)")
+        
+    title_str = (
+        f"$L = {generator.length}$ m, " +
+        f"$v = {generator.velocity}$ m/s, " +
+        f"$\\Delta s = {generator.ds} m$"
+                 )
+        
+    if isinstance(generator, RandomRoadGenerator):
+        title_str += (
+            "\n" + 
+            f"Prob(straight) $= {generator.straight_section_prob}$, " +
+            f"Max rel. size $= {generator.straight_section_max_rel_size}$"
+        )
+    else:
+        title_str += (
+            f"$, \\alpha = {generator.alpha}$" +
+            "\n" +
+            str(generator.segments)
+            )
+
+    plt.title(title_str)
+
+    plt.tight_layout()
+    
+    if save:
+        plt.savefig(filename)
+    else:
+        plt.show()

src/road_gen/prefabs/prefabs.py

@@ -0,0 +1,19 @@
+import numpy as np
+
+def straight(length: int) -> np.ndarray:
+    return np.zeros(length)
+
+def turn_left(length: int, radius: float) -> np.ndarray:
+    return np.full(length, 1.0 / radius)
+
+def turn_right(length: int, radius: float) -> np.ndarray:
+    return - turn_left(length, radius)
+
+PREFABS = {
+    'straight': straight,
+    'turn_left': turn_left,
+    'turn_right': turn_right,
+}
+
+def print_available_prefabs():
+    print(PREFABS.keys())

src/road_gen/utils/export.py

@@ -0,0 +1,49 @@
+import json
+
+import numpy
+
+from ..generators.random_road_generator import RandomRoadGenerator
+from ..generators.segmented_road_generator import SegmentedRoadGenerator
+
+def coords_to_json(
+    x: numpy.ndarray,
+    y: numpy.ndarray,
+    filename: str = "output.json",
+    x_key: str = "x",
+    y_key: str = "y"
+) -> str:
+    """Convert x and y arrays of waypoints to a JSON.
+
+    Args:
+        x (numpy.ndarray): x coordinates.
+        y (numpy.ndarray): y coordinates.
+        x_key (str, optional): Key for x coordinates. Defaults to "x".
+        y_key (str, optional): Key for y coordinates. Defaults to "y".
+
+    Returns:
+        str: JSON string containing the data.
+    """
+    data = {
+        x_key: x.tolist(),  # Convert numpy array to list
+        y_key: y.tolist()
+    }
+
+    with open(filename, "w") as file:
+        json.dump(data, file, indent=4)
+        
+def params_to_json(
+    generator: RandomRoadGenerator | SegmentedRoadGenerator,
+    filename: str = "output.json"
+):
+
+    params = {}
+    
+    for attr in dir(generator):
+        # skip private and special attributes
+        if not attr.startswith('_'):
+            value = getattr(generator, attr)
+            if not callable(value):
+                params[attr] = value
+
+    with open(filename, "w") as file:
+        json.dump(params, file, indent=4)