Added cost map constructor

src/components/mapping/cost/cost_grid_mapper.py

@@ -0,0 +1,63 @@
+"""
+cost_grid_mapper.py
+
+Author: Bhavesh Lokesh Agarwal
+"""
+
+from cost_map import CostMap
+
+
+class CostGridMapper:
+    """
+    Cost grid map construction class
+    """
+
+    def __init__(self, width_m=60.0, height_m=60.0, resolution_m=1.0,
+                 center_x_m=0.0, center_y_m=0.0, sensor_params=None,
+                 max_cost=100.0, obstacle_cost=50.0, free_cost=0.0):
+        """
+        Constructor
+        width_m: Width size of map[m]
+        height_m: Height size of map[m]
+        resolution: Size of each cells[m]
+        center_x_m: Center x position of map[m]
+        center_y_m: Center y position of map[m]
+        sensor_params: Parameters object of sensor
+        max_cost: Maximum cost value
+        obstacle_cost: Cost value for obstacles
+        free_cost: Cost value for free space
+        """
+
+        self.map = CostMap(width_m, height_m, resolution_m, center_x_m, center_y_m,
+                          max_cost, obstacle_cost, free_cost)
+        self.params = sensor_params
+
+    def update(self, point_cloud, state, cost_increment=1.0):
+        """
+        Function to update cost grid map
+        point_cloud: List of points from LiDAR
+        state: Vehicle's state to transform into global coordinate
+        cost_increment: Cost value to add per hit
+        """
+
+        vehicle_pose = state.x_y_yaw()
+
+        points_x_list, points_y_list = [], []
+        for point in point_cloud:
+            global_x, global_y = point.get_transformed_data(self.params.INST_LON_M, self.params.INST_LAT_M, self.params.INST_YAW_RAD,
+                                                            vehicle_pose[0, 0], vehicle_pose[1, 0], vehicle_pose[2, 0])
+            points_x_list.append(global_x)
+            points_y_list.append(global_y)
+
+        self.map.update_map(points_x_list, points_y_list, cost_increment)
+
+    def draw(self, axes, elems, colormap='RdYlGn_r'):
+        """
+        Function to draw cost map data
+        axes: Axes object of figure
+        elems: List of plot object
+        colormap: Matplotlib colormap name for visualization
+        """
+
+        self.map.draw_map(axes, elems, colormap)
+

src/components/mapping/cost/cost_map.py

@@ -0,0 +1,130 @@
+"""
+cost_map.py
+
+Author: Bhavesh Lokesh Agarwal
+"""
+
+import numpy as np
+import sys
+from pathlib import Path
+import matplotlib.patches as patches
+import matplotlib.cm as cm
+sys.path.append(str(Path(__file__).absolute().parent) + "/../grid")
+from grid_map import GridMap
+from grid_map import FloatGrid
+
+
+class CostMap:
+    """
+    Cost grid map class
+    """
+
+    def __init__(self, width_m=60.0, height_m=60.0, resolution_m=1.0,
+                 center_x_m=0.0, center_y_m=0.0, max_cost=100.0, 
+                 obstacle_cost=50.0, free_cost=0.0):
+        """
+        Constructor
+        width_m: Width size of map[m]
+        height_m: Height size of map[m]
+        resolution_m: Size of each cells[m]
+        center_x_m: Center x position of map[m]
+        center_y_m: Center y position of map[m]
+        max_cost: Maximum cost value
+        obstacle_cost: Cost value for obstacles
+        free_cost: Cost value for free space
+        """
+
+        self.map = GridMap(width_m, height_m, resolution_m,
+                           center_x_m, center_y_m)
+        self.max_cost = max_cost
+        self.obstacle_cost = obstacle_cost
+        self.free_cost = free_cost
+
+    def update_map(self, points_x_list, points_y_list, cost_increment=1.0):
+        """
+        Function to update cost map by adding cost to cells with obstacles
+        points_x_list: List of x coordinates of point cloud
+        points_y_list: List of y coordinates of point cloud
+        cost_increment: Cost value to add per hit
+        """
+
+        points_x_array, points_y_array = np.array(points_x_list), np.array(points_y_list)
+
+        cells_hit = {}
+
+        for i in range(len(points_x_array)):
+            index = self.map.calculate_vector_index_from_position(points_x_array[i],
+                                                                  points_y_array[i])
+            if index is not None:
+                if index not in cells_hit:
+                    cells_hit[index] = 0
+                cells_hit[index] += 1
+
+        for index, hit_count in cells_hit.items():
+            current_grid = self.map.get_grid_data(index)
+            current_cost = current_grid.get_data()
+
+            # Increment cost based on hits, but cap at max_cost
+            new_cost = min(current_cost + (cost_increment * hit_count), self.max_cost)
+            self.map.set_grid_data(index, FloatGrid(value=new_cost))
+
+    def set_cost(self, x_m, y_m, cost_value):
+        """
+        Function to set cost value at a specific position
+        x_m: x coordinate position[m]
+        y_m: y coordinate position[m]
+        cost_value: Cost value to set
+        """
+
+        index = self.map.calculate_vector_index_from_position(x_m, y_m)
+        if index is not None:
+            clamped_cost = max(self.free_cost, min(cost_value, self.max_cost))
+            self.map.set_grid_data(index, FloatGrid(value=clamped_cost))
+
+    def get_cost(self, x_m, y_m):
+        """
+        Function to get cost value at a specific position
+        x_m: x coordinate position[m]
+        y_m: y coordinate position[m]
+        Return: Cost value at position, or None if out of bounds
+        """
+
+        index = self.map.calculate_vector_index_from_position(x_m, y_m)
+        if index is not None:
+            return self.map.get_grid_data(index).get_data()
+        return None
+
+    def draw_map(self, axes, elems, colormap='RdYlGn_r'):
+        """
+        Function to draw cost map data with color gradient
+        axes: Axes object of figure
+        elems: List of plot object
+        colormap: Matplotlib colormap name (default: 'RdYlGn_r' for red-yellow-green reversed)
+        """
+
+        # Normalize cost values for colormap (0 to 1)
+        cost_normalizer = cm.ScalarMappable(cmap=colormap)
+        cost_normalizer.set_clim(vmin=self.free_cost, vmax=self.max_cost)
+
+        for vector_idx in range(self.map.all_grids_num):
+            grid_data = self.map.get_grid_data(vector_idx)
+            cost_value = grid_data.get_data()
+
+            if cost_value > self.free_cost:
+                center_x, center_y = self.map.calculate_grid_center_xy_pos_from_vector_index(vector_idx)
+                bottom_left_x = center_x - self.map.resolution_m / 2.0
+                bottom_left_y = center_y - self.map.resolution_m / 2.0
+
+                # Get color based on cost value
+                rgba = cost_normalizer.to_rgba(cost_value)
+
+                rect = patches.Rectangle((bottom_left_x, bottom_left_y),
+                                        self.map.resolution_m,
+                                        self.map.resolution_m,
+                                        linewidth=0.1,
+                                        edgecolor='k',
+                                        facecolor=rgba,
+                                        alpha=0.7)
+                axes.add_patch(rect)
+                elems.append(rect)
+

src/simulations/mapping/cost_grid_map_construction/cost_grid_map_construction.py

@@ -0,0 +1,103 @@
+"""
+cost_grid_map_construction.py
+
+Author: Bhavesh Lokesh Agarwal
+"""
+
+# import path setting
+import sys
+import numpy as np
+from pathlib import Path
+
+abs_dir_path = str(Path(__file__).absolute().parent)
+relative_path = "/../../../components/"
+
+sys.path.append(abs_dir_path + relative_path + "visualization")
+sys.path.append(abs_dir_path + relative_path + "state")
+sys.path.append(abs_dir_path + relative_path + "vehicle")
+sys.path.append(abs_dir_path + relative_path + "obstacle")
+sys.path.append(abs_dir_path + relative_path + "sensors")
+sys.path.append(abs_dir_path + relative_path + "sensors/lidar")
+sys.path.append(abs_dir_path + relative_path + "mapping/cost")
+sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course")
+sys.path.append(abs_dir_path + relative_path + "control/pure_pursuit")
+
+
+# import component modules
+from global_xy_visualizer import GlobalXYVisualizer
+from min_max import MinMax
+from time_parameters import TimeParameters
+from vehicle_specification import VehicleSpecification
+from state import State
+from four_wheels_vehicle import FourWheelsVehicle
+from obstacle import Obstacle
+from obstacle_list import ObstacleList
+from sensors import Sensors
+from sensor_parameters import SensorParameters
+from omni_directional_lidar import OmniDirectionalLidar
+from cost_grid_mapper import CostGridMapper
+from cubic_spline_course import CubicSplineCourse
+from pure_pursuit_controller import PurePursuitController
+
+
+# flag to show plot figure
+# when executed as unit test, this flag is set as false
+show_plot = True
+
+
+def main():
+    """
+    Main process function
+    """
+
+    # set simulation parameters
+    x_lim, y_lim = MinMax(-5, 55), MinMax(-20, 25)
+    vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25), show_zoom=False)
+
+    # create course data instance
+    course = CubicSplineCourse([0.0, 10.0, 25, 40, 50],
+                               [0.0, 4, -12, 20, -13],
+                               20)
+    vis.add_object(course)
+
+    # create obstacle instances
+    obst_list = ObstacleList()
+    obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=15.0), length_m=10, width_m=8))
+    obst_list.add_obstacle(Obstacle(State(x_m=40.0, y_m=0.0), length_m=2, width_m=10))
+    obst_list.add_obstacle(Obstacle(State(x_m=10.0, y_m=-10.0, yaw_rad=np.rad2deg(45)), length_m=5, width_m=5))
+    obst_list.add_obstacle(Obstacle(State(x_m=30.0, y_m=15.0, yaw_rad=np.rad2deg(10)), length_m=5, width_m=2))
+    obst_list.add_obstacle(Obstacle(State(x_m=50.0, y_m=15.0, yaw_rad=np.rad2deg(15)), length_m=5, width_m=2))
+    obst_list.add_obstacle(Obstacle(State(x_m=25.0, y_m=0.0), length_m=2, width_m=2))
+    obst_list.add_obstacle(Obstacle(State(x_m=35.0, y_m=-15.0), length_m=7, width_m=2))
+    vis.add_object(obst_list)
+
+    # create vehicle instance with cost grid mapper
+    spec = VehicleSpecification()
+    pure_pursuit = PurePursuitController(spec, course)
+    sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2, max_m=15, dist_std_rate=0.05)
+    lidar = OmniDirectionalLidar(obst_list, sensor_params)
+    # Create cost grid mapper with configurable cost parameters
+    mapper = CostGridMapper(sensor_params=sensor_params, 
+                           center_x_m=25.0, 
+                           center_y_m=5.0,
+                           max_cost=100.0,
+                           obstacle_cost=50.0,
+                           free_cost=0.0)
+    vehicle = FourWheelsVehicle(State(color=spec.color), spec,
+                                controller=pure_pursuit,
+                                sensors=Sensors(lidar=lidar),
+                                mapper=mapper,
+                                show_zoom=False)
+    vis.add_object(vehicle)
+
+    # plot figure is not shown when executed as unit test
+    if not show_plot: vis.not_show_plot()
+
+    # show plot figure
+    vis.draw()
+
+
+# execute main process
+if __name__ == "__main__":
+    main()
+

test/test_cost_grid_map_construction.py

@@ -0,0 +1,19 @@
+"""
+Test of Cost grid map construction
+
+Author: Bhavesh Lokesh Agarwal
+"""
+
+from pathlib import Path
+import sys
+import pytest
+
+sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/mapping/cost_grid_map_construction")
+import cost_grid_map_construction
+
+
+def test_simulation():
+    cost_grid_map_construction.show_plot = False
+
+    cost_grid_map_construction.main()
+