From 25384775b8b6a3c64c00fd3e84e08d1d9c2cace4 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 10 Dec 2024 04:43:44 -0500 Subject: [PATCH 01/35] Add old osm files --- utils/osm_dataset_generator/CachingNone.py | 19 ++ utils/osm_dataset_generator/README.md | 17 ++ .../generate_50cities_dataset.py | 84 +++++++ .../osm_dataset_generator/generate_dataset.py | 31 +++ .../osm_dataset_generator/generate_osm_map.py | 211 ++++++++++++++++++ utils/osm_dataset_generator/idf.py | 84 +++++++ .../leaflet_geojson_viz/index.html | 23 ++ .../leaflet_geojson_viz/main.js | 64 ++++++ .../leaflet_geojson_viz/styles.css | 33 +++ utils/osm_dataset_generator/semantic_map.py | 91 ++++++++ .../semantic_objects.yaml | 24 ++ 11 files changed, 681 insertions(+) create mode 100644 utils/osm_dataset_generator/CachingNone.py create mode 100644 utils/osm_dataset_generator/README.md create mode 100644 utils/osm_dataset_generator/generate_50cities_dataset.py create mode 100644 utils/osm_dataset_generator/generate_dataset.py create mode 100644 utils/osm_dataset_generator/generate_osm_map.py create mode 100644 utils/osm_dataset_generator/idf.py create mode 100644 utils/osm_dataset_generator/leaflet_geojson_viz/index.html create mode 100644 utils/osm_dataset_generator/leaflet_geojson_viz/main.js create mode 100644 utils/osm_dataset_generator/leaflet_geojson_viz/styles.css create mode 100644 utils/osm_dataset_generator/semantic_map.py create mode 100644 utils/osm_dataset_generator/semantic_objects.yaml diff --git a/utils/osm_dataset_generator/CachingNone.py b/utils/osm_dataset_generator/CachingNone.py new file mode 100644 index 00000000..62555932 --- /dev/null +++ b/utils/osm_dataset_generator/CachingNone.py @@ -0,0 +1,19 @@ +import os.path +import ujson + +from OSMPythonTools.cachingStrategy.base import CachingStrategyBase + +class CachingNone(CachingStrategyBase): + def __init__(self, cacheDir='cache'): + self._cacheDir = cacheDir + + def _filename(self, key): + return os.path.join(self._cacheDir, key) + + def get(self, key): + filename = self._filename(key) + data = None + return data + + def set(self, key, value): + return diff --git a/utils/osm_dataset_generator/README.md b/utils/osm_dataset_generator/README.md new file mode 100644 index 00000000..868ee6d4 --- /dev/null +++ b/utils/osm_dataset_generator/README.md @@ -0,0 +1,17 @@ +See the python script `generate_osm_map.py`. +The function OverpassOSMQuery requires a params file, an origin, and a filename to save data. +The origin is the bottom left corner of the bounding box. +The semantic data is stored in the geoJSON format. + +The semantic data can be visualized using the web interface in `leaflet_geojson_viz` directory. +Check semantic data in `leaflet_geojson_viz/data/semantic_data.json`. Create the `data` directory first else it gives an error while saving data +Then launch a local server inside `leaflet_geojson_viz` directory using `python -m http.server` +Open the link shown in the above step in a browser. + +The OSM planet data can be hosted on a local server: +https://hub.docker.com/r/wiktorn/overpass-api + +```bash +docker run -e OVERPASS_META=yes -e OVERPASS_MODE=clone -e OVERPASS_DIFF_URL=https://planet.openstreetmap.org/replication/minute/ -v /mnt/data/osm_overpass_db/:/db -p 12346:80 -i -t --name overpass_world wiktorn/overpass-api +docker start overpass-api +``` diff --git a/utils/osm_dataset_generator/generate_50cities_dataset.py b/utils/osm_dataset_generator/generate_50cities_dataset.py new file mode 100644 index 00000000..3d0e7ca3 --- /dev/null +++ b/utils/osm_dataset_generator/generate_50cities_dataset.py @@ -0,0 +1,84 @@ +import generate_osm_map +import random +import pickle +import yaml +import os +import csv +import numpy as np +import math +import pyCoverageControl # Main library +from pyCoverageControl import BivariateNormalDistribution as BND # for defining bivariate normal distributions +from pyCoverageControl import GeoLocalTransform as GeoTransform +from pyCoverageControl import WorldIDF # for defining world idf +import json +import geojson +from pyCoverageControl import CoverageSystem +from pyCoverageControl import Point2 # for defining points +from pyCoverageControl import PointVector # for defining list of points + + +dataset_base_dir = '/root/CoverageControl_ws/results/50_cities/' +dataset_filename = 'cities_origins' + +params_filename = dataset_base_dir + 'semantic_objects.yaml' +path_to_dataset = dataset_base_dir + '/envs/' + dataset_filename + +with open(params_filename, 'r') as file: + params = yaml.safe_load(file) + +with open(path_to_dataset, 'r') as file: + csv_reader = csv.reader(file, delimiter='\t') + city_origin_points = [] + city_names = [] + for row in csv_reader: + city_origin_points.append({'lat': float(row[1]), 'lon': float(row[2])}) + city_names.append(row[0]) + +params_ = pyCoverageControl.Parameters(dataset_base_dir + '/env_params.yaml') +count = 0 +data_count = 0 +no_data_count = 0 +for i in range(data_count, len(city_origin_points)): +# for i in range(data_count, 5): + origin = city_origin_points[i] + print(city_names[i]) + # print(origin) + data_path = dataset_base_dir + '/envs/' + city_names[i] + semantic_data_filename = data_path + '/semantic_data.json' + print(semantic_data_filename) + if not os.path.exists(data_path): + os.makedirs(data_path) + if(generate_osm_map.OverpassOSMQuery(params, origin, semantic_data_filename) == False): + no_data_count = no_data_count + 1 + print('=====================No data for ' + city_names[i]) + count = count + 1 + world_idf = WorldIDF(params_) + with open(semantic_data_filename) as file_: + semantic_data = geojson.load(file_) + + [origin_lon, origin_lat] = semantic_data.bbox[0:2] + origin_alt = 0 + geo_transform = GeoTransform(origin_lat, origin_lon, origin_alt) + + for feature in semantic_data.features: + if(feature['properties']['type'] == "traffic_signal"): + coords = feature['geometry']['coordinates'] + mean = geo_transform.Forward(coords[1], coords[0], origin_alt) + + traffic_signals_sigma = random.uniform(params_.pMinSigma, params_.pMaxSigma) + traffic_signals_scale = random.uniform(params_.pMinPeak, params_.pMaxPeak) + + dist = BND(mean[0:2], traffic_signals_sigma, traffic_signals_scale) # circular gaussian + world_idf.AddNormalDistribution(dist) + + world_idf.GenerateMapCuda() + robot_positions = PointVector() + for i in range(params_.pNumRobots): + robot_positions.append(np.array([random.uniform(0, params_.pRobotInitDist), random.uniform(0, params_.pRobotInitDist)])) + env = CoverageSystem(params_, world_idf, robot_positions) + env.WriteEnvironment(data_path + '/pos.dat', data_path + '/idf.dat') + env.PlotWorldMap(data_path, 'idf') + env.PlotInitMap(data_path, 'InitMap') + +print('Total cities: ' + str(count)) +print('Cities with no data: ' + str(no_data_count)) diff --git a/utils/osm_dataset_generator/generate_dataset.py b/utils/osm_dataset_generator/generate_dataset.py new file mode 100644 index 00000000..c70e7129 --- /dev/null +++ b/utils/osm_dataset_generator/generate_dataset.py @@ -0,0 +1,31 @@ +import generate_osm_map +import pickle +import yaml +import os + +dataset_base_dir = '../../../data/osm_city_data/' +dataset_filename = 'km_region_origin_points' +path_to_dataset = dataset_base_dir + dataset_filename + +params_filename = 'semantic_objects.yaml' + +with open(params_filename, 'r') as file: + params = yaml.safe_load(file) + +with open(path_to_dataset, 'rb') as file: + city_origin_points = pickle.load(file) + +print("No. of points: ", len(city_origin_points)) +count = 0 +data_count = 0 +for i in range(data_count, len(city_origin_points)): + origin = city_origin_points[i] + print(i, " lat: ", origin['lat'], " lon: ", origin['lon']) + data_path = dataset_base_dir + str(count) + semantic_data_filename = data_path + '/semantic_data.json' + if not os.path.exists(data_path): + os.makedirs(data_path) + if(generate_osm_map.OverpassOSMQuery(params, origin, semantic_data_filename) == False): + continue + count = count + 1 + print(str(count)) diff --git a/utils/osm_dataset_generator/generate_osm_map.py b/utils/osm_dataset_generator/generate_osm_map.py new file mode 100644 index 00000000..241db08e --- /dev/null +++ b/utils/osm_dataset_generator/generate_osm_map.py @@ -0,0 +1,211 @@ +# Requirements: +# - OSMPythonTools +# - geopy +# - geojson +# - geographiclib +# - pyyaml +# - shapely + + +import sys +import math +import yaml +import json +import geojson +import shapely +from shapely.geometry import LineString, Polygon +from shapely.ops import split + +from geographiclib.geodesic import Geodesic +from OSMPythonTools.api import Api as OSMApi +from OSMPythonTools.nominatim import Nominatim +from OSMPythonTools.overpass import Overpass, overpassQueryBuilder +from OSMPythonTools.data import Data, dictRangeYears, ALL +from OSMPythonTools.cachingStrategy import CachingStrategy, JSON, Pickle +from CachingNone import CachingNone + + +def ClipWay(bbox, way): + polyline = LineString(way) + polygon = Polygon(bbox) + way_split = shapely.ops.split(polyline, polygon) + clipped_ways = [] + for shapely_way in way_split.geoms: + clipped_way_coords = [] + for pt in shapely_way.coords: + if pt[0] < bbox[0][0] or pt[0] > bbox[2][0] or pt[1] < bbox[0][1] or pt[1] > bbox[2][1]: + break + clipped_way_coords.append([pt[0], pt[1]]) + if len(clipped_way_coords) > 1: + clipped_ways.append(clipped_way_coords) + return clipped_ways + + +def OverpassOSMQuery(params, origin, semantic_data_filename): + + contains_data = False + + osmApi = OSMApi() + overpass = Overpass() + osmApi = OSMApi(endpoint="http://localhost/api/") + overpass = Overpass(endpoint="http://localhost/api/") + # CachingStrategy.use(CachingNone) + geod = Geodesic.WGS84 + + feature_collection = geojson.FeatureCollection([]) + + area_dim = params['area_dim'] + + relative_pos = geod.Direct(origin['lat'], origin['lon'], 0, area_dim) + bottom_right = {'lat': relative_pos['lat2'], 'lon': relative_pos['lon2']} + relative_pos = geod.Direct(bottom_right['lat'], bottom_right['lon'], 90, area_dim) + top_right = {'lat': relative_pos['lat2'], 'lon': relative_pos['lon2']} + relative_pos = geod.Direct(top_right['lat'], top_right['lon'], 180, area_dim) + top_left = {'lat': relative_pos['lat2'], 'lon': relative_pos['lon2']} + poly_bounds = [origin, bottom_right, top_right, top_left] + feature_collection['bbox'] = [origin['lon'], origin['lat'], top_right['lon'], top_right['lat']] + bounding_polygon = [(origin['lon'], origin['lat']), (bottom_right['lon'], bottom_right['lat']), (top_right['lon'], top_right['lat']), (top_left['lon'], top_left['lat'])] + + feature_collection['center'] = [(origin['lat'] + top_right['lat'])/2., (origin['lon'] + top_right['lon'])/2.] + + out_str = "(._;>;);out geom;" + bounds_str = '(poly: "' + for i in range(0, len(poly_bounds)): + pt = poly_bounds[i] + if i == 0: + bounds_str += str(pt['lat']) + ' ' + str(pt['lon']) + else: + bounds_str += ' ' + str(pt['lat']) + ' ' + str(pt['lon']) + bounds_str += '");' + + semantic_objects = params['semantic_objects'] + road_network_query = 'way[highway~"^(' + # Check if tags are present in the semantic data file + if 'road_network' in semantic_objects: + road_network_tags = semantic_objects['road_network'] + for iTag in range(0, len(road_network_tags)): + if iTag > 0: + road_network_query += '|' + road_network_query += road_network_tags[iTag] + road_network_query += ')$"]' + bounds_str + out_str + road_network = overpass.query(road_network_query) + print(type(road_network)) + + if road_network.ways(): + contains_data = True + for ways in road_network.ways(): + ways_coordinates = ways.geometry()["coordinates"] + if ways.geometry()["type"] == "LineString": + ways_coordinates = [ways_coordinates] + clipped_ways_list = [] + for way_coords in ways_coordinates: + clipped_ways = ClipWay(bounding_polygon, way_coords) + for clipped_way in clipped_ways: + clipped_ways_list.append(clipped_way) + for clipped_way in clipped_ways_list: + if len(clipped_way) > 0: + way_feature = geojson.Feature(geometry = geojson.LineString(clipped_way), id = ways.id(), properties={"type": "road_network", "subtype": ways.tags()['highway'], "osmtype": "way"}) + feature_collection.features.append(way_feature) + + leisure_query = 'nwr[leisure~"^(' + if 'leisure' in semantic_objects: + leisure_tags = semantic_objects['leisure'] + if leisure_tags != None: + for iTag in range(0, len(leisure_tags)): + if iTag > 0: + leisure_query += '|' + leisure_query += leisure_tags[iTag] + leisure_query += ')$"]' + bounds_str + out_str + leisure = overpass.query(leisure_query) + leisure_json = leisure.toJSON() + + if leisure.ways(): + contains_data = True + for way in leisure.ways(): + tags = way.tags() + if tags == None or 'leisure' not in tags: + continue + leisure_tag = way.tags()['leisure'] + if leisure_tag in semantic_objects['leisure']: + coords_list = way.geometry()["coordinates"] + poly_feature = geojson.Feature(geometry = geojson.Polygon(coords_list), id = way.id(), properties={"type": "leisure", "subtype": leisure_tag, "osmtype": "way"}) + feature_collection.features.append(poly_feature) + + amenity_query = 'nwr[amenity~"^(' + if 'amenity' in semantic_objects: + amenity_tags = semantic_objects['amenity'] + if amenity_tags != None: + for iTag in range(0, len(amenity_tags)): + if iTag > 0: + amenity_query += '|' + amenity_query += amenity_tags[iTag] + amenity_query += ')$"]' + bounds_str + out_str + amenity = overpass.query(amenity_query) + amenity_json = amenity.toJSON() + + if amenity.ways(): + contains_data = True + for way in amenity.ways(): + tags = way.tags() + if tags == None or 'amenity' not in tags: + continue + amenity_tag = way.tags()['amenity'] + if amenity_tag in semantic_objects['amenity']: + if 'parking' in tags and tags['parking'] == 'underground': + continue + coords_list = way.geometry()["coordinates"] + poly_feature = geojson.Feature(geometry = geojson.Polygon(coords_list), id = way.id(), properties={"type": "amenity", "subtype": amenity_tag, "osmtype": "way"}) + feature_collection.features.append(poly_feature) + + if amenity.relations(): + contains_data = True + for relation in amenity.relations(): + try: + geom = relation.geometry() + except: + continue + contains_data = True + tags = relation.tags() + if tags == None or 'amenity' not in tags: + continue + amenity_tag = relation.tags()['amenity'] + if amenity_tag in semantic_objects['amenity']: + coords_list = geom["coordinates"] + if geom["type"] == "Polygon": + coords_list = [coords_list] + for coords in coords_list: + poly_feature = geojson.Feature(geometry = geojson.Polygon(coords), id = relation.id(), properties={"type": "amenity", "subtype": amenity_tag, "osmtype": "relation"}) + feature_collection.features.append(poly_feature) + + if semantic_objects['traffic_signals'] == True: + traffic_signal_query = 'node[highway~"^(traffic_signals)$"]' + bounds_str + out_str + traffic_signals = overpass.query(traffic_signal_query) + + if traffic_signals.nodes(): + contains_data = True + for node in traffic_signals.nodes(): + feature = geojson.Feature(geometry=geojson.Point((node.lon(), node.lat())), id=node.id(), properties={"type": "traffic_signal", "osmtype": "node"}) + feature_collection.features.append(feature) + + if contains_data == False: + return False + + with open(semantic_data_filename, 'w', encoding='utf-8') as f: + geojson.dump(feature_collection, f) + + return True + + +if __name__ == '__main__': + params_filename = 'semantic_objects.yaml' + if len(sys.argv) > 1: + params_filename = sys.argv[1] + with open(params_filename, 'r') as file: + params = yaml.safe_load(file) + + origin = {'lat': 39.945827951386065, 'lon': -75.2047706396303} # UPenn + # origin = {'lat': 40.74050005471615, 'lon': -74.1759877275644} # New York + # origin = {'lat': 41.381775552849454, 'lon': -73.96846537685275} # West Point + + semantic_data_filename = 'leaflet_geojson_viz/data/semantic_data.json' + OverpassOSMQuery(params, origin, semantic_data_filename) diff --git a/utils/osm_dataset_generator/idf.py b/utils/osm_dataset_generator/idf.py new file mode 100644 index 00000000..3d0e7ca3 --- /dev/null +++ b/utils/osm_dataset_generator/idf.py @@ -0,0 +1,84 @@ +import generate_osm_map +import random +import pickle +import yaml +import os +import csv +import numpy as np +import math +import pyCoverageControl # Main library +from pyCoverageControl import BivariateNormalDistribution as BND # for defining bivariate normal distributions +from pyCoverageControl import GeoLocalTransform as GeoTransform +from pyCoverageControl import WorldIDF # for defining world idf +import json +import geojson +from pyCoverageControl import CoverageSystem +from pyCoverageControl import Point2 # for defining points +from pyCoverageControl import PointVector # for defining list of points + + +dataset_base_dir = '/root/CoverageControl_ws/results/50_cities/' +dataset_filename = 'cities_origins' + +params_filename = dataset_base_dir + 'semantic_objects.yaml' +path_to_dataset = dataset_base_dir + '/envs/' + dataset_filename + +with open(params_filename, 'r') as file: + params = yaml.safe_load(file) + +with open(path_to_dataset, 'r') as file: + csv_reader = csv.reader(file, delimiter='\t') + city_origin_points = [] + city_names = [] + for row in csv_reader: + city_origin_points.append({'lat': float(row[1]), 'lon': float(row[2])}) + city_names.append(row[0]) + +params_ = pyCoverageControl.Parameters(dataset_base_dir + '/env_params.yaml') +count = 0 +data_count = 0 +no_data_count = 0 +for i in range(data_count, len(city_origin_points)): +# for i in range(data_count, 5): + origin = city_origin_points[i] + print(city_names[i]) + # print(origin) + data_path = dataset_base_dir + '/envs/' + city_names[i] + semantic_data_filename = data_path + '/semantic_data.json' + print(semantic_data_filename) + if not os.path.exists(data_path): + os.makedirs(data_path) + if(generate_osm_map.OverpassOSMQuery(params, origin, semantic_data_filename) == False): + no_data_count = no_data_count + 1 + print('=====================No data for ' + city_names[i]) + count = count + 1 + world_idf = WorldIDF(params_) + with open(semantic_data_filename) as file_: + semantic_data = geojson.load(file_) + + [origin_lon, origin_lat] = semantic_data.bbox[0:2] + origin_alt = 0 + geo_transform = GeoTransform(origin_lat, origin_lon, origin_alt) + + for feature in semantic_data.features: + if(feature['properties']['type'] == "traffic_signal"): + coords = feature['geometry']['coordinates'] + mean = geo_transform.Forward(coords[1], coords[0], origin_alt) + + traffic_signals_sigma = random.uniform(params_.pMinSigma, params_.pMaxSigma) + traffic_signals_scale = random.uniform(params_.pMinPeak, params_.pMaxPeak) + + dist = BND(mean[0:2], traffic_signals_sigma, traffic_signals_scale) # circular gaussian + world_idf.AddNormalDistribution(dist) + + world_idf.GenerateMapCuda() + robot_positions = PointVector() + for i in range(params_.pNumRobots): + robot_positions.append(np.array([random.uniform(0, params_.pRobotInitDist), random.uniform(0, params_.pRobotInitDist)])) + env = CoverageSystem(params_, world_idf, robot_positions) + env.WriteEnvironment(data_path + '/pos.dat', data_path + '/idf.dat') + env.PlotWorldMap(data_path, 'idf') + env.PlotInitMap(data_path, 'InitMap') + +print('Total cities: ' + str(count)) +print('Cities with no data: ' + str(no_data_count)) diff --git a/utils/osm_dataset_generator/leaflet_geojson_viz/index.html b/utils/osm_dataset_generator/leaflet_geojson_viz/index.html new file mode 100644 index 00000000..18dd288b --- /dev/null +++ b/utils/osm_dataset_generator/leaflet_geojson_viz/index.html @@ -0,0 +1,23 @@ + + + + + OSM Semantic Data Visualizer + + + + + + + + + + + + + + +
+ + + diff --git a/utils/osm_dataset_generator/leaflet_geojson_viz/main.js b/utils/osm_dataset_generator/leaflet_geojson_viz/main.js new file mode 100644 index 00000000..b323eb5d --- /dev/null +++ b/utils/osm_dataset_generator/leaflet_geojson_viz/main.js @@ -0,0 +1,64 @@ +/** + * + * @author Saurav Agarwal + * @contact SauravAg@UPenn.edu + * @contact agr.saurav1@gmail.com + * + */ + +var map = L.map('map').setView([39.9577545, -75.1883271], 16); +var map_data_json; +var overpass_query_string = 'highway~"^(motorway|motorway_link|trunk|trunk_link|primary|secondary|tertiary|unclassified|residential)$"'; + +L.tileLayer('https://api.mapbox.com/styles/v1/{id}/tiles/{z}/{x}/{y}?access_token=pk.eyJ1Ijoic2FnYXJ3MTAiLCJhIjoiY2t4MWU0eHprMWR4ZDJ1cW94ZjRkamM1bCJ9.b5dLgGYJuIxpIR5woCF8lw', { + maxZoom: 20, + attribution: 'Saurav Agarwal | Map data © OpenStreetMap contributors, ' + + 'Imagery © Mapbox', + id: 'mapbox/streets-v11', + tileSize: 512, + zoomOffset: -1 +}).addTo(map); + + + +var geojsonMarkerOptions = { + radius: 8, + fillColor: "#d62728", + color: "#000", + weight: 2, + opacity: 1, + fillOpacity: 0.8, + marker: 'circle' +}; + +stylefn = function(feature) { + if(feature.properties.type == "road_network") { + return { weight: 2 } + } + if(feature.properties.type == "traffic_signal") { + return geojsonMarkerOptions; + } + if(feature.properties.type == "amenity") { + switch(feature.properties.subtype) { + case 'parking': return {fillColor: '#8c564b', color: 'black', weight: 2, fillOpacity: 0.6}; + case 'hospital': return {fillColor: '#1b4f72', color: 'black', weight: 2, fillOpacity: 0.6}; + case 'fuel': return {fillColor: '#9467bd', color: 'black', weight: 2, fillOpacity: 0.6}; + } + } +} + +var semantic_data; +fetch("data/semantic_data.json") + .then(response => response.json()) + .then(data => { + semantic_data = data; + L.geoJson(data, { + pointToLayer: function (feature, latlng) { + return L.circleMarker(latlng, geojsonMarkerOptions); + }, + style: stylefn + }).addTo(map); + // bbox = semantic_data.bbox; + // L.rectangle([[bbox[1], bbox[0]], [bbox[3], bbox[2]]], {color: "#ff7800", weight: 1}).addTo(map); + map.flyTo(semantic_data.center, 16); + }); diff --git a/utils/osm_dataset_generator/leaflet_geojson_viz/styles.css b/utils/osm_dataset_generator/leaflet_geojson_viz/styles.css new file mode 100644 index 00000000..8658fb00 --- /dev/null +++ b/utils/osm_dataset_generator/leaflet_geojson_viz/styles.css @@ -0,0 +1,33 @@ +body { + padding: 0; + margin: 0; +} + +html, body, #map { + height: 100%; + width: 100%; +} + +.leaflet-fade-anim .leaflet-tile,.leaflet-zoom-anim .leaflet-zoom-animated { will-change:auto !important; } + +.download-icon { + background-image: url(data:image/svg+xml;utf8;base64,PD94bWwgdmVyc2lvbj0iMS4wIiBlbmNvZGluZz0iaXNvLTg4NTktMSI/Pgo8IS0tIEdlbmVyYXRvcjogQWRvYmUgSWxsdXN0cmF0b3IgMTYuMC4wLCBTVkcgRXhwb3J0IFBsdWctSW4gLiBTVkcgVmVyc2lvbjogNi4wMCBCdWlsZCAwKSAgLS0+CjwhRE9DVFlQRSBzdmcgUFVCTElDICItLy9XM0MvL0RURCBTVkcgMS4xLy9FTiIgImh0dHA6Ly93d3cudzMub3JnL0dyYXBoaWNzL1NWRy8xLjEvRFREL3N2ZzExLmR0ZCI+CjxzdmcgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIiB4bWxuczp4bGluaz0iaHR0cDovL3d3dy53My5vcmcvMTk5OS94bGluayIgdmVyc2lvbj0iMS4xIiBpZD0iQ2FwYV8xIiB4PSIwcHgiIHk9IjBweCIgd2lkdGg9IjE2cHgiIGhlaWdodD0iMTZweCIgdmlld0JveD0iMCAwIDQzMy41IDQzMy41IiBzdHlsZT0iZW5hYmxlLWJhY2tncm91bmQ6bmV3IDAgMCA0MzMuNSA0MzMuNTsiIHhtbDpzcGFjZT0icHJlc2VydmUiPgo8Zz4KCTxnIGlkPSJmaWxlLWRvd25sb2FkIj4KCQk8cGF0aCBkPSJNMzk1LjI1LDE1M2gtMTAyVjBoLTE1M3YxNTNoLTEwMmwxNzguNSwxNzguNUwzOTUuMjUsMTUzeiBNMzguMjUsMzgyLjV2NTFoMzU3di01MUgzOC4yNXoiIGZpbGw9IiMwMDAwMDAiLz4KCTwvZz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8Zz4KPC9nPgo8L3N2Zz4K); + background-size: 16px 16px; + background-repeat: no-repeat; + background-position: center; + cursor: pointer; +} + +.leaflet-div-icon { + background-color: #f4f4f4; + border: 1px solid #666; + border-radius: 50%; + display: inline-block; +} + +.leaflet-editing-icon { + background-color: #f4f4f4; + border: 1px solid #666; + border-radius: 50%; + display: inline-block; +} diff --git a/utils/osm_dataset_generator/semantic_map.py b/utils/osm_dataset_generator/semantic_map.py new file mode 100644 index 00000000..8ee3c67c --- /dev/null +++ b/utils/osm_dataset_generator/semantic_map.py @@ -0,0 +1,91 @@ +import numpy as np +import math +import pyCoverageControl # Main library +from pyCoverageControl import BivariateNormalDistribution as BND # for defining bivariate normal distributions +from pyCoverageControl import GeoLocalTransform as GeoTransform + +import json +import geojson + +# We can visualize the map in python +import matplotlib.pylab as plt +import seaborn as sns +colormap = sns.color_palette("light:b", as_cmap=True) +def plot_map(map): + ax = sns.heatmap(map.transpose(), vmax=1, cmap=colormap, square=True) + ax.invert_yaxis() + nrow, ncol = map.shape + septicks = 10 ** (math.floor(math.log(nrow, 10)) - 1) + plt.xticks(np.arange(0, nrow, septicks), np.arange(0, nrow, septicks)) + plt.yticks(np.arange(0, ncol, septicks), np.arange(0, ncol, septicks)) + plt.show() + + +################ +## Parameters ## +################ +# The parameters can be changed from python without needing to recompile. +# The parameters are given in params/parameters.yaml +# After changing the parameters, use the following function call to use the yaml file. +# Make sure the path of the file is correct +params_ = pyCoverageControl.Parameters('../core/params/parameters.yaml') + +############## +## WorldIDF ## +############## +from pyCoverageControl import WorldIDF # for defining world idf +world_idf = WorldIDF(params_) + +with open("../../../data/osm_city_data/7441/semantic_data.json") as file_: +# with open("leaflet_geojson_viz/data/semantic_data.json") as file_: + semantic_data = geojson.load(file_) + +[origin_lon, origin_lat] = semantic_data.bbox[0:2] +origin_alt = 0 +geo_transform = GeoTransform(origin_lat, origin_lon, origin_alt) + +# BivariateNormalDistribution with peak value of 1 +traffic_signals_sigma = 10 +traffic_signals_scale = 2 * math.pi * traffic_signals_sigma * traffic_signals_sigma + +# Uniform density polygon +hostpital_importance = 0.9 +parking_importance = 0.6 +park_importance = 0.8 + +flag = True +for feature in semantic_data.features: + if(feature['properties']['type'] == "traffic_signal"): + coords = feature['geometry']['coordinates'] + mean = geo_transform.Forward(coords[1], coords[0], origin_alt) + dist = BND(mean[0:2], traffic_signals_sigma, traffic_signals_scale) # circular gaussian + world_idf.AddNormalDistribution(dist) + + if(feature['properties']['type'] == "leisure" and feature['geometry']['type'] == "Polygon"): + for coords_list in feature['geometry']['coordinates']: + polygon_feature = pyCoverageControl.PolygonFeature() + if(feature['properties']['subtype'] == "park"): + polygon_feature.imp = park_importance + for pt in coords_list[:-1]: + cartesian_pt = geo_transform.Forward(pt[1], pt[0], origin_alt) + polygon_feature.poly.append(cartesian_pt[0:2]) + world_idf.AddUniformDistributionPolygon(polygon_feature) + + if(feature['properties']['type'] == "amenity" and feature['geometry']['type'] == "Polygon"): + for coords_list in feature['geometry']['coordinates']: + polygon_feature = pyCoverageControl.PolygonFeature() + if(feature['properties']['subtype'] == "hospital"): + polygon_feature.imp = hostpital_importance + if(feature['properties']['subtype'] == "parking"): + polygon_feature.imp = parking_importance + for pt in coords_list[:-1]: + cartesian_pt = geo_transform.Forward(pt[1], pt[0], origin_alt) + polygon_feature.poly.append(cartesian_pt[0:2]) + world_idf.AddUniformDistributionPolygon(polygon_feature) + + +world_idf.GenerateMapCuda() # Generate map, use GenerateMap() for cpu version +world_idf.PrintMapSize() +# world_idf.WriteWorldMap("map.dat"); # Writes the matrix to the file. Map should have been generated before writing +map = world_idf.GetWorldMap(); # Get the world map as numpy nd-array. You can only "view", i.e., flags.writeable = False, flags.owndata = False +plot_map(map) diff --git a/utils/osm_dataset_generator/semantic_objects.yaml b/utils/osm_dataset_generator/semantic_objects.yaml new file mode 100644 index 00000000..1b7ec36d --- /dev/null +++ b/utils/osm_dataset_generator/semantic_objects.yaml @@ -0,0 +1,24 @@ +area_dim: 1024 # data is extracted from a square of side area_dim in meters + +semantic_objects: + + traffic_signals: true + + leisure: + - 'park' + + amenity: # https://wiki.openstreetmap.org/wiki/Key:amenity + - 'fuel' + - 'hospital' + - 'parking' + + road_network: # https://wiki.openstreetmap.org/wiki/Key:highway + - 'motorway' + - 'motorway_link' + - 'trunk' + - 'trunk_link' + - 'primary' + - 'secondary' + - 'tertiary' + - 'unclassified' + - 'residential' From 81b9dc4cf17c18f819f15fd0c4292a00d9293727 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 20 Mar 2025 14:45:31 -0400 Subject: [PATCH 02/35] update model save --- python/coverage_control/nn/trainers/trainer.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/coverage_control/nn/trainers/trainer.py b/python/coverage_control/nn/trainers/trainer.py index 7bd25f56..93355c4b 100644 --- a/python/coverage_control/nn/trainers/trainer.py +++ b/python/coverage_control/nn/trainers/trainer.py @@ -152,11 +152,11 @@ def train(self) -> None: # torch.save(self.optimizer.state_dict(), self.model_dir + "/optimizer_curr.pt") if epoch % 5 == 0: - torch.save({"epoch": epoch, - "model_state_dict": self.model.state_dict(), - "optimizer_state_dict": self.optimizer.state_dict(), - "loss": train_loss}, - self.model_dir + "/model_epoch" + str(epoch) + ".pt") + model_state_dict = self.model.state_dict() + model_state_dict["epoch"] = epoch + model_state_dict["optimizer_state_dict"] = self.optimizer.state_dict() + model_state_dict["loss"] = train_loss + torch.save(model_state_dict, self.model_dir + "/model_epoch" + str(epoch) + ".pt") torch.save(best_model_state_dict, self.model_dir + "/model.pt") torch.save(best_train_model_state_dict, self.model_dir + "/model_train.pt") From 793633e8dae37c81f62a0d9ded14ef2bae7fed99 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 20 Mar 2025 14:38:46 -0400 Subject: [PATCH 03/35] add dual updater arg --- python/evaluators/constrained_learning.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/evaluators/constrained_learning.py b/python/evaluators/constrained_learning.py index e17ffabe..4c6dfbeb 100644 --- a/python/evaluators/constrained_learning.py +++ b/python/evaluators/constrained_learning.py @@ -299,6 +299,7 @@ def fun_dual_updater(self, configs, lambdas): T_0s = [25] eta_duals = [1] eval_dir = sys.argv[2] + dual_updater = sys.argv[3] for eta_dual in eta_duals: for T_0 in T_0s: @@ -309,7 +310,7 @@ def fun_dual_updater(self, configs, lambdas): env_id, eta_dual, T_0, - dual_updater="proj_1", + dual_updater, alpha=0.0, normalize=True, obj_normalize_factor=1e10, From f4fb93cc58fe9d949b80816e7d478b7227ba5ad1 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Fri, 21 Mar 2025 13:21:18 -0400 Subject: [PATCH 04/35] update model saving --- python/coverage_control/nn/trainers/trainer.py | 13 +++++++------ utils/scripts/run.sh | 8 ++++---- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/python/coverage_control/nn/trainers/trainer.py b/python/coverage_control/nn/trainers/trainer.py index 93355c4b..d9a8b528 100644 --- a/python/coverage_control/nn/trainers/trainer.py +++ b/python/coverage_control/nn/trainers/trainer.py @@ -139,6 +139,7 @@ def train(self) -> None: if val_loss < best_val_loss: best_val_loss = val_loss best_model_state_dict = deepcopy(self.model.state_dict()) + best_model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": val_loss} # torch.save(self.model.state_dict(), self.model_dir + "/model.pt") # torch.save(self.optimizer.state_dict(), self.model_dir + "/optimizer.pt") print(f"Epoch: {epoch + 1}/{self.num_epochs} ", @@ -148,18 +149,18 @@ def train(self) -> None: if train_loss < best_train_loss: best_train_loss = train_loss best_train_model_state_dict = deepcopy(self.model.state_dict()) - # torch.save(self.model.state_dict(), self.model_dir + "/model_curr.pt") - # torch.save(self.optimizer.state_dict(), self.model_dir + "/optimizer_curr.pt") + best_train_model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": train_loss} - if epoch % 5 == 0: + if (epoch + 1) % 5 == 0: model_state_dict = self.model.state_dict() - model_state_dict["epoch"] = epoch - model_state_dict["optimizer_state_dict"] = self.optimizer.state_dict() - model_state_dict["loss"] = train_loss torch.save(model_state_dict, self.model_dir + "/model_epoch" + str(epoch) + ".pt") + model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": train_loss} + torch.save(model_data, self.model_dir + "/model_data_epoch" + str(epoch) + ".pt") torch.save(best_model_state_dict, self.model_dir + "/model.pt") + torch.save(best_model_data, self.model_dir + "/model_data.pt") torch.save(best_train_model_state_dict, self.model_dir + "/model_train.pt") + torch.save(best_train_model_data, self.model_dir + "/model_train_data.pt") elapsed_time = time.time() - start_time # Print elapsed time in minutes print(f"Elapsed time: {elapsed_time / 60:.2f} minutes") diff --git a/utils/scripts/run.sh b/utils/scripts/run.sh index 9b0cfc63..270e42db 100644 --- a/utils/scripts/run.sh +++ b/utils/scripts/run.sh @@ -4,13 +4,13 @@ SCRIPT_DIR="${CoverageControl_ws}/src/CoverageControl/python" # Set the parameters directory based on the environment variable -PARAMS_DIR="${CoverageControl_ws}/lpac/params/" +PARAMS_DIR="${CoverageControl_ws}/lpac_512/params/" # Define the parameter file names DATA_PARAMS_FILE="data_params.toml" # DATA_GEN_ALGORITHM="--algorithm CentralizedCVT" LEARNING_PARAMS_FILE="learning_params.toml" -EVAL_PARAMS_FILE="eval.toml" +EVAL_PARAMS_FILE="eval_multi.toml" # Function to print messages in red print_error() { @@ -58,10 +58,10 @@ fi # Edit and execute process_data.sh # Running the data generation script -run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" +# run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" # Running the training script -run_command "python ${SCRIPT_DIR}/training/train_lpac.py ${PARAMS_DIR}/${LEARNING_PARAMS_FILE} 1024" "Model Training" +# run_command "python ${SCRIPT_DIR}/training/train_lpac.py ${PARAMS_DIR}/${LEARNING_PARAMS_FILE} 512" "Model Training" # Running the evaluation script run_command "python ${SCRIPT_DIR}/evaluators/eval.py ${PARAMS_DIR}/${EVAL_PARAMS_FILE}" "Model Evaluation" From 0902c2e9be987c6b88d43b265a5ae01aa7b6ad06 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Fri, 21 Mar 2025 13:38:52 -0400 Subject: [PATCH 05/35] restore script --- utils/scripts/run.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/utils/scripts/run.sh b/utils/scripts/run.sh index 270e42db..9b0cfc63 100644 --- a/utils/scripts/run.sh +++ b/utils/scripts/run.sh @@ -4,13 +4,13 @@ SCRIPT_DIR="${CoverageControl_ws}/src/CoverageControl/python" # Set the parameters directory based on the environment variable -PARAMS_DIR="${CoverageControl_ws}/lpac_512/params/" +PARAMS_DIR="${CoverageControl_ws}/lpac/params/" # Define the parameter file names DATA_PARAMS_FILE="data_params.toml" # DATA_GEN_ALGORITHM="--algorithm CentralizedCVT" LEARNING_PARAMS_FILE="learning_params.toml" -EVAL_PARAMS_FILE="eval_multi.toml" +EVAL_PARAMS_FILE="eval.toml" # Function to print messages in red print_error() { @@ -58,10 +58,10 @@ fi # Edit and execute process_data.sh # Running the data generation script -# run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" +run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" # Running the training script -# run_command "python ${SCRIPT_DIR}/training/train_lpac.py ${PARAMS_DIR}/${LEARNING_PARAMS_FILE} 512" "Model Training" +run_command "python ${SCRIPT_DIR}/training/train_lpac.py ${PARAMS_DIR}/${LEARNING_PARAMS_FILE} 1024" "Model Training" # Running the evaluation script run_command "python ${SCRIPT_DIR}/evaluators/eval.py ${PARAMS_DIR}/${EVAL_PARAMS_FILE}" "Model Evaluation" From a74e5593e1ea85f70a606b169c58cdb43d319867 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Fri, 21 Mar 2025 18:33:41 -0400 Subject: [PATCH 06/35] add progress bar to trainer --- .../coverage_control/nn/trainers/trainer.py | 128 +++++++++++------- utils/scripts/run.sh | 7 +- 2 files changed, 82 insertions(+), 53 deletions(-) diff --git a/python/coverage_control/nn/trainers/trainer.py b/python/coverage_control/nn/trainers/trainer.py index d9a8b528..7a46c729 100644 --- a/python/coverage_control/nn/trainers/trainer.py +++ b/python/coverage_control/nn/trainers/trainer.py @@ -23,11 +23,20 @@ Train a model using pytorch """ -import time from copy import deepcopy import torch +from rich.progress import ( + Progress, + BarColumn, + TextColumn, + TaskProgressColumn, + TimeRemainingColumn, + TimeElapsedColumn, + MofNCompleteColumn, +) + __all__ = ["TrainModel"] @@ -70,7 +79,6 @@ def __init__( self.num_epochs = num_epochs self.device = device self.model_dir = model_dir - self.start_time = time.time() def load_saved_model_dict(self, model_file: str) -> None: """ @@ -111,59 +119,82 @@ def train(self) -> None: # Initialize the loss history train_loss_history = [] val_loss_history = [] - start_time = time.time() best_model_state_dict = None best_train_model_state_dict = None - # Train the model - - for epoch in range(self.num_epochs): - # Training - train_loss = self.train_epoch() - train_loss_history.append(train_loss) + columns = [ + BarColumn(), + TaskProgressColumn(), + TextColumn("[progress.description]{task.description}"), + MofNCompleteColumn(), + TextColumn("[bold]Loss ", justify="right"), + TextColumn("[bold blue]T:[/] {task.fields[train_loss]:>.2e}"), + TextColumn("[bold blue]V:[/] {task.fields[val_loss]:>.2e}"), + TextColumn("[bold blue]B:[/] {task.fields[best_val_loss]:>.2e}"), + TextColumn("[bold blue]@[/] {task.fields[best_epoch]:<3.0f}"), + TimeRemainingColumn(), + TimeElapsedColumn(), + ] + + val_loss = float("Inf") + train_loss = float("Inf") + best_val_loss_epoch = -1 + + with Progress(*columns) as progress: + epoch_task = progress.add_task( + "[bold blue]Training", + total=self.num_epochs, + auto_refresh=False, + train_loss=train_loss, + val_loss=val_loss, + best_val_loss=best_val_loss, + best_epoch=best_val_loss_epoch, + ) + + best_train_model_state_dict = deepcopy(self.model.state_dict()) + for epoch in range(self.num_epochs): + # Training + train_loss = self.train_epoch() + train_loss_history.append(train_loss) + + if train_loss < best_train_loss: + best_train_loss = train_loss + best_train_model_state_dict = deepcopy(self.model.state_dict()) + best_train_model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": train_loss} + + if self.val_loader is not None: + val_loss = self.validate_epoch(self.val_loader) + val_loss_history.append(val_loss) + + if val_loss < best_val_loss: + best_val_loss = val_loss + best_model_state_dict = deepcopy(self.model.state_dict()) + best_model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": val_loss} + best_val_loss_epoch = epoch + + if (epoch + 1) % 5 == 0: + model_state_dict = self.model.state_dict() + torch.save(model_state_dict, self.model_dir + "/model_epoch" + str(epoch) + ".pt") + model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": train_loss} + torch.save(model_data, self.model_dir + "/model_data_epoch" + str(epoch) + ".pt") + + progress.update( + epoch_task, + advance=1, + train_loss=train_loss, + val_loss=val_loss, + best_val_loss=best_val_loss, + best_epoch=best_val_loss_epoch, + ) + progress.refresh() + + torch.save(val_loss_history, self.model_dir + "/val_loss.pt") torch.save(train_loss_history, self.model_dir + "/train_loss.pt") - # Print the loss - print(f"Epoch: {epoch + 1}/{self.num_epochs} ", - f"Training Loss: {train_loss:.3e} ") - - # Validation - - if self.val_loader is not None: - val_loss = self.validate_epoch(self.val_loader) - val_loss_history.append(val_loss) - torch.save(val_loss_history, self.model_dir + "/val_loss.pt") - - # Save the best model - - if val_loss < best_val_loss: - best_val_loss = val_loss - best_model_state_dict = deepcopy(self.model.state_dict()) - best_model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": val_loss} - # torch.save(self.model.state_dict(), self.model_dir + "/model.pt") - # torch.save(self.optimizer.state_dict(), self.model_dir + "/optimizer.pt") - print(f"Epoch: {epoch + 1}/{self.num_epochs} ", - f"Validation Loss: {val_loss:.3e} ", - f"Best Validation Loss: {best_val_loss:.3e}") - - if train_loss < best_train_loss: - best_train_loss = train_loss - best_train_model_state_dict = deepcopy(self.model.state_dict()) - best_train_model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": train_loss} - - if (epoch + 1) % 5 == 0: - model_state_dict = self.model.state_dict() - torch.save(model_state_dict, self.model_dir + "/model_epoch" + str(epoch) + ".pt") - model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": train_loss} - torch.save(model_data, self.model_dir + "/model_data_epoch" + str(epoch) + ".pt") - torch.save(best_model_state_dict, self.model_dir + "/model.pt") torch.save(best_model_data, self.model_dir + "/model_data.pt") torch.save(best_train_model_state_dict, self.model_dir + "/model_train.pt") torch.save(best_train_model_data, self.model_dir + "/model_train_data.pt") - elapsed_time = time.time() - start_time - # Print elapsed time in minutes - print(f"Elapsed time: {elapsed_time / 60:.2f} minutes") # Train the model in batches def train_epoch(self) -> float: @@ -198,11 +229,6 @@ def train_epoch(self) -> float: # Calculate the loss loss = self.criterion(output, target) - # Print batch number and loss - - if batch_idx % 10 == 0: - print(f"Batch: {batch_idx}, Loss: {loss:.3e} ") - # Backward propagation loss.backward() diff --git a/utils/scripts/run.sh b/utils/scripts/run.sh index 9b0cfc63..d023c6fe 100644 --- a/utils/scripts/run.sh +++ b/utils/scripts/run.sh @@ -6,6 +6,9 @@ SCRIPT_DIR="${CoverageControl_ws}/src/CoverageControl/python" # Set the parameters directory based on the environment variable PARAMS_DIR="${CoverageControl_ws}/lpac/params/" +# Set env size +ENV_SIZE=1024 + # Define the parameter file names DATA_PARAMS_FILE="data_params.toml" # DATA_GEN_ALGORITHM="--algorithm CentralizedCVT" @@ -58,10 +61,10 @@ fi # Edit and execute process_data.sh # Running the data generation script -run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" +# run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" # Running the training script -run_command "python ${SCRIPT_DIR}/training/train_lpac.py ${PARAMS_DIR}/${LEARNING_PARAMS_FILE} 1024" "Model Training" +run_command "python ${SCRIPT_DIR}/training/train_lpac.py ${PARAMS_DIR}/${LEARNING_PARAMS_FILE} ${ENV_SIZE}" "Model Training" # Running the evaluation script run_command "python ${SCRIPT_DIR}/evaluators/eval.py ${PARAMS_DIR}/${EVAL_PARAMS_FILE}" "Model Evaluation" From 2d9372469691478f709d746fce8f5ba1de66e0d3 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Fri, 21 Mar 2025 20:32:15 -0400 Subject: [PATCH 07/35] fix double computation of comm maps --- python/coverage_control/coverage_env_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/coverage_control/coverage_env_utils.py b/python/coverage_control/coverage_env_utils.py index aa44673b..c5288e0d 100644 --- a/python/coverage_control/coverage_env_utils.py +++ b/python/coverage_control/coverage_env_utils.py @@ -256,7 +256,7 @@ def get_maps( ) if use_comm_map: - comm_maps = env.GetCommunicationMaps(resized_map_size) + # comm_maps = env.GetCommunicationMaps(resized_map_size) # comm_maps = torch.tensor(numpy.array(env.GetCommunicationMaps(resized_map_size)), dtype=torch.float32).reshape(num_robots, 2, resized_map_size, resized_map_size) comm_maps = CoverageEnvUtils.get_communication_maps( env, params, resized_map_size From cdf357383f51ac2e38bd29aff82ce765176f86df Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Fri, 21 Mar 2025 20:26:35 -0400 Subject: [PATCH 08/35] minor fixes --- python/coverage_control/nn/trainers/trainer.py | 2 +- utils/scripts/run.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/coverage_control/nn/trainers/trainer.py b/python/coverage_control/nn/trainers/trainer.py index 7a46c729..2aa97de8 100644 --- a/python/coverage_control/nn/trainers/trainer.py +++ b/python/coverage_control/nn/trainers/trainer.py @@ -173,7 +173,7 @@ def train(self) -> None: best_model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": val_loss} best_val_loss_epoch = epoch - if (epoch + 1) % 5 == 0: + if (epoch) % 5 == 0 or epoch == self.num_epochs - 1: model_state_dict = self.model.state_dict() torch.save(model_state_dict, self.model_dir + "/model_epoch" + str(epoch) + ".pt") model_data = {"epoch": epoch, "optimizer_state_dict": deepcopy(self.optimizer.state_dict()), "loss": train_loss} diff --git a/utils/scripts/run.sh b/utils/scripts/run.sh index d023c6fe..8d0b39c4 100644 --- a/utils/scripts/run.sh +++ b/utils/scripts/run.sh @@ -61,7 +61,7 @@ fi # Edit and execute process_data.sh # Running the data generation script -# run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" +run_command "python ${SCRIPT_DIR}/data_generation/data_generation.py ${PARAMS_DIR}/${DATA_PARAMS_FILE} ${DATA_GEN_ALGORITHM} --split True" "Data Generation" # Running the training script run_command "python ${SCRIPT_DIR}/training/train_lpac.py ${PARAMS_DIR}/${LEARNING_PARAMS_FILE} ${ENV_SIZE}" "Model Training" From 864307256323d34baced12e112d25ccf3907d4df Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 25 Mar 2025 10:32:58 -0400 Subject: [PATCH 09/35] remove objective value call from controller --- python/coverage_control/algorithms/controllers.py | 4 ++-- python/evaluators/alpha.py | 2 +- python/evaluators/eval.py | 3 ++- python/evaluators/eval_single_env.py | 3 ++- 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/python/coverage_control/algorithms/controllers.py b/python/coverage_control/algorithms/controllers.py index bfa657aa..2e59838f 100644 --- a/python/coverage_control/algorithms/controllers.py +++ b/python/coverage_control/algorithms/controllers.py @@ -85,7 +85,7 @@ def step(self, env: CoverageSystem) -> (float, bool): if error_flag: raise ValueError("Error in step") - return env.GetObjectiveValue(), converged + return converged class ControllerNN: @@ -153,4 +153,4 @@ def step(self, env): # Check if actions are all zeros (1e-12) if torch.allclose(actions, torch.zeros_like(actions), atol=1e-5): return env.GetObjectiveValue(), True - return env.GetObjectiveValue(), False + return False diff --git a/python/evaluators/alpha.py b/python/evaluators/alpha.py index bd247940..f0154708 100644 --- a/python/evaluators/alpha.py +++ b/python/evaluators/alpha.py @@ -156,7 +156,7 @@ def update_idf(self, coefficients, normalize=False): ) def advance_state(self): - obj_val, is_converged = self.controller.step(self.env_main) + is_converged = self.controller.step(self.env_main) self.step_counter = self.step_counter + 1 if self.generate_video and self.step_counter % 1 == 0: diff --git a/python/evaluators/eval.py b/python/evaluators/eval.py index 1a6a5e39..8020a077 100644 --- a/python/evaluators/eval.py +++ b/python/evaluators/eval.py @@ -113,7 +113,8 @@ def evaluate(self, save=True): step_count = step_count + 1 while step_count < self.num_steps: - objective_value, converged = controller.step(env) + converged = controller.step(env) + objective_value = env.GetObjectiveValue() normalized_objective_value = ( objective_value / initial_objective_value ) diff --git a/python/evaluators/eval_single_env.py b/python/evaluators/eval_single_env.py index 271993e1..7848d00a 100644 --- a/python/evaluators/eval_single_env.py +++ b/python/evaluators/eval_single_env.py @@ -106,7 +106,8 @@ def evaluate(self, save=True): step_count = step_count + 1 while step_count < self.num_steps: - objective_value, converged = controller.step(env) + converged = controller.step(env) + objective_value = env.GetObjectiveValue() cost_data[controller_id, step_count] = ( objective_value / initial_objective_value ) From 42c5c5154ae027d59c6dab791a3fa56a9feeaafa Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 25 Mar 2025 10:40:31 -0400 Subject: [PATCH 10/35] fix return --- python/coverage_control/algorithms/controllers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/coverage_control/algorithms/controllers.py b/python/coverage_control/algorithms/controllers.py index 2e59838f..4f009d86 100644 --- a/python/coverage_control/algorithms/controllers.py +++ b/python/coverage_control/algorithms/controllers.py @@ -152,5 +152,5 @@ def step(self, env): # Check if actions are all zeros (1e-12) if torch.allclose(actions, torch.zeros_like(actions), atol=1e-5): - return env.GetObjectiveValue(), True + return True return False From baa21ce36915689b0cd5734451612291d45654d0 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 27 Mar 2025 05:00:25 -0400 Subject: [PATCH 11/35] add eval_area_coverage --- python/evaluators/eval_area_coverage.py | 197 ++++++++++++++++++++++++ 1 file changed, 197 insertions(+) create mode 100644 python/evaluators/eval_area_coverage.py diff --git a/python/evaluators/eval_area_coverage.py b/python/evaluators/eval_area_coverage.py new file mode 100644 index 00000000..d65008b1 --- /dev/null +++ b/python/evaluators/eval_area_coverage.py @@ -0,0 +1,197 @@ +# @file eval.py +# @brief Evaluates the performance of the controllers on a set of environments +import os +import argparse + +import coverage_control as cc +import numpy as np +from rich.progress import ( + Progress, + BarColumn, + TextColumn, + TimeRemainingColumn, + TimeElapsedColumn, + TaskProgressColumn, + MofNCompleteColumn, +) +from coverage_control import CoverageSystem +from coverage_control import IOUtils +from coverage_control import WorldIDF +from coverage_control.algorithms import ControllerCVT +from coverage_control.algorithms import ControllerNN + + +class Evaluator: + """ + Evaluates the performance of the controllers on a set of environments + """ + + def __init__(self, in_config): + self.config = in_config + self.eval_dir = IOUtils.sanitize_path(self.config["EvalDir"]) + self.env_dir = IOUtils.sanitize_path(self.config["EnvironmentDataDir"]) + self.controller_dir = None + + if not os.path.exists(self.env_dir): + os.makedirs(self.env_dir) + + self.num_controllers = len(self.config["Controllers"]) + self.controllers_configs = self.config["Controllers"] + + for controller_config in self.controllers_configs: + c_dir = self.eval_dir + "/" + controller_config["Name"] + + if not os.path.exists(c_dir): + os.makedirs(c_dir) + + self.env_config_file = IOUtils.sanitize_path(self.config["EnvironmentConfig"]) + self.env_config = IOUtils.load_toml(self.env_config_file) + self.cc_params = cc.Parameters(self.env_config_file) + + self.num_robots = self.cc_params.pNumRobots + self.num_features = self.cc_params.pNumGaussianFeatures + self.num_envs = self.config["NumEnvironments"] + self.num_steps = self.config["NumSteps"] + os.makedirs(self.env_dir + "/init_maps", exist_ok=True) + + self.columns = [ + BarColumn(bar_width=None), + TaskProgressColumn(), + TextColumn("[progress.description]{task.description}"), + MofNCompleteColumn(), + TextColumn("{task.fields[info]}"), + TextColumn("Step: {task.fields[step_count]:>4}"), + TextColumn("Obj: {task.fields[obj]:.2e}"), + TimeRemainingColumn(), + TimeElapsedColumn(), + ] + + def evaluate(self, save=True): + cost_data = np.zeros((self.num_controllers, self.num_envs, self.num_steps)) + area_coverage = np.zeros((self.num_controllers, self.num_envs, self.num_steps)) + + with Progress(*self.columns, expand=True) as progress: + task = progress.add_task( + "[bold blue]Evaluation", + total=self.num_envs, + info="Initializing...", + step_count=0, + obj=np.nan, + auto_refresh=False, + ) + + for env_count in range(self.num_envs): + pos_file = self.env_dir + "/" + str(env_count) + ".pos" + env_file = self.env_dir + "/" + str(env_count) + ".env" + + if os.path.isfile(env_file) and os.path.isfile(pos_file): + world_idf = WorldIDF(self.cc_params, env_file) + env_main = CoverageSystem(self.cc_params, world_idf, pos_file) + else: + # print(f"Creating new environment {env_count}") + env_main = CoverageSystem(self.cc_params) + env_main.WriteEnvironment(pos_file, env_file) + world_idf = env_main.GetWorldIDFObject() + + # env_main.PlotInitMap(self.env_dir + "/init_maps", f"{env_count}") + robot_init_pos = env_main.GetRobotPositions(force_no_noise=True) + + for controller_id in range(self.num_controllers): + step_count = 0 + env = CoverageSystem(self.cc_params, world_idf, robot_init_pos) + + if self.controllers_configs[controller_id]["Type"] == "Learning": + Controller = ControllerNN + else: + Controller = ControllerCVT + controller = Controller( + self.controllers_configs[controller_id], self.cc_params, env + ) + initial_objective_value = env.GetObjectiveValue() + cost_data[controller_id, env_count, step_count] = ( + env.GetObjectiveValue() / initial_objective_value + ) + area_coverage[controller_id, env_count, step_count] = ( + env.GetExplorationRatio() + ) + step_count = step_count + 1 + + while step_count < self.num_steps: + converged = controller.step(env) + objective_value = env.GetObjectiveValue() + + normalized_objective_value = ( + objective_value / initial_objective_value + ) + cost_data[controller_id, env_count, step_count] = ( + normalized_objective_value + ) + area_coverage[controller_id, env_count, step_count] = ( + env.GetExplorationRatio() + ) + + step_count = step_count + 1 + + if converged: + cost_data[controller_id, env_count, step_count:] = ( + normalized_objective_value + ) + area_coverage[controller_id, env_count, step_count:] = ( + env.GetExplorationRatio() + ) + step_count = self.num_steps + + if (step_count) % 10 == 0 or step_count == self.num_steps: + info = f"Controller {controller_id}/{self.num_controllers}: {controller.name} " + + progress.update( + task, + info=info, + step_count=step_count, + obj=normalized_objective_value, + ) + progress.refresh() + + if converged: + break + + if controller_id == self.num_controllers - 1: + info = f"Controller {controller_id + 1}/{self.num_controllers}: {controller.name} " + progress.update(task, info=info) + progress.refresh() + + if save is True: + self.controller_dir = ( + self.eval_dir + + "/" + + self.controllers_configs[controller_id]["Name"] + ) + controller_data_file = self.controller_dir + "/" + "eval.csv" + np.savetxt( + controller_data_file, + cost_data[controller_id, : env_count + 1, :], + delimiter=",", + ) + area_coverage_data_file = self.controller_dir + "/" + "eval_area_coverage.csv" + np.savetxt( + area_coverage_data_file, + area_coverage[controller_id, : env_count + 1, :], + delimiter=",", + ) + # env.RenderRecordedMap(self.eval_dir + "/" + self.controllers[controller_id]["Name"] + "/", "video.mp4") + del controller + del env + progress.advance(task) + progress.refresh() + + return cost_data + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("config_file", type=str, help="Path to config file") + args = parser.parse_args() + config = IOUtils.load_toml(args.config_file) + + evaluator = Evaluator(config) + evaluator.evaluate() From 04d6e94813831aba33ac2ccfe27a51ccdbd89988 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 27 Mar 2025 07:26:14 -0400 Subject: [PATCH 12/35] single pos normalizer --- python/coverage_control/coverage_env_utils.py | 19 ++++++++++++++++++- .../nn/data_loaders/loaders.py | 3 +-- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/python/coverage_control/coverage_env_utils.py b/python/coverage_control/coverage_env_utils.py index c5288e0d..bb2b0771 100644 --- a/python/coverage_control/coverage_env_utils.py +++ b/python/coverage_control/coverage_env_utils.py @@ -310,6 +310,23 @@ def get_robot_positions(env: CoverageSystem) -> torch.Tensor: return robot_positions + @staticmethod + def normalize_robot_positions(robot_positions: torch.Tensor, map_size: int) -> torch.Tensor: + """ + Normalize robot positions + + Args: + robot_positions: robot positions + map_size: size of the map + + Returns: + torch.Tensor: normalized robot positions + """ + # half_map_size = map_size / 2.0 + # normalized_positions = (robot_positions - half_map_size) / half_map_size + normalized_positions = (robot_positions + map_size / 2.0) / map_size + return normalized_positions + @staticmethod def get_weights(env: CoverageSystem, params: Parameters) -> torch.Tensor: """ @@ -366,7 +383,7 @@ def get_torch_geometric_data( edge_index = edge_weights.indices().long() weights = edge_weights.values().float() pos = CoverageEnvUtils.get_robot_positions(env) - pos = (pos + params.pWorldMapSize / 2.0) / params.pWorldMapSize + pos = CoverageEnvUtils.normalize_robot_positions(pos, params.pWorldMapSize) data = torch_geometric.data.Data( x=features, edge_index=edge_index.clone().detach(), diff --git a/python/coverage_control/nn/data_loaders/loaders.py b/python/coverage_control/nn/data_loaders/loaders.py index fb917252..a515ae16 100644 --- a/python/coverage_control/nn/data_loaders/loaders.py +++ b/python/coverage_control/nn/data_loaders/loaders.py @@ -115,7 +115,7 @@ def __init__(self, data_dir, stage, use_comm_map, world_size): self.robot_positions = DataLoaderUtils.load_robot_positions( f"{data_dir}/{stage}" ) - self.robot_positions = (self.robot_positions + world_size / 2) / world_size + self.robot_positions = CoverageEnvUtils.normalize_robot_positions(self.robot_positions, world_size) # Print the details of the dataset with device information print(f"Dataset: {self.stage} | Size: {self.dataset_size}", @@ -132,7 +132,6 @@ def get(self, idx): data = DataLoaderUtils.to_torch_geometric_data( self.maps[idx], self.edge_weights[idx], self.robot_positions[idx] ) - # data = CoverageEnvUtils.GetTorchGeometricDataRobotPositions(self.maps[idx], self.robot_positions[idx]) targets = self.targets[idx] if targets.dim == 3: From d066b7c2a9ce5bea46f7d615730a2a0b6a8cac54 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 27 Mar 2025 23:49:57 -0400 Subject: [PATCH 13/35] update names --- python/coverage_control/nn/trainers/trainer.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/coverage_control/nn/trainers/trainer.py b/python/coverage_control/nn/trainers/trainer.py index 2aa97de8..9ac4510d 100644 --- a/python/coverage_control/nn/trainers/trainer.py +++ b/python/coverage_control/nn/trainers/trainer.py @@ -191,10 +191,10 @@ def train(self) -> None: torch.save(val_loss_history, self.model_dir + "/val_loss.pt") torch.save(train_loss_history, self.model_dir + "/train_loss.pt") - torch.save(best_model_state_dict, self.model_dir + "/model.pt") - torch.save(best_model_data, self.model_dir + "/model_data.pt") + torch.save(best_model_state_dict, self.model_dir + "/model_val.pt") + torch.save(best_model_data, self.model_dir + "/model_data_val.pt") torch.save(best_train_model_state_dict, self.model_dir + "/model_train.pt") - torch.save(best_train_model_data, self.model_dir + "/model_train_data.pt") + torch.save(best_train_model_data, self.model_dir + "/model_data_train.pt") # Train the model in batches def train_epoch(self) -> float: From 0c570e35bc9c62b5ef20ee7648d66c4d56b9586a Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Fri, 28 Mar 2025 08:09:36 -0400 Subject: [PATCH 14/35] update plots --- utils/scripts/plot_costs_time.py | 61 ++++++++++++++++++++++++++------ 1 file changed, 50 insertions(+), 11 deletions(-) diff --git a/utils/scripts/plot_costs_time.py b/utils/scripts/plot_costs_time.py index 22728f0a..47cbc188 100755 --- a/utils/scripts/plot_costs_time.py +++ b/utils/scripts/plot_costs_time.py @@ -4,6 +4,7 @@ import pathlib import numpy as np import plotly.graph_objects as go +from plotly.subplots import make_subplots import sys # Import sys to handle command-line arguments class CostAnalyzer: @@ -26,6 +27,12 @@ def __init__(self, base_dir, csv_file_name="eval.csv"): 'rgba(229, 200, 144, 1.0)', # Yellow 'rgba(108, 111, 133, 1.0)', # subtext0 ] + self.num_controllers = len(self.controller_dirs) + self.num_envs = 0 + self.num_steps = 0 + self.all_costs = None + self.best_envs = None + @staticmethod def str2path(s): """Convert a string path to a pathlib.Path object with expanded user and variables.""" @@ -47,11 +54,28 @@ def load_and_normalize_costs(self): costs = np.loadtxt(csv_file_path, delimiter=",") costs = costs / costs[:, 0][:, None] # Normalize each row by the first element costs_dict[controller_dir] = costs + self.num_envs = costs_dict[self.controller_dirs[0]].shape[0] + self.num_steps = costs_dict[self.controller_dirs[0]].shape[1] + self.all_costs = np.zeros((self.num_controllers, self.num_envs, self.num_steps)) + for idx, controller_dir in enumerate(self.controller_dirs): + self.all_costs[idx] = costs_dict[controller_dir] return costs_dict + def compute_best_num_envs(self, costs_dict): + """Compute the best number of environments for each controller over time.""" + all_costs_temp = self.all_costs.copy() + # Find index of ClairvoyantCVT controller, if it exists, if not ignore it + clair_str = "ClairvoyantCVT" + clairvoyant_idx = self.controller_dirs.index(clair_str) if clair_str in self.controller_dirs else None + if clairvoyant_idx is not None: + all_costs_temp[clairvoyant_idx] = np.inf + controller_indices = np.arange(self.num_controllers)[:, None, None] + self.best_envs = (controller_indices == np.argmin(all_costs_temp, axis=0)[None, ...]).sum(axis=1) + self.best_envs[:, 0] = self.num_envs // (self.num_controllers - 1) + def plot_costs(self, costs_dict): """Plot the normalized costs over time for each controller.""" - fig = go.Figure() + fig = make_subplots(rows=3, cols=1, vertical_spacing=0.05, shared_xaxes=True, specs=[[{'rowspan': 2}], [{}], [{}]]) for idx, controller_dir in enumerate(self.controller_dirs): costs = costs_dict[controller_dir] mean_cost = np.mean(costs, axis=0) @@ -66,10 +90,10 @@ def plot_costs(self, costs_dict): fill="toself", fillcolor=color.replace('1.0', '0.2'), line=dict(color='rgba(255,255,255,0)'), - name=controller_dir + " ± std", legendgroup=controller_dir, - legendgrouptitle_text=controller_dir, - )) + showlegend=False, + ), + row=1, col=1) for idx, controller_dir in enumerate(self.controller_dirs): costs = costs_dict[controller_dir] @@ -77,24 +101,34 @@ def plot_costs(self, costs_dict): std_cost = np.std(costs, axis=0) time_steps = np.arange(costs.shape[1]) color = self.colors[idx % len(self.colors)] # Cycle through colors + + best_envs = self.best_envs[idx] # Mean cost line fig.add_trace(go.Scatter( x=time_steps, y=mean_cost, mode="lines", - name=controller_dir, + name="", line=dict(color=color), - # legendgroup="means", - # legendgrouptitle_text="Mean costs", legendgroup=controller_dir, legendgrouptitle_text=controller_dir, - )) + ), + row=1, col=1) + + fig.add_trace(go.Scatter( + x=time_steps, + y=best_envs, + mode="lines", + showlegend=False, + line=dict(color=color), + legendgroup=controller_dir, + ), + row=3, col=1) + # Update plot layout fig.update_layout( - title="Normalized costs over time", - xaxis_title="Time step", yaxis_title="Normalized cost", legend=dict( # orientation="h", @@ -102,13 +136,18 @@ def plot_costs(self, costs_dict): x=1, y=1, bgcolor="rgba(255, 255, 255, 0.8)" - ) + + ), + yaxis3_title="Number of Best Environments", + xaxis3_title="Time Step", ) + return fig def run_analysis(self): """Load data, generate plots, and output results.""" costs_dict = self.load_and_normalize_costs() + self.compute_best_num_envs(costs_dict) fig = self.plot_costs(costs_dict) fig.write_html(self.base_dir_path / "costs_time.html") From 2bc09259a747f29b3f74af7342efd283f9d9ce8f Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Wed, 2 Apr 2025 06:42:14 -0400 Subject: [PATCH 15/35] update plots --- python/evaluators/eval.py | 2 - python/evaluators/eval_area_coverage.py | 2 - utils/scripts/plot_costs_time.py | 154 +++++++++++++++++------- 3 files changed, 111 insertions(+), 47 deletions(-) diff --git a/python/evaluators/eval.py b/python/evaluators/eval.py index 8020a077..c68d3c5b 100644 --- a/python/evaluators/eval.py +++ b/python/evaluators/eval.py @@ -52,7 +52,6 @@ def __init__(self, in_config): self.num_features = self.cc_params.pNumGaussianFeatures self.num_envs = self.config["NumEnvironments"] self.num_steps = self.config["NumSteps"] - os.makedirs(self.env_dir + "/init_maps", exist_ok=True) self.columns = [ BarColumn(bar_width=None), @@ -92,7 +91,6 @@ def evaluate(self, save=True): env_main.WriteEnvironment(pos_file, env_file) world_idf = env_main.GetWorldIDFObject() - # env_main.PlotInitMap(self.env_dir + "/init_maps", f"{env_count}") robot_init_pos = env_main.GetRobotPositions(force_no_noise=True) for controller_id in range(self.num_controllers): diff --git a/python/evaluators/eval_area_coverage.py b/python/evaluators/eval_area_coverage.py index d65008b1..9b29fbf1 100644 --- a/python/evaluators/eval_area_coverage.py +++ b/python/evaluators/eval_area_coverage.py @@ -52,7 +52,6 @@ def __init__(self, in_config): self.num_features = self.cc_params.pNumGaussianFeatures self.num_envs = self.config["NumEnvironments"] self.num_steps = self.config["NumSteps"] - os.makedirs(self.env_dir + "/init_maps", exist_ok=True) self.columns = [ BarColumn(bar_width=None), @@ -93,7 +92,6 @@ def evaluate(self, save=True): env_main.WriteEnvironment(pos_file, env_file) world_idf = env_main.GetWorldIDFObject() - # env_main.PlotInitMap(self.env_dir + "/init_maps", f"{env_count}") robot_init_pos = env_main.GetRobotPositions(force_no_noise=True) for controller_id in range(self.num_controllers): diff --git a/utils/scripts/plot_costs_time.py b/utils/scripts/plot_costs_time.py index 47cbc188..19fe9421 100755 --- a/utils/scripts/plot_costs_time.py +++ b/utils/scripts/plot_costs_time.py @@ -17,19 +17,20 @@ def __init__(self, base_dir, csv_file_name="eval.csv"): self.csv_file_name = csv_file_name # Catppuccin colors self.colors = [ - 'rgba(239, 159, 118, 1.0)', # Peach - 'rgba(166, 209, 137, 1.0)', # Green - 'rgba(202, 158, 230, 1.0)', # Mauve - 'rgba(133, 193, 220, 1.0)', # Sapphire - 'rgba(231, 130, 132, 1.0)', # Red - 'rgba(129, 200, 190, 1.0)', # Teal - 'rgba(242, 213, 207, 1.0)', # Rosewater - 'rgba(229, 200, 144, 1.0)', # Yellow - 'rgba(108, 111, 133, 1.0)', # subtext0 - ] + 'rgba(239, 159, 118, 1.0)', # Peach + 'rgba(166, 209, 137, 1.0)', # Green + 'rgba(202, 158, 230, 1.0)', # Mauve + 'rgba(133, 193, 220, 1.0)', # Sapphire + 'rgba(231, 130, 132, 1.0)', # Red + 'rgba(129, 200, 190, 1.0)', # Teal + 'rgba(242, 213, 207, 1.0)', # Rosewater + 'rgba(229, 200, 144, 1.0)', # Yellow + 'rgba(108, 111, 133, 1.0)', # subtext0 + ] self.num_controllers = len(self.controller_dirs) self.num_envs = 0 self.num_steps = 0 + self.time_steps = None self.all_costs = None self.best_envs = None @@ -56,6 +57,7 @@ def load_and_normalize_costs(self): costs_dict[controller_dir] = costs self.num_envs = costs_dict[self.controller_dirs[0]].shape[0] self.num_steps = costs_dict[self.controller_dirs[0]].shape[1] + self.time_steps = np.arange(self.num_steps) self.all_costs = np.zeros((self.num_controllers, self.num_envs, self.num_steps)) for idx, controller_dir in enumerate(self.controller_dirs): self.all_costs[idx] = costs_dict[controller_dir] @@ -75,75 +77,141 @@ def compute_best_num_envs(self, costs_dict): def plot_costs(self, costs_dict): """Plot the normalized costs over time for each controller.""" - fig = make_subplots(rows=3, cols=1, vertical_spacing=0.05, shared_xaxes=True, specs=[[{'rowspan': 2}], [{}], [{}]]) + fig = go.Figure() for idx, controller_dir in enumerate(self.controller_dirs): costs = costs_dict[controller_dir] mean_cost = np.mean(costs, axis=0) std_cost = np.std(costs, axis=0) - time_steps = np.arange(costs.shape[1]) color = self.colors[idx % len(self.colors)] # Cycle through colors - + # Shaded area for standard deviation fig.add_trace(go.Scatter( - x=np.concatenate([time_steps, time_steps[::-1]]), + x=np.concatenate([self.time_steps, self.time_steps[::-1]]), y=np.concatenate([mean_cost + std_cost, (mean_cost - std_cost)[::-1]]), fill="toself", fillcolor=color.replace('1.0', '0.2'), line=dict(color='rgba(255,255,255,0)'), legendgroup=controller_dir, showlegend=False, - ), - row=1, col=1) + visible=True, + )) for idx, controller_dir in enumerate(self.controller_dirs): costs = costs_dict[controller_dir] mean_cost = np.mean(costs, axis=0) - std_cost = np.std(costs, axis=0) - time_steps = np.arange(costs.shape[1]) color = self.colors[idx % len(self.colors)] # Cycle through colors - best_envs = self.best_envs[idx] - # Mean cost line fig.add_trace(go.Scatter( - x=time_steps, + x=self.time_steps, y=mean_cost, mode="lines", - name="", + name=controller_dir, line=dict(color=color), legendgroup=controller_dir, - legendgrouptitle_text=controller_dir, - ), - row=1, col=1) + visible=True, + )) + for idx, controller_dir in enumerate(self.controller_dirs): + best_envs = self.best_envs[idx] + color = self.colors[idx % len(self.colors)] # Cycle through colors fig.add_trace(go.Scatter( - x=time_steps, + x=self.time_steps, y=best_envs, mode="lines", - showlegend=False, + showlegend=True, + name=controller_dir, line=dict(color=color), legendgroup=controller_dir, - ), - row=3, col=1) - + visible=False, + )) + + for idx, controller_dir in enumerate(self.controller_dirs): + final_costs = costs_dict[controller_dir][:, -1] + color = self.colors[idx % len(self.colors)] + fig.add_trace( + go.Violin( + y=final_costs, + name=controller_dir, + line_color=color, + box_visible=True, + meanline_visible=True, + showlegend=False, + points="all",visible=False,), + ) + + + costs_button = dict(label="Costs", + method="update", + args=[{"visible": self.visibility_masking([True, True, False, False])}, + {"xaxis.title.text": "Time Steps", + "yaxis.title.text": "Normalized Cost", + "xaxis.type": "linear"}]) + + costs_button_wo_std = dict(label="Costs (mean only)", + method="update", + args=[{"visible": self.visibility_masking([False, True, False, False])}, + {"xaxis.title.text": "Time Steps", + "yaxis.title.text": "Normalized Cost", + "xaxis.type": "linear"}]) + + best_envs_button = dict(label="Best Environments", + method="update", + args=[{"visible": self.visibility_masking([False, False, True, False])}, + {"xaxis.title.text": "Time Steps", + "yaxis.title.text": "Number of Best Environments", + "xaxis.type": "linear"}]) + + viols_button = dict(label="Violin Plots", + method="update", + args=[{"visible": self.visibility_masking([False, False, False, True])}, + {"xaxis.title.text": "", + "yaxis.title.text": "Final Normalized Cost", + "xaxis.type": "category"}]) - # Update plot layout fig.update_layout( - yaxis_title="Normalized cost", - legend=dict( - # orientation="h", - # xanchor="right", - x=1, - y=1, - bgcolor="rgba(255, 255, 255, 0.8)" - - ), - yaxis3_title="Number of Best Environments", - xaxis3_title="Time Step", - ) + updatemenus=[ + dict( + type="buttons", + direction="left", + buttons=[costs_button, costs_button_wo_std, best_envs_button, viols_button], + showactive=True, + x=0.01, y=1.05, + xanchor='left', + yanchor='top' + ) + ], + legend=dict(x=1.01, y=1), + autosize=True, + template="plotly_white", + xaxis=dict( + title="Time Steps", + showgrid=True, # Show vertical grid lines + gridcolor='lightgray', # Color of grid lines + gridwidth=1, + mirror=True, + ticks='outside', + showline=True, + # make axis lines thicker + linecolor='black', + ), + yaxis=dict( + title="Normalized Cost", + showgrid=True, + gridcolor='lightgray', + gridwidth=1, + mirror=True, + ticks='outside', + showline=True, + linecolor='black', + ),) return fig + def visibility_masking(self, group_masks): + return [val for group_visible in group_masks for val in [group_visible] * self.num_controllers] + + def run_analysis(self): """Load data, generate plots, and output results.""" costs_dict = self.load_and_normalize_costs() From 5c2ef11cda8890b96d41b75b8e62d569a1559c85 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Wed, 2 Apr 2025 18:13:26 -0400 Subject: [PATCH 16/35] add get_action --- .../algorithms/controllers.py | 46 +++++++++++++------ 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/python/coverage_control/algorithms/controllers.py b/python/coverage_control/algorithms/controllers.py index 4f009d86..3c5fd554 100644 --- a/python/coverage_control/algorithms/controllers.py +++ b/python/coverage_control/algorithms/controllers.py @@ -64,6 +64,16 @@ def __init__(self, config: dict, params: Parameters, env: CoverageSystem): case _: raise ValueError(f"Unknown controller type: {controller_type}") + def get_actions(self, env=None): + """ + Get the actions from the CVT controller + Returns: + Actions as a PointVector object + Convergence flag + """ + self.alg.ComputeActions() + return self.alg.GetActions(), self.alg.IsConverged() + def step(self, env: CoverageSystem) -> (float, bool): """ Step function for the CVT controller @@ -77,9 +87,7 @@ def step(self, env: CoverageSystem) -> (float, bool): Returns: Objective value and convergence flag """ - self.alg.ComputeActions() - actions = self.alg.GetActions() - converged = self.alg.IsConverged() + actions, converged = self.get_actions() error_flag = env.StepActions(actions) if error_flag: @@ -128,18 +136,14 @@ def __init__(self, config: dict, params: Parameters, env: CoverageSystem): self.model.eval() self.model = torch.compile(self.model, dynamic=True) - def step(self, env): + def get_actions(self, env): """ - step function for the neural network controller - - Performs three steps: - 1. Get the data from the environment - 2. Get the actions from the model - 3. Step the environment using the actions + Get the actions from the neural network controller Args: env: CoverageSystem object Returns: - Objective value and convergence flag + Actions as a PointVector object + Convergence flag """ pyg_data = CoverageEnvUtils.get_torch_geometric_data( env, self.params, True, self.use_comm_map, self.cnn_map_size @@ -147,10 +151,22 @@ def step(self, env): with torch.no_grad(): actions = self.model(pyg_data) actions = actions * self.actions_std + self.actions_mean + converged = False + if torch.allclose(actions, torch.zeros_like(actions), atol=1e-5): + converged = True point_vector_actions = PointVector(actions.cpu().numpy()) - env.StepActions(point_vector_actions) + return point_vector_actions, converged + + def step(self, env): + """ + step function for the neural network controller + Args: + env: CoverageSystem object + Returns: + Objective value and convergence flag + """ + actions, converged = self.get_actions(env) + env.StepActions(actions) # Check if actions are all zeros (1e-12) - if torch.allclose(actions, torch.zeros_like(actions), atol=1e-5): - return True - return False + return converged From 31492997e8a4787b7b9fe1e790786a1e0fe1d1ef Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 17 Apr 2025 06:02:39 -0400 Subject: [PATCH 17/35] add map writer --- cppsrc/core/include/CoverageControl/coverage_system.h | 1 + cppsrc/core/src/coverage_system.cpp | 7 +++++-- cppsrc/python_bindings/core_binds.h | 1 + python/system_env/env_generator.py | 4 +++- 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h index b9eeabcb..4fe66453 100644 --- a/cppsrc/core/include/CoverageControl/coverage_system.h +++ b/cppsrc/core/include/CoverageControl/coverage_system.h @@ -416,6 +416,7 @@ class CoverageSystem { PointVector const &positions) const; int WriteEnvironment(std::string const &pos_filename, std::string const &env_filename) const; + int WriteWorldMap(std::string const &) const; //! @} //! \name Plot related functions diff --git a/cppsrc/core/src/coverage_system.cpp b/cppsrc/core/src/coverage_system.cpp index a26db2c3..9de7b192 100644 --- a/cppsrc/core/src/coverage_system.cpp +++ b/cppsrc/core/src/coverage_system.cpp @@ -398,8 +398,11 @@ int CoverageSystem::WriteRobotPositions(std::string const &file_name, int CoverageSystem::WriteEnvironment(std::string const &pos_filename, std::string const &env_filename) const { WriteRobotPositions(pos_filename); - world_idf_ptr_->WriteDistributions(env_filename); - return 0; + return world_idf_ptr_->WriteDistributions(env_filename); +} + +int CoverageSystem::WriteWorldMap(std::string const &file_name) const { + return world_idf_ptr_->WriteWorldMap(file_name); } void CoverageSystem::RenderRecordedMap(std::string const &dir_name, diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index 0d56039d..79f7ffe3 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -379,6 +379,7 @@ void pyCoverageControl_core_coverage_system(py::module &m) { &CoverageSystem::RecordPlotData)) .def("RenderRecordedMap", &CoverageSystem::RenderRecordedMap) .def("WriteEnvironment", &CoverageSystem::WriteEnvironment) + .def("WriteWorldMap", &CoverageSystem::WriteWorldMap) .def("GetNumRobots", &CoverageSystem::GetNumRobots) .def("ClearRobotMaps", &CoverageSystem::ClearRobotMaps) .def("ClearExploredIDF", &CoverageSystem::ClearExploredIDF); diff --git a/python/system_env/env_generator.py b/python/system_env/env_generator.py index ae0ed7b3..4ed8572e 100644 --- a/python/system_env/env_generator.py +++ b/python/system_env/env_generator.py @@ -29,10 +29,12 @@ def main(env_config_file: str, num_envs: int, env_dir: str): os.makedirs(envs_path) for i in range(num_envs): + cc_system = CoverageSystem(cc_params) pos_file = str(envs_path / f"{i:04}.pos") env_file = str(envs_path / f"{i:04}.env") - cc_system = CoverageSystem(cc_params) + map_file = str(envs_path / f"{i:04}.map") cc_system.WriteEnvironment(pos_file, env_file) + cc_system.WriteWorldMap(map_file) if __name__ == "__main__": From 70d88ffe3cb2b2366d808b46f74f10ff0bf4272b Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 17 Apr 2025 10:32:34 -0400 Subject: [PATCH 18/35] fix cuda mem leaks --- .../CoverageControl/generate_world_map.h | 1 + cppsrc/core/src/cuda/generate_world_map.cu | 49 +++++++++---------- cppsrc/core/src/world_idf.cpp | 5 +- python/system_env/env_generator.py | 33 ++++++++----- 4 files changed, 49 insertions(+), 39 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/generate_world_map.h b/cppsrc/core/include/CoverageControl/generate_world_map.h index 45b85adf..a53fae5b 100644 --- a/cppsrc/core/include/CoverageControl/generate_world_map.h +++ b/cppsrc/core/include/CoverageControl/generate_world_map.h @@ -43,6 +43,7 @@ struct BND_Cuda { float mean_x, mean_y; float sigma_x, sigma_y; float scale, rho; + float sqrt_one_minus_rho_squared; }; //! Structure to store the rectangular bounds of the polygons diff --git a/cppsrc/core/src/cuda/generate_world_map.cu b/cppsrc/core/src/cuda/generate_world_map.cu index 5c9dfa76..6f15be05 100644 --- a/cppsrc/core/src/cuda/generate_world_map.cu +++ b/cppsrc/core/src/cuda/generate_world_map.cu @@ -38,6 +38,8 @@ #include #include #include +#include +#include #include #include @@ -58,6 +60,12 @@ __device__ __constant__ float cu_normalization_factor; __device__ __constant__ int cu_polygons_num_pts; __device__ __constant__ int cu_num_polygons; +struct Normalize { + __device__ + float operator()(float x) { + return x * cu_normalization_factor; + } +}; __device__ float2 TransformPoint(BND_Cuda const *device_dists, int i, float2 const &in_point) { float2 pt; @@ -69,7 +77,7 @@ __device__ float2 TransformPoint(BND_Cuda const *device_dists, int i, } pt.x = (in_point.x - bnd.mean_x) / bnd.sigma_x; pt.y = (in_point.y - bnd.mean_y) / bnd.sigma_y; - pt.x = (pt.x - bnd.rho * pt.y) / (sqrt(1 - bnd.rho * bnd.rho)); + pt.x = (pt.x - bnd.rho * pt.y) / bnd.sqrt_one_minus_rho_squared; return pt; } @@ -82,11 +90,11 @@ __device__ float IntegrateQuarterPlane(BND_Cuda const &bnd, } else { pt.x = (in_point.x - bnd.mean_x) / bnd.sigma_x; pt.y = (in_point.y - bnd.mean_y) / bnd.sigma_y; - pt.x = (pt.x - bnd.rho * pt.y) / (sqrt(1 - bnd.rho * bnd.rho)); + pt.x = (pt.x - bnd.rho * pt.y) / bnd.sqrt_one_minus_rho_squared; } /* auto transformed_point = TransformPoint(i, in_point); */ float sc = bnd.scale; - /* return sc; */ + // return sc; return sc * erfc(pt.x * cu_OneBySqrt2) * erfc(pt.y * cu_OneBySqrt2) / 4.f; } @@ -108,7 +116,7 @@ __device__ float ComputeImportanceBND(BND_Cuda const *device_dists, mid_pt.x = (mid_pt_cell.x - bnd.mean_x) / bnd.sigma_x; mid_pt.y = (mid_pt_cell.y - bnd.mean_y) / bnd.sigma_y; mid_pt.x = - (mid_pt.x - bnd.rho * mid_pt.y) / (sqrt(1 - bnd.rho * bnd.rho)); + (mid_pt.x - bnd.rho * mid_pt.y) / bnd.sqrt_one_minus_rho_squared; } if (mid_pt.x * mid_pt.x + mid_pt.y * mid_pt.y > cu_trun_sq) { continue; @@ -146,8 +154,8 @@ __device__ float ComputeImportancePoly(Polygons_Cuda const &device_polygons, __global__ void kernel(BND_Cuda const *device_dists, Polygons_Cuda const device_polygons, float *importance_vec) { - int idx = blockIdx.x * blockDim.x + threadIdx.x; - int idy = blockIdx.y * blockDim.y + threadIdx.y; + int idy = blockIdx.x * blockDim.x + threadIdx.x; + int idx = blockIdx.y * blockDim.y + threadIdx.y; int vec_idx = idx * cu_map_size + idy; if (not(idx < cu_map_size and idy < cu_map_size)) { return; @@ -163,15 +171,6 @@ __global__ void kernel(BND_Cuda const *device_dists, poly_importance; } -__global__ void normalize(float *importance_vec) { - int idx = blockIdx.x * blockDim.x + threadIdx.x; - int idy = blockIdx.y * blockDim.y + threadIdx.y; - int vec_idx = idx * cu_map_size + idy; - if (not(idx < cu_map_size and idy < cu_map_size)) { - return; - } - importance_vec[vec_idx] *= cu_normalization_factor; -} void generate_world_map_cuda(BND_Cuda *host_dists, Polygons_Cuda_Host const &host_polygons, int const num_dists, int const map_size, @@ -234,9 +233,6 @@ void generate_world_map_cuda(BND_Cuda *host_dists, checkCudaErrors( cudaMalloc(&device_importance_vec, map_size * map_size * sizeof(float))); - /* dim3 dimBlock(1, 1, 1); */ - /* dim3 dimGrid(1,1,1); */ - dim3 dimBlock(32, 32, 1); dim3 dimGrid(map_size / dimBlock.x, map_size / dimBlock.x, 1); @@ -245,19 +241,17 @@ void generate_world_map_cuda(BND_Cuda *host_dists, cudaDeviceSynchronize(); - thrust::device_ptr d_ptr = - thrust::device_pointer_cast(device_importance_vec); - float max = *(thrust::max_element(d_ptr, d_ptr + map_size * map_size)); + // thrust::device_ptr d_ptr = + // thrust::device_pointer_cast(device_importance_vec); + thrust::device_ptr d_ptr(device_importance_vec); + size_t num_map_cells = size_t(map_size) * map_size; + float max = *(thrust::max_element(d_ptr, d_ptr + num_map_cells)); - if (max < kEps) { - normalization_factor = pNorm; - } else { + if (max > kEps) { normalization_factor = pNorm / max; - } - if (normalization_factor > 1e-5) { checkCudaErrors(cudaMemcpyToSymbol(cu_normalization_factor, &normalization_factor, sizeof(float))); - normalize<<>>(device_importance_vec); + thrust::transform(d_ptr, d_ptr + num_map_cells, d_ptr, Normalize()); } checkCudaErrors(cudaMemcpy(host_importance_vec, device_importance_vec, @@ -268,6 +262,7 @@ void generate_world_map_cuda(BND_Cuda *host_dists, checkCudaErrors(cudaFree(device_importance_vec)); checkCudaErrors(cudaFree(device_polygons.x)); checkCudaErrors(cudaFree(device_polygons.y)); + checkCudaErrors(cudaFree(device_polygons.imp)); checkCudaErrors(cudaFree(device_polygons.sz)); checkCudaErrors(cudaFree(device_polygons.bounds)); diff --git a/cppsrc/core/src/world_idf.cpp b/cppsrc/core/src/world_idf.cpp index b7ad264d..0ecc0524 100644 --- a/cppsrc/core/src/world_idf.cpp +++ b/cppsrc/core/src/world_idf.cpp @@ -117,7 +117,9 @@ void WorldIDF::GenerateMapCuda(float const resolution, float const truncation, Point2 sigma = normal_distributions_[i].GetSigma(); host_dists[i].sigma_x = static_cast(sigma.x()); host_dists[i].sigma_y = static_cast(sigma.y()); - host_dists[i].rho = static_cast(normal_distributions_[i].GetRho()); + float rho = static_cast(normal_distributions_[i].GetRho()); + host_dists[i].rho = rho; + host_dists[i].sqrt_one_minus_rho_squared = std::sqrtf(1. - rho * rho); host_dists[i].scale = static_cast(normal_distributions_[i].GetScale()); } @@ -160,6 +162,7 @@ void WorldIDF::GenerateMapCuda(float const resolution, float const truncation, resolution, truncation, params_.pNorm, world_map_.data(), f_norm); normalization_factor_ = static_cast(f_norm); + delete [] host_dists; } #endif diff --git a/python/system_env/env_generator.py b/python/system_env/env_generator.py index 4ed8572e..398e161f 100644 --- a/python/system_env/env_generator.py +++ b/python/system_env/env_generator.py @@ -9,34 +9,40 @@ import pathlib import argparse +from coverage_control import IOUtils from coverage_control import Parameters +from coverage_control import WorldIDF from coverage_control import CoverageSystem -from coverage_control import IOUtils -def main(env_config_file: str, num_envs: int, env_dir: str): +def main(args): """ Generates large number of random environments """ - env_config_file = IOUtils.sanitize_path(env_config_file) + env_config_file = IOUtils.sanitize_path(args.config) cc_params = Parameters(env_config_file) - envs_path = pathlib.Path(IOUtils.sanitize_path(env_dir)) + envs_path = pathlib.Path(IOUtils.sanitize_path(args.env_dir)) if not os.path.exists(envs_path): print(f"Creating directory {envs_path}") os.makedirs(envs_path) - for i in range(num_envs): - cc_system = CoverageSystem(cc_params) - pos_file = str(envs_path / f"{i:04}.pos") - env_file = str(envs_path / f"{i:04}.env") - map_file = str(envs_path / f"{i:04}.map") + pad_width = len(str(args.num_envs - 1)) + for i in range(args.num_envs): + pos_file = str(envs_path / f"{i:0{pad_width}}.pos") + env_file = str(envs_path / f"{i:0{pad_width}}.env") + map_file = str(envs_path / f"{i:0{pad_width}}.map") + if os.path.isfile(env_file) and os.path.isfile(pos_file) and args.use_existing == True: + world_idf = WorldIDF(cc_params, env_file) + cc_system = CoverageSystem(cc_params, world_idf, pos_file) + else: + cc_system = CoverageSystem(cc_params) + cc_system.WriteEnvironment(pos_file, env_file) cc_system.WriteWorldMap(map_file) - if __name__ == "__main__": parser = argparse.ArgumentParser( description="Generates large number of random environments" @@ -44,6 +50,11 @@ def main(env_config_file: str, num_envs: int, env_dir: str): parser.add_argument("config", type=str, help="Environment configuration file") parser.add_argument("num_envs", type=int, help="Number of environments to generate") parser.add_argument("env_dir", type=str, help="Directory to save the environments") + parser.add_argument("--use_existing", type=bool, default=False, help="Use existing env if files exist") + parser.add_argument("--write_pos", type=bool, default=True, help="Write initial positions of robots") + parser.add_argument("--write_env", type=bool, default=True, help="Write idf info of environment") + parser.add_argument("--write_map", type=bool, default=True, help="Write environment map") + args = parser.parse_args() - main(args.config, args.num_envs, args.env_dir) + main(args) From 001d9de1004f0fbd9ece7574171612d79a79f615 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Fri, 18 Apr 2025 10:37:37 -0400 Subject: [PATCH 19/35] add try block --- utils/scripts/plot_costs_time.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/utils/scripts/plot_costs_time.py b/utils/scripts/plot_costs_time.py index 19fe9421..2f9a622f 100755 --- a/utils/scripts/plot_costs_time.py +++ b/utils/scripts/plot_costs_time.py @@ -60,7 +60,13 @@ def load_and_normalize_costs(self): self.time_steps = np.arange(self.num_steps) self.all_costs = np.zeros((self.num_controllers, self.num_envs, self.num_steps)) for idx, controller_dir in enumerate(self.controller_dirs): - self.all_costs[idx] = costs_dict[controller_dir] + try: + self.all_costs[idx] = costs_dict[controller_dir] + except ValueError as e: + print(f"[Shape Error] controller_dir={controller_dir}, " + f"self.all_costs[idx].shape={self.all_costs[idx].shape}, " + f"costs_dict[controller_dir].shape={costs_dict[controller_dir].shape}") + raise return costs_dict def compute_best_num_envs(self, costs_dict): From 9767bd0e513ed762d9faf85d8689726272a7e098 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Sat, 19 Apr 2025 12:57:43 -0400 Subject: [PATCH 20/35] add norm factor --- cppsrc/python_bindings/core_binds.h | 1 + 1 file changed, 1 insertion(+) diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index 79f7ffe3..7bef00fa 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -344,6 +344,7 @@ void pyCoverageControl_core_coverage_system(py::module &m) { .def("GetSystemMap", &CoverageSystem::GetSystemMap, py::return_value_policy::reference_internal) .def("GetObjectiveValue", &CoverageSystem::GetObjectiveValue) + .def("GetNormalizationFactor", &CoverageSystem::GetNormalizationFactor) .def("PlotSystemMap", py::overload_cast( &CoverageSystem::PlotSystemMap, py::const_)) .def("PlotSystemMap", py::overload_cast( From 9ddbbdf56991f37b66e37d0a34820544a7d29557 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Mon, 28 Apr 2025 10:37:51 -0400 Subject: [PATCH 21/35] improve cost plot --- utils/scripts/plot_costs_time.py | 1 + 1 file changed, 1 insertion(+) diff --git a/utils/scripts/plot_costs_time.py b/utils/scripts/plot_costs_time.py index 2f9a622f..166fc5be 100755 --- a/utils/scripts/plot_costs_time.py +++ b/utils/scripts/plot_costs_time.py @@ -116,6 +116,7 @@ def plot_costs(self, costs_dict): line=dict(color=color), legendgroup=controller_dir, visible=True, + hovertemplate="(%{x}, %{y:.2f})" )) for idx, controller_dir in enumerate(self.controller_dirs): From a531512ad3dd93cbc7bb23154804d152ec2403cc Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Mon, 28 Apr 2025 17:14:33 -0400 Subject: [PATCH 22/35] fix load_yaml --- python/coverage_control/io_utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/coverage_control/io_utils.py b/python/coverage_control/io_utils.py index 5dfc3e54..c5fe1b67 100644 --- a/python/coverage_control/io_utils.py +++ b/python/coverage_control/io_utils.py @@ -111,8 +111,8 @@ def load_yaml(path: str) -> dict: if not os.path.exists(path): raise FileNotFoundError(f"IOUtils::load_yaml File not found: {path}") # Load data - with open(path, "rb") as f: - data = yaml.load(f, Loader=yaml.FullLoader) + with open(path, "r") as f: + data = yaml.load(f, Loader=yaml.SafeLoader) return data From 97af831c473d15c328e95a0eae7efdc0f5c73d35 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Sun, 25 May 2025 00:10:50 -0400 Subject: [PATCH 23/35] use sqrt for older gcc --- cppsrc/core/include/CoverageControl/constants.h | 6 +++--- cppsrc/core/src/world_idf.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/constants.h b/cppsrc/core/include/CoverageControl/constants.h index 659f4c35..e4720986 100644 --- a/cppsrc/core/include/CoverageControl/constants.h +++ b/cppsrc/core/include/CoverageControl/constants.h @@ -48,9 +48,9 @@ namespace CoverageControl { double const kEps = 1e-10; /*!< Epsilon for double comparison */ float const kEpsf = 1e-6f; /*!< Epsilon for float comparison */ double const kLargeEps = 1e-4; /*!< Large epsilon for double comparison */ -double const kSqrt2 = std::sqrt(2); /*!< Square root of 2 */ -double const kOneBySqrt2 = 1. / std::sqrt(2); /*!< 1 by square root of 2 */ -float const kOneBySqrt2f = 1.f / sqrtf(2.f); /*!< 1 by square root of 2 */ +double const kSqrt2 = std::sqrt(2.); /*!< Square root of 2 */ +double const kOneBySqrt2 = 1.0 / kSqrt2; /*!< 1 by square root of 2 */ +float const kOneBySqrt2f = 1.0f / std::sqrt(2.0f); /*!< 1 by square root of 2 */ double const kInfD = std::numeric_limits::infinity(); /*!< Infinity for double */ constexpr auto kMaxPrecision{std::numeric_limits::digits10 + diff --git a/cppsrc/core/src/world_idf.cpp b/cppsrc/core/src/world_idf.cpp index 0ecc0524..21ab7e53 100644 --- a/cppsrc/core/src/world_idf.cpp +++ b/cppsrc/core/src/world_idf.cpp @@ -119,7 +119,7 @@ void WorldIDF::GenerateMapCuda(float const resolution, float const truncation, host_dists[i].sigma_y = static_cast(sigma.y()); float rho = static_cast(normal_distributions_[i].GetRho()); host_dists[i].rho = rho; - host_dists[i].sqrt_one_minus_rho_squared = std::sqrtf(1. - rho * rho); + host_dists[i].sqrt_one_minus_rho_squared = std::sqrt(1. - rho * rho); host_dists[i].scale = static_cast(normal_distributions_[i].GetScale()); } From f2f23f3431249fb5463a7c548db27ab687a4d007 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Sun, 27 Apr 2025 12:08:19 -0400 Subject: [PATCH 24/35] add persistent noisy --- .../algorithms/centralized_cvt.h | 13 +-- .../algorithms/clairvoyant_cvt.h | 13 +-- .../algorithms/decentralized_cvt.h | 23 ++--- .../algorithms/near_optimal_cvt.h | 13 +-- .../algorithms/oracle_bang_explore_exploit.h | 10 ++- .../include/CoverageControl/coverage_system.h | 46 +++------- .../core/include/CoverageControl/parameters.h | 5 +- .../include/CoverageControl/robot_model.h | 86 +++++++++++++------ cppsrc/core/src/coverage_system.cpp | 53 +----------- cppsrc/core/src/parameters.cpp | 18 ++-- cppsrc/python_bindings/core_binds.h | 8 +- cppsrc/setup.sh | 2 + .../algorithms/controllers.py | 10 +-- python/data_generation/data_generation.py | 5 +- 14 files changed, 147 insertions(+), 158 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/algorithms/centralized_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/centralized_cvt.h index b1733e27..4f83c812 100644 --- a/cppsrc/core/include/CoverageControl/algorithms/centralized_cvt.h +++ b/cppsrc/core/include/CoverageControl/algorithms/centralized_cvt.h @@ -64,14 +64,15 @@ class CentralizedCVT : public AbstractController { std::vector voronoi_mass_; bool is_converged_ = false; + bool force_no_noise_ = false; public: - CentralizedCVT(Parameters const ¶ms, CoverageSystem &env) - : CentralizedCVT(params, params.pNumRobots, env) {} + CentralizedCVT(Parameters const ¶ms, CoverageSystem &env, bool force_no_noise = false) + : CentralizedCVT(params, params.pNumRobots, env, force_no_noise) {} CentralizedCVT(Parameters const ¶ms, size_t const &num_robots, - CoverageSystem &env) - : params_{params}, num_robots_{num_robots}, env_{env} { - robot_global_positions_ = env_.GetRobotPositions(); + CoverageSystem &env, bool force_no_noise = false) + : params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} { + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); actions_.resize(num_robots_); goals_ = robot_global_positions_; voronoi_mass_.resize(num_robots_, 0); @@ -102,7 +103,7 @@ class CentralizedCVT : public AbstractController { int ComputeActions() { is_converged_ = true; - robot_global_positions_ = env_.GetRobotPositions(); + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); ComputeGoals(); auto voronoi_cells = voronoi_.GetVoronoiCells(); for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { diff --git a/cppsrc/core/include/CoverageControl/algorithms/clairvoyant_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/clairvoyant_cvt.h index aee04fea..ac84e1c2 100644 --- a/cppsrc/core/include/CoverageControl/algorithms/clairvoyant_cvt.h +++ b/cppsrc/core/include/CoverageControl/algorithms/clairvoyant_cvt.h @@ -63,14 +63,15 @@ class ClairvoyantCVT : public AbstractController { std::vector voronoi_mass_; bool is_converged_ = false; + bool force_no_noise_ = false; public: - ClairvoyantCVT(Parameters const ¶ms, CoverageSystem &env) - : ClairvoyantCVT(params, params.pNumRobots, env) {} + ClairvoyantCVT(Parameters const ¶ms, CoverageSystem &env, bool force_no_noise = false) + : ClairvoyantCVT(params, params.pNumRobots, env, force_no_noise) {} ClairvoyantCVT(Parameters const ¶ms, size_t const &num_robots, - CoverageSystem &env) - : params_{params}, num_robots_{num_robots}, env_{env} { - robot_global_positions_ = env_.GetRobotPositions(); + CoverageSystem &env, bool force_no_noise = false) + : params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} { + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); actions_.resize(num_robots_); goals_ = robot_global_positions_; voronoi_mass_.resize(num_robots_, 0); @@ -99,7 +100,7 @@ class ClairvoyantCVT : public AbstractController { int ComputeActions() { is_converged_ = true; - robot_global_positions_ = env_.GetRobotPositions(); + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); ComputeGoals(); auto voronoi_cells = voronoi_.GetVoronoiCells(); for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { diff --git a/cppsrc/core/include/CoverageControl/algorithms/decentralized_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/decentralized_cvt.h index 16aa9c4d..d43b019b 100644 --- a/cppsrc/core/include/CoverageControl/algorithms/decentralized_cvt.h +++ b/cppsrc/core/include/CoverageControl/algorithms/decentralized_cvt.h @@ -64,14 +64,15 @@ class DecentralizedCVT : public AbstractController { std::vector voronoi_mass_; bool is_converged_ = false; + bool force_no_noise_ = false; public: - DecentralizedCVT(Parameters const ¶ms, CoverageSystem &env) - : DecentralizedCVT(params, params.pNumRobots, env) {} + DecentralizedCVT(Parameters const ¶ms, CoverageSystem &env, bool force_no_noise = false) + : DecentralizedCVT(params, params.pNumRobots, env, force_no_noise) {} DecentralizedCVT(Parameters const ¶ms, size_t const &num_robots, - CoverageSystem &env) - : params_{params}, num_robots_{num_robots}, env_{env} { - robot_global_positions_ = env_.GetRobotPositions(); + CoverageSystem &env, bool force_no_noise = false) + : params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} { + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); actions_.resize(num_robots_); goals_ = robot_global_positions_; voronoi_mass_.resize(num_robots_, 0); @@ -80,17 +81,17 @@ class DecentralizedCVT : public AbstractController { PointVector GetActions() { return actions_; } - PointVector GetGoals() { return goals_; } + auto GetGoals() { return goals_; } void ComputeGoals() { #pragma omp parallel for num_threads(num_robots_) for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { - Point2 const &pos = robot_global_positions_[iRobot]; + auto const &pos = robot_global_positions_[iRobot]; MapUtils::MapBounds index, offset; MapUtils::ComputeOffsets(params_.pResolution, pos, params_.pLocalMapSize, params_.pWorldMapSize, index, offset); - MapType robot_map = env_.GetRobotMap(iRobot); - MapType robot_local_map = robot_map.block(index.left + offset.left, + auto robot_map = env_.GetRobotMap(iRobot); + auto robot_local_map = robot_map.block(index.left + offset.left, index.bottom + offset.bottom, offset.width, offset.height); Point2 map_translation( @@ -114,7 +115,7 @@ class DecentralizedCVT : public AbstractController { /* std::cout << "Voronoi: " << robot_positions[0][0] << " " << * robot_positions[0][1] << std::endl; */ int count = 1; - for (Point2 const &pos : robot_neighbors_pos) { + for (auto const &pos : robot_neighbors_pos) { robot_positions[count] = pos - map_translation; ++count; } @@ -144,7 +145,7 @@ class DecentralizedCVT : public AbstractController { int ComputeActions() { is_converged_ = true; - robot_global_positions_ = env_.GetRobotPositions(); + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); ComputeGoals(); for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { actions_[iRobot] = Point2(0, 0); diff --git a/cppsrc/core/include/CoverageControl/algorithms/near_optimal_cvt.h b/cppsrc/core/include/CoverageControl/algorithms/near_optimal_cvt.h index 272f0539..7668c71e 100644 --- a/cppsrc/core/include/CoverageControl/algorithms/near_optimal_cvt.h +++ b/cppsrc/core/include/CoverageControl/algorithms/near_optimal_cvt.h @@ -67,14 +67,15 @@ class NearOptimalCVT : public AbstractController { PointVector goals_, actions_; bool is_converged_ = false; + bool force_no_noise_ = false; public: - NearOptimalCVT(Parameters const ¶ms, CoverageSystem &env) - : NearOptimalCVT(params, params.pNumRobots, env) {} + NearOptimalCVT(Parameters const ¶ms, CoverageSystem &env, bool force_no_noise = false) + : NearOptimalCVT(params, params.pNumRobots, env, force_no_noise) {} NearOptimalCVT(Parameters const ¶ms, size_t const &num_robots, - CoverageSystem &env) - : params_{params}, num_robots_{num_robots}, env_{env} { - robot_global_positions_ = env_.GetRobotPositions(); + CoverageSystem &env, bool force_no_noise = false) + : params_{params}, num_robots_{num_robots}, env_{env}, force_no_noise_{force_no_noise} { + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); actions_.resize(num_robots_); goals_ = robot_global_positions_; ComputeGoals(); @@ -95,7 +96,7 @@ class NearOptimalCVT : public AbstractController { int ComputeActions() { is_converged_ = true; - robot_global_positions_ = env_.GetRobotPositions(); + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { actions_[iRobot] = Point2(0, 0); Point2 diff = goals_[iRobot] - robot_global_positions_[iRobot]; diff --git a/cppsrc/core/include/CoverageControl/algorithms/oracle_bang_explore_exploit.h b/cppsrc/core/include/CoverageControl/algorithms/oracle_bang_explore_exploit.h index 55502a84..43e51975 100644 --- a/cppsrc/core/include/CoverageControl/algorithms/oracle_bang_explore_exploit.h +++ b/cppsrc/core/include/CoverageControl/algorithms/oracle_bang_explore_exploit.h @@ -77,18 +77,20 @@ class OracleBangExploreExploit : public AbstractController { double time_step_dist_ = 0; double eps = 0.0001; double better_threshold_ = 2; + bool force_no_noise_ = false; public: OracleBangExploreExploit(Parameters const ¶ms, size_t const &num_robots, - CoverageSystem &env) + CoverageSystem &env, bool force_no_noise = false) : params_{params}, num_robots_{num_robots}, env_{env}, + force_no_noise_{force_no_noise}, exploration_map_{env.GetSystemExplorationMap()}, explored_idf_map_{env.GetSystemExploredIDFMap()} { voronoi_cells_.resize(num_robots_); - robot_global_positions_ = env_.GetRobotPositions(); + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); actions_.resize(num_robots_); goals_ = robot_global_positions_; sensor_area_ = params_.pSensorSize * params_.pSensorSize; @@ -119,7 +121,7 @@ class OracleBangExploreExploit : public AbstractController { std::vector GetVoronoiCells() { return voronoi_cells_; } void ManageRobotStatus() { - robot_global_positions_ = env_.GetRobotPositions(); + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { robot_status_[iRobot] = 0; @@ -341,7 +343,7 @@ class OracleBangExploreExploit : public AbstractController { int ComputeActions() { if (first_step_ == true) { first_step_ = false; - robot_global_positions_ = env_.GetRobotPositions(); + robot_global_positions_ = env_.GetRobotPositions(force_no_noise_); for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { goals_[iRobot] = robot_global_positions_[iRobot] + Point2{rand_dist_(gen_), rand_dist_(gen_)}; diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h index 4fe66453..4bc2437e 100644 --- a/cppsrc/core/include/CoverageControl/coverage_system.h +++ b/cppsrc/core/include/CoverageControl/coverage_system.h @@ -64,9 +64,9 @@ namespace CoverageControl { * library. */ class CoverageSystem { - Parameters const params_; //!< Parameters for the coverage system - std::shared_ptr world_idf_ptr_; //!< World IDF object - size_t num_robots_ = 0; //!< Number of robots + Parameters const params_; //!< Parameters for the coverage system + std::shared_ptr world_idf_ptr_; //!< World IDF object + size_t num_robots_ = 0; //!< Number of robots std::vector robots_; //!< Vector of robots of type RobotModel double normalization_factor_ = 0; //!< Normalization factor for the world IDF Voronoi voronoi_; //!< Voronoi object @@ -78,6 +78,7 @@ class CoverageSystem { std::uniform_real_distribution<> distrib_pts_; //!< Uniform distribution for generating random points PointVector robot_global_positions_; //!< Global positions of the robots + PointVector noisy_robot_global_positions_; //!< Global noisy positions of the robots MapType system_map_; //!< System map contains explored and unexplored locations MapType @@ -146,6 +147,10 @@ class CoverageSystem { for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { robot_global_positions_[iRobot] = robots_[iRobot].GetGlobalCurrentPosition(); + if (params_.pAddNoisePositions) { + noisy_robot_global_positions_[iRobot] = + robots_[iRobot].GetNoisyGlobalCurrentPosition(); + } } } @@ -347,7 +352,8 @@ class CoverageSystem { } auto const &history = robot_positions_history_[robot_id]; Point2 const last_pos = history.back(); - auto it_end = std::next(history.crbegin(), std::min(6, static_cast(history.size()) - 1)); + auto it_end = std::next(history.crbegin(), + std::min(6, static_cast(history.size()) - 1)); bool flag = false; int count = 0; std::for_each(history.crbegin(), it_end, @@ -515,24 +521,17 @@ class CoverageSystem { PointVector GetRobotPositions(bool force_no_noise = false) { UpdateRobotPositions(); if (params_.pAddNoisePositions and not force_no_noise) { - PointVector noisy_robot_global_positions; - for (Point2 pt : robot_global_positions_) { - noisy_robot_global_positions.push_back(AddNoise(pt)); - } - return noisy_robot_global_positions; + return noisy_robot_global_positions_; } return robot_global_positions_; } Point2 GetRobotPosition(int const robot_id, bool force_no_noise = false) const { - Point2 robot_pos; - robot_pos[0] = robots_[robot_id].GetGlobalCurrentPosition()[0]; - robot_pos[1] = robots_[robot_id].GetGlobalCurrentPosition()[1]; if (params_.pAddNoisePositions and not force_no_noise) { - return AddNoise(robot_pos); + return robots_[robot_id].GetNoisyGlobalCurrentPosition(); } - return robot_pos; + return robots_[robot_id].GetGlobalCurrentPosition(); } const MapType &GetRobotLocalMap(size_t const id) { @@ -564,25 +563,6 @@ class CoverageSystem { return robots_[id].GetSensorView(); } - auto GetRobotsInCommunication(size_t const id) const { - CheckRobotID(id); - PointVector robot_neighbors_pos; - for (size_t i = 0; i < num_robots_; ++i) { - if (id == i) { - continue; - } - Point2 relative_pos = - robot_global_positions_[i] - robot_global_positions_[id]; - if (relative_pos.norm() < params_.pCommunicationRange) { - robot_neighbors_pos.push_back(relative_pos); - } - } - std::sort( - robot_neighbors_pos.begin(), robot_neighbors_pos.end(), - [](Point2 const &a, Point2 const &b) { return a.norm() < b.norm(); }); - return robot_neighbors_pos; - } - std::pair GetRobotCommunicationMaps(size_t const, size_t); std::vector GetCommunicationMaps(size_t map_size) { diff --git a/cppsrc/core/include/CoverageControl/parameters.h b/cppsrc/core/include/CoverageControl/parameters.h index 3026b751..cdf12718 100644 --- a/cppsrc/core/include/CoverageControl/parameters.h +++ b/cppsrc/core/include/CoverageControl/parameters.h @@ -35,6 +35,8 @@ #include #include +#include + namespace CoverageControl { /*! @@ -143,7 +145,8 @@ class Parameters { * @{ */ bool pAddNoisePositions = false; - double pPositionsNoiseSigma = 0.; + double pPositionsNoiseSigmaMin = 0; + double pPositionsNoiseSigmaMax = 0; /*! @} */ /*! @} */ diff --git a/cppsrc/core/include/CoverageControl/robot_model.h b/cppsrc/core/include/CoverageControl/robot_model.h index 9fae1265..60a208bc 100644 --- a/cppsrc/core/include/CoverageControl/robot_model.h +++ b/cppsrc/core/include/CoverageControl/robot_model.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include "CoverageControl/constants.h" @@ -62,8 +63,8 @@ class RobotModel { private: Parameters const params_; - Point2 global_start_position_, global_current_position_; - Point2 local_start_position_, local_current_position_; + Point2 global_start_position_, global_current_position_, noisy_global_current_position_; + Point2 local_start_position_, local_current_position_, noisy_local_current_position_; double normalization_factor_ = 0; MapType robot_map_; //!< Stores what the robot has seen. Has the same @@ -78,6 +79,11 @@ class RobotModel { double time_step_dist_ = 0; double sensor_area_ = 0; + mutable std::mt19937 gen_; + std::uniform_real_distribution<> noise_sigma_dist_; + mutable std::normal_distribution noise_normal_dist_; + double noise_sigma_; + std::shared_ptr world_idf_; //!< Robots cannot change the world // const WorldIDF *world_idf_; //!< Robots cannot change the world @@ -122,8 +128,26 @@ class RobotModel { } void Initialize() { + if (params_.pAddNoisePositions) { + // std::srand(std::time(nullptr)); + gen_ = std::mt19937(std::random_device{}()); + double sigma_min = params_.pPositionsNoiseSigmaMin; + double sigma_max = params_.pPositionsNoiseSigmaMax; + if (std::abs(sigma_min - sigma_max) < kEps) { + noise_sigma_ = sigma_min; + } else { + noise_sigma_dist_ = std::uniform_real_distribution( + params_.pPositionsNoiseSigmaMin, params_.pPositionsNoiseSigmaMax); + noise_sigma_ = noise_sigma_dist_(gen_); + } + noise_normal_dist_ = std::normal_distribution{0, noise_sigma_}; + } + normalization_factor_ = world_idf_->GetNormalizationFactor(); global_current_position_ = global_start_position_; + noisy_global_current_position_ = ComputeNoisyGlobalCurrentPosition(); + noisy_local_current_position_ = noisy_global_current_position_ - + global_start_position_; sensor_view_ = MapType::Zero(params_.pSensorSize, params_.pSensorSize); local_map_ = MapType::Zero(params_.pLocalMapSize, params_.pLocalMapSize); @@ -152,6 +176,31 @@ class RobotModel { sensor_area_ = params_.pSensorSize * params_.pSensorSize; } + Point2 GetNoise() { + Point2 noise; + noise[0] = noise_normal_dist_(gen_); + noise[1] = noise_normal_dist_(gen_); + return noise; + } + + Point2 ComputeNoisyGlobalCurrentPosition() { + Point2 noisy_pt = global_current_position_ + GetNoise(); + if (noisy_pt[0] < kLargeEps) { + noisy_pt[0] = kLargeEps; + } + if (noisy_pt[1] < kLargeEps) { + noisy_pt[1] = kLargeEps; + } + if (noisy_pt[0] > params_.pWorldMapSize - kLargeEps) { + noisy_pt[0] = params_.pWorldMapSize - kLargeEps; + } + if (noisy_pt[1] > params_.pWorldMapSize - kLargeEps) { + noisy_pt[1] = params_.pWorldMapSize - kLargeEps; + } + return noisy_pt; + } + + public: /*! * \brief Constructor for the robot model @@ -228,31 +277,7 @@ class RobotModel { //! Set robot position relative to the current position void SetRobotPosition(Point2 const &pos) { Point2 new_global_pos = pos + global_start_position_; - if (new_global_pos.x() <= 0) { - new_global_pos[0] = 0 + kLargeEps; - } - if (new_global_pos.y() <= 0) { - new_global_pos[1] = 0 + kLargeEps; - } - double max_xy = params_.pWorldMapSize * params_.pResolution; - if (new_global_pos.x() >= max_xy) { - new_global_pos[0] = max_xy - kLargeEps; - } - if (new_global_pos.y() >= max_xy) { - new_global_pos[1] = max_xy - kLargeEps; - } - - local_current_position_ = new_global_pos - global_start_position_; - global_current_position_ = new_global_pos; - if (params_.pUpdateSensorView == true) { - UpdateSensorView(); - } - if (params_.pUpdateRobotMap == true) { - UpdateRobotMap(); - } - if (params_.pUpdateExplorationMap == true) { - UpdateExplorationMap(); - } + SetGlobalRobotPosition(new_global_pos); } void SetGlobalRobotPosition(Point2 const &pos) { @@ -273,6 +298,11 @@ class RobotModel { local_current_position_ = new_global_pos - global_start_position_; global_current_position_ = new_global_pos; + if (params_.pAddNoisePositions == true) { + noisy_global_current_position_ = ComputeNoisyGlobalCurrentPosition(); + noisy_local_current_position_ = noisy_global_current_position_ - + global_start_position_; + } if (params_.pUpdateSensorView == true) { UpdateSensorView(); } @@ -286,6 +316,8 @@ class RobotModel { Point2 GetGlobalStartPosition() const { return global_start_position_; } Point2 GetGlobalCurrentPosition() const { return global_current_position_; } + Point2 GetNoisyGlobalCurrentPosition() const { return noisy_global_current_position_; } + const MapType &GetRobotMap() const { return robot_map_; } const MapType &GetSensorView() const { return sensor_view_; } diff --git a/cppsrc/core/src/coverage_system.cpp b/cppsrc/core/src/coverage_system.cpp index 9de7b192..01473214 100644 --- a/cppsrc/core/src/coverage_system.cpp +++ b/cppsrc/core/src/coverage_system.cpp @@ -205,6 +205,7 @@ void CoverageSystem::InitSetup() { voronoi_cells_.resize(num_robots_); robot_global_positions_.resize(num_robots_); + noisy_robot_global_positions_.resize(num_robots_); for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { robot_global_positions_[iRobot] = robots_[iRobot].GetGlobalCurrentPosition(); @@ -276,10 +277,11 @@ void CoverageSystem::UpdateNeighbors() { relative_positions_neighbors_[iRobot].clear(); neighbor_ids_[iRobot].clear(); } + PointVector robot_global_positions = GetRobotPositions(); // Can be noisy for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { for (size_t jRobot = iRobot + 1; jRobot < num_robots_; ++jRobot) { Point2 relative_pos = - robot_global_positions_[jRobot] - robot_global_positions_[iRobot]; + robot_global_positions[jRobot] - robot_global_positions[iRobot]; if (relative_pos.norm() < params_.pCommunicationRange) { relative_positions_neighbors_[iRobot].push_back(relative_pos); neighbor_ids_[iRobot].push_back(jRobot); @@ -336,35 +338,6 @@ bool CoverageSystem::StepRobotsToGoals(PointVector const &goals, return cont_flag; } -Point2 CoverageSystem::AddNoise(Point2 const pt) const { - Point2 noisy_pt; - noisy_pt[0] = pt[0]; - noisy_pt[1] = pt[1]; - auto noise_sigma = params_.pPositionsNoiseSigma; - { // Wrap noise generation in a mutex to avoid issues with random number - // generation - // Random number generation is not thread safe - std::lock_guard lock(mutex_); - std::normal_distribution pos_noise{0.0, noise_sigma}; - noisy_pt += Point2(pos_noise(gen_), pos_noise(gen_)); - } - - /* std::normal_distribution pos_noise{0.0, noise_sigma}; */ - /* noisy_pt += Point2(pos_noise(gen_), pos_noise(gen_)); */ - if (noisy_pt[0] < kLargeEps) { - noisy_pt[0] = kLargeEps; - } - if (noisy_pt[1] < kLargeEps) { - noisy_pt[1] = kLargeEps; - } - if (noisy_pt[0] > params_.pWorldMapSize - kLargeEps) { - noisy_pt[0] = params_.pWorldMapSize - kLargeEps; - } - if (noisy_pt[1] > params_.pWorldMapSize - kLargeEps) { - noisy_pt[1] = params_.pWorldMapSize - kLargeEps; - } - return noisy_pt; -} int CoverageSystem::WriteRobotPositions(std::string const &file_name) const { std::ofstream file_obj(file_name); if (!file_obj) { @@ -627,24 +600,6 @@ void CoverageSystem::PlotRobotCommunicationMaps(std::string const &dir_name, PointVector CoverageSystem::GetRelativePositonsNeighbors( size_t const robot_id) { - if (params_.pAddNoisePositions) { - PointVector noisy_positions = GetRobotPositions(); - for (Point2 &pt : noisy_positions) { - pt = AddNoise(pt); - } - PointVector relative_positions; - for (size_t i = 0; i < num_robots_; ++i) { - if (i == robot_id) { - continue; - } - if ((noisy_positions[i] - noisy_positions[robot_id]).norm() < - params_.pCommunicationRange) { - relative_positions.push_back(noisy_positions[i] - - noisy_positions[robot_id]); - } - } - return relative_positions; - } return relative_positions_neighbors_[robot_id]; } @@ -663,7 +618,7 @@ std::vector CoverageSystem::GetLocalVoronoiFeatures( Point2 map_translation((index.left + offset.left) * params_.pResolution, (index.bottom + offset.bottom) * params_.pResolution); - auto robot_neighbors_pos = GetRobotsInCommunication(robot_id); + PointVector robot_neighbors_pos = relative_positions_neighbors_[robot_id]; PointVector robot_positions(robot_neighbors_pos.size() + 1); robot_positions[0] = pos - map_translation; diff --git a/cppsrc/core/src/parameters.cpp b/cppsrc/core/src/parameters.cpp index 3bfb5f6a..4ecdac00 100644 --- a/cppsrc/core/src/parameters.cpp +++ b/cppsrc/core/src/parameters.cpp @@ -35,7 +35,7 @@ namespace CoverageControl { void Parameters::ParseParameters() { std::cout << std::boolalpha; - std::cout << "Using config file: " << config_file_ << std::endl; + // std::cout << "Using config file: " << config_file_ << std::endl; if (not std::filesystem::exists(config_file_)) { std::cerr << "Could not find config file " << config_file_ << std::endl; throw std::runtime_error("Could not open config file"); @@ -219,13 +219,18 @@ void Parameters::ParseParameters() { if (toml_RobotModel["AddNoise"]) { auto toml_AddNoisePositions = toml_RobotModel["AddNoise"]["AddNoisePositions"].value(); - auto toml_PositionsNoiseSigma = - toml_RobotModel["AddNoise"]["PositionsNoiseSigma"].value(); + auto toml_PositionsNoiseSigmaMin = + toml_RobotModel["AddNoise"]["PositionsNoiseSigmaMin"].value(); + auto toml_PositionsNoiseSigmaMax = + toml_RobotModel["AddNoise"]["PositionsNoiseSigmaMax"].value(); if (toml_AddNoisePositions) { pAddNoisePositions = toml_AddNoisePositions.value(); } - if (toml_PositionsNoiseSigma) { - pPositionsNoiseSigma = toml_PositionsNoiseSigma.value(); + if (toml_PositionsNoiseSigmaMin) { + pPositionsNoiseSigmaMin = toml_PositionsNoiseSigmaMin.value(); + } + if (toml_PositionsNoiseSigmaMax) { + pPositionsNoiseSigmaMax = toml_PositionsNoiseSigmaMax.value(); } } @@ -302,7 +307,8 @@ void Parameters::PrintParameters() const { std::cout << "TimeStep: " << pTimeStep << std::endl; std::cout << "AddNoisePositions: " << pAddNoisePositions << std::endl; - std::cout << "PositionsNoiseSigma: " << pPositionsNoiseSigma << std::endl; + std::cout << "PositionsNoiseSigmaMin: " << pPositionsNoiseSigmaMin << std::endl; + std::cout << "PositionsNoiseSigmaMax: " << pPositionsNoiseSigmaMax << std::endl; std::cout << "EpisodeSteps: " << pEpisodeSteps << std::endl; std::cout << "CheckOscillations: " << pCheckOscillations << std::endl; diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index 7bef00fa..aff64f4e 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -188,7 +188,9 @@ void pyCoverageControl_core(py::module &m) { py::class_(m, "ClairvoyantCVT") .def(py::init()) + .def(py::init()) .def(py::init()) + .def(py::init()) .def("ComputeActions", &ClairvoyantCVT::ComputeActions) .def("IsConverged", &ClairvoyantCVT::IsConverged) .def("GetActions", &ClairvoyantCVT::GetActions) @@ -197,7 +199,9 @@ void pyCoverageControl_core(py::module &m) { py::class_(m, "DecentralizedCVT") .def(py::init()) + .def(py::init()) .def(py::init()) + .def(py::init()) .def("ComputeActions", &DecentralizedCVT::ComputeActions) .def("IsConverged", &DecentralizedCVT::IsConverged) .def("GetActions", &DecentralizedCVT::GetActions) @@ -205,7 +209,9 @@ void pyCoverageControl_core(py::module &m) { py::class_(m, "CentralizedCVT") .def(py::init()) + .def(py::init()) .def(py::init()) + .def(py::init()) .def("ComputeActions", &CentralizedCVT::ComputeActions) .def("IsConverged", &CentralizedCVT::IsConverged) .def("GetActions", &CentralizedCVT::GetActions) @@ -310,8 +316,6 @@ void pyCoverageControl_core_coverage_system(py::module &m) { .def("GetRobotSensorView", &CoverageSystem::GetRobotSensorView, py::return_value_policy::reference_internal) .def("GetCommunicationMaps", &CoverageSystem::GetCommunicationMaps) - .def("GetRobotsInCommunication", - &CoverageSystem::GetRobotsInCommunication) .def("GetSystemExploredIDFMap", &CoverageSystem::GetSystemExploredIDFMap, py::return_value_policy::reference_internal) .def("GetSystemExploredIDFMapMutable", diff --git a/cppsrc/setup.sh b/cppsrc/setup.sh index 5722a93d..a3231e7d 100755 --- a/cppsrc/setup.sh +++ b/cppsrc/setup.sh @@ -43,6 +43,8 @@ while true; do esac done +# Add CMAKE_BUILD_TYPE RelWithDebInfo +CMAKE_END_FLAGS="${CMAKE_END_FLAGS} -DCMAKE_BUILD_TYPE=RelWithDebInfo" if [[ ${WS_DIR} ]] then BUILD_DIR=${WS_DIR}/build/ diff --git a/python/coverage_control/algorithms/controllers.py b/python/coverage_control/algorithms/controllers.py index 3c5fd554..0e67cfe2 100644 --- a/python/coverage_control/algorithms/controllers.py +++ b/python/coverage_control/algorithms/controllers.py @@ -42,7 +42,7 @@ class ControllerCVT: Controller class for CVT based controllers """ - def __init__(self, config: dict, params: Parameters, env: CoverageSystem): + def __init__(self, config: dict, params: Parameters, env: CoverageSystem, force_no_noise=False): """ Constructor for the CVT controller Args: @@ -54,13 +54,13 @@ def __init__(self, config: dict, params: Parameters, env: CoverageSystem): self.params = params match config["Algorithm"]: case "DecentralizedCVT": - self.alg = DecentralizedCVT(params, env) + self.alg = DecentralizedCVT(params, env, force_no_noise) case "ClairvoyantCVT": - self.alg = ClairvoyantCVT(params, env) + self.alg = ClairvoyantCVT(params, env, force_no_noise) case "CentralizedCVT": - self.alg = CentralizedCVT(params, env) + self.alg = CentralizedCVT(params, env, force_no_noise) case "NearOptimalCVT": - self.alg = NearOptimalCVT(params, env) + self.alg = NearOptimalCVT(params, env, force_no_noise) case _: raise ValueError(f"Unknown controller type: {controller_type}") diff --git a/python/data_generation/data_generation.py b/python/data_generation/data_generation.py index 52701013..c0e86503 100644 --- a/python/data_generation/data_generation.py +++ b/python/data_generation/data_generation.py @@ -220,12 +220,13 @@ def run_data_generation(self): while self.dataset_count < self.num_dataset: self.env = CoverageSystem(self.env_params) + self.force_no_noise = True if self.algorithm == "CentralizedCVT": self.alg = CentralizedCVT( - self.env_params, self.num_robots, self.env) + self.env_params, self.num_robots, self.env, self.force_no_noise) else: self.alg = ClairvoyantCVT( - self.env_params, self.num_robots, self.env) + self.env_params, self.num_robots, self.env, self.force_no_noise) self.env_count += 1 self.progress.update( From 46a5d7cceab5482cac486f4bdd7a5dc8e864cf7a Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Wed, 28 May 2025 23:19:15 -0400 Subject: [PATCH 25/35] add robot world local map --- .../include/CoverageControl/coverage_system.h | 5 + .../include/CoverageControl/robot_model.h | 12 + cppsrc/python_bindings/core_binds.h | 2 + python/coverage_control/coverage_env_utils.py | 22 ++ python/data_generation/data_generation.py | 301 ++++++++++-------- 5 files changed, 215 insertions(+), 127 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h index 4fe66453..2083c79a 100644 --- a/cppsrc/core/include/CoverageControl/coverage_system.h +++ b/cppsrc/core/include/CoverageControl/coverage_system.h @@ -545,6 +545,11 @@ class CoverageSystem { return robots_[id].GetRobotMap(); } + const MapType &GetRobotWorldMap(size_t const id) { + CheckRobotID(id); + return robots_[id].GetWorldLocalMap(); + } + const MapType &GetRobotExplorationMap(size_t const id) { CheckRobotID(id); return robots_[id].GetExplorationMap(); diff --git a/cppsrc/core/include/CoverageControl/robot_model.h b/cppsrc/core/include/CoverageControl/robot_model.h index 9fae1265..71e4b4dc 100644 --- a/cppsrc/core/include/CoverageControl/robot_model.h +++ b/cppsrc/core/include/CoverageControl/robot_model.h @@ -70,6 +70,7 @@ class RobotModel { //!< reference as world map. MapType sensor_view_; //!< Stores the current sensor view of the robot MapType local_map_; //!< Stores the local map of the robot + MapType world_local_map_; //!< Stores the obstacle map MapType obstacle_map_; //!< Stores the obstacle map MapType system_map_; //!< Stores the obstacle map MapType @@ -128,6 +129,7 @@ class RobotModel { sensor_view_ = MapType::Zero(params_.pSensorSize, params_.pSensorSize); local_map_ = MapType::Zero(params_.pLocalMapSize, params_.pLocalMapSize); obstacle_map_ = MapType::Zero(params_.pLocalMapSize, params_.pLocalMapSize); + world_local_map_ = MapType::Zero(params_.pLocalMapSize, params_.pLocalMapSize); local_start_position_ = Point2{0, 0}; local_current_position_ = local_start_position_; @@ -318,6 +320,16 @@ class RobotModel { return local_map_; } + const MapType &GetWorldLocalMap() { + if (not MapUtils::IsPointOutsideBoundary( + params_.pResolution, global_current_position_, + params_.pLocalMapSize, params_.pWorldMapSize)) { + world_idf_->GetSubWorldMap(global_current_position_, params_.pSensorSize, + world_local_map_); + } + return world_local_map_; + } + const MapType &GetExplorationMap() { /* local_exploration_map_ = MapType::Constant(params_.pLocalMapSize, * params_.pLocalMapSize, 0); */ diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index 7bef00fa..24653b15 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -303,6 +303,8 @@ void pyCoverageControl_core_coverage_system(py::module &m) { "Get Positions of Robots", py::arg("force_no_noise") = false) .def("GetRobotLocalMap", &CoverageSystem::GetRobotLocalMap, py::return_value_policy::reference_internal) + .def("GetRobotWorldMap", &CoverageSystem::GetRobotWorldMap, + py::return_value_policy::reference_internal) .def("GetRobotMap", &CoverageSystem::GetRobotMap, py::return_value_policy::reference_internal) .def("GetRobotMapMutable", &CoverageSystem::GetRobotMapMutable, diff --git a/python/coverage_control/coverage_env_utils.py b/python/coverage_control/coverage_env_utils.py index bb2b0771..b981440e 100644 --- a/python/coverage_control/coverage_env_utils.py +++ b/python/coverage_control/coverage_env_utils.py @@ -116,6 +116,28 @@ def get_raw_local_maps(env: CoverageSystem, params: Parameters) -> torch.Tensor: return local_maps + @staticmethod + def get_raw_robot_world_local_maps(env: CoverageSystem, params: Parameters) -> torch.Tensor: + """ + Get robot world local maps + + Args: + env: coverage environment + params: parameters + + Returns: + torch.Tensor: raw robot world local maps + + """ + robot_world_local_maps = torch.zeros( + (env.GetNumRobots(), params.pLocalMapSize, params.pLocalMapSize) + ) + + for r_idx in range(env.GetNumRobots()): + robot_world_local_maps[r_idx] = CoverageEnvUtils.to_tensor(env.GetRobotWorldMap(r_idx)) + + return robot_world_local_maps + @staticmethod def get_raw_obstacle_maps(env: CoverageSystem, params: Parameters) -> torch.Tensor: """ diff --git a/python/data_generation/data_generation.py b/python/data_generation/data_generation.py index 52701013..d48e0952 100644 --- a/python/data_generation/data_generation.py +++ b/python/data_generation/data_generation.py @@ -19,6 +19,10 @@ # TriggerPostProcessing = 100 # # CNNMapSize = 32 +# EveryNumSteps = 5 +# SaveObjective = false +# TimeStep = 5 +# SaveRobotWorldLocalMaps = false # SaveAsSparseQ = true # NormalizeQ = true # @@ -26,6 +30,7 @@ # TrainRatio = 0.7 # ValRatio = 0.2 # TestRatio = 0.1 +# # @file data_generation.py # @brief Class to generate CoverageControl dataset for LPAC architecture import datetime @@ -37,14 +42,14 @@ from distutils.util import strtobool from rich.progress import ( - Progress, - BarColumn, - TextColumn, - TimeRemainingColumn, - TimeElapsedColumn, - TaskProgressColumn, - MofNCompleteColumn, -) + Progress, + BarColumn, + TextColumn, + TimeRemainingColumn, + TimeElapsedColumn, + TaskProgressColumn, + MofNCompleteColumn, + ) import coverage_control import torch from coverage_control import CoverageSystem @@ -73,7 +78,7 @@ def __init__(self, args): self.algorithm = args.algorithm self.data_dir = pathlib.Path( - IOUtils.sanitize_path(self.config["DataDir"])) + IOUtils.sanitize_path(self.config["DataDir"])) if not self.data_dir.exists(): print(f"{self.data_dir} does not exist") @@ -88,7 +93,7 @@ def __init__(self, args): os.makedirs(self.dataset_dir) env_config_file = IOUtils.sanitize_path( - self.config["EnvironmentConfig"]) + self.config["EnvironmentConfig"]) env_config_file = pathlib.Path(env_config_file) if not env_config_file.exists(): @@ -96,7 +101,7 @@ def __init__(self, args): sys.exit() self.env_params = coverage_control.Parameters( - env_config_file.as_posix()) + env_config_file.as_posix()) # Initialize variables self.dataset_count = 0 @@ -109,8 +114,8 @@ def __init__(self, args): self.num_dataset = self.config["NumDataset"] self.converged_data_ratio = self.config["ConvergedDataRatio"] self.num_converged_dataset = math.ceil( - self.converged_data_ratio * self.num_dataset - ) + self.converged_data_ratio * self.num_dataset + ) self.num_non_converged_dataset = self.num_dataset - self.num_converged_dataset self.num_robots = self.env_params.pNumRobots self.comm_range = self.env_params.pCommunicationRange @@ -126,6 +131,10 @@ def __init__(self, args): if "TimeStep" in self.config: self.env_params.pTimeStep = self.config["TimeStep"] + self.save_robot_world_local_maps = False + if "SaveRobotWorldLocalMaps" in self.config: + self.save_robot_world_local_maps = self.config["SaveRobotWorldLocalMaps"] + self.trigger_size = self.config["TriggerPostProcessing"] if self.trigger_size == 0 or self.trigger_size > self.num_dataset: @@ -139,44 +148,58 @@ def __init__(self, args): # Initialize tensors self.actions = torch.zeros((self.num_dataset, self.num_robots, 2)) self.robot_positions = torch.zeros( - (self.num_dataset, self.num_robots, 2)) + (self.num_dataset, self.num_robots, 2)) self.raw_local_maps = torch.zeros( - ( - self.trigger_size, - self.num_robots, - self.env_params.pLocalMapSize, - self.env_params.pLocalMapSize, - ) - ) + ( + self.trigger_size, + self.num_robots, + self.env_params.pLocalMapSize, + self.env_params.pLocalMapSize, + ) + ) self.raw_obstacle_maps = torch.zeros( - ( - self.trigger_size, - self.num_robots, - self.env_params.pLocalMapSize, - self.env_params.pLocalMapSize, - ) - ) + ( + self.trigger_size, + self.num_robots, + self.env_params.pLocalMapSize, + self.env_params.pLocalMapSize, + ) + ) self.local_maps = torch.zeros( - (self.num_dataset, self.num_robots, - self.cnn_map_size, self.cnn_map_size) - ) + (self.num_dataset, self.num_robots, + self.cnn_map_size, self.cnn_map_size) + ) self.obstacle_maps = torch.zeros( - (self.num_dataset, self.num_robots, - self.cnn_map_size, self.cnn_map_size) - ) + (self.num_dataset, self.num_robots, + self.cnn_map_size, self.cnn_map_size) + ) self.comm_maps = torch.zeros( - (self.num_dataset, self.num_robots, 2, - self.cnn_map_size, self.cnn_map_size) - ) + (self.num_dataset, self.num_robots, 2, + self.cnn_map_size, self.cnn_map_size) + ) self.coverage_features = torch.zeros( - (self.num_dataset, self.num_robots, 7)) + (self.num_dataset, self.num_robots, 7)) self.edge_weights = torch.zeros( - (self.num_dataset, self.num_robots, self.num_robots) - ) + (self.num_dataset, self.num_robots, self.num_robots) + ) if self.save_objective: self.objectives = torch.zeros(self.num_dataset) + if self.save_robot_world_local_maps: + self.raw_robot_world_local_maps = torch.zeros( + self.trigger_size, + self.num_robots, + self.env_params.pLocalMapSize, + self.env_params.pLocalMapSize, + ) + self.robot_world_local_maps = torch.zeros( + self.num_dataset, + self.num_robots, + self.cnn_map_size, + self.cnn_map_size + ) + self.start_time = datetime.datetime.now() # Write metrics self.metrics_file = self.dataset_dir_path / "metrics.txt" @@ -191,20 +214,20 @@ def __init__(self, args): self.print_tensor_sizes() columns = [ - BarColumn(bar_width=None), - TaskProgressColumn(), - TextColumn("[progress.description]{task.description}"), - MofNCompleteColumn(), - TextColumn("#Envs: {task.fields[num_envs]}"), - TimeRemainingColumn(), - TimeElapsedColumn(), - ] + BarColumn(bar_width=None), + TaskProgressColumn(), + TextColumn("[progress.description]{task.description}"), + MofNCompleteColumn(), + TextColumn("#Envs: {task.fields[num_envs]}"), + TimeRemainingColumn(), + TimeElapsedColumn(), + ] with Progress(*columns, expand=True) as self.progress: self.task = self.progress.add_task( - "[bold blue]Generating dataset", - total=self.num_dataset, - num_envs="", - ) + "[bold blue]Generating dataset", + total=self.num_dataset, + num_envs="", + ) self.run_data_generation() @@ -222,25 +245,25 @@ def run_data_generation(self): if self.algorithm == "CentralizedCVT": self.alg = CentralizedCVT( - self.env_params, self.num_robots, self.env) + self.env_params, self.num_robots, self.env) else: self.alg = ClairvoyantCVT( - self.env_params, self.num_robots, self.env) + self.env_params, self.num_robots, self.env) self.env_count += 1 self.progress.update( - self.task, - num_envs=f"{self.env_count}" - ) + self.task, + num_envs=f"{self.env_count}" + ) self.progress.refresh() num_steps = 0 is_converged = False while ( - num_steps < self.env_params.pEpisodeSteps - and not is_converged - and self.dataset_count < self.num_dataset - ): + num_steps < self.env_params.pEpisodeSteps + and not is_converged + and self.dataset_count < self.num_dataset + ): if num_steps % self.every_num_step == 0 and self.non_converged_dataset_count < self.num_non_converged_dataset: is_converged = self.step_with_save() @@ -265,21 +288,25 @@ def step_with_save(self): count = self.dataset_count self.actions[count] = CoverageEnvUtils.to_tensor(actions) self.robot_positions[count] = CoverageEnvUtils.get_robot_positions( - self.env) + self.env) self.coverage_features[count] = CoverageEnvUtils.get_voronoi_features( - self.env) + self.env) self.raw_local_maps[self.trigger_count] = CoverageEnvUtils.get_raw_local_maps( - self.env, self.env_params - ) + self.env, self.env_params + ) self.raw_obstacle_maps[self.trigger_count] = ( - CoverageEnvUtils.get_raw_obstacle_maps(self.env, self.env_params) - ) + CoverageEnvUtils.get_raw_obstacle_maps(self.env, self.env_params) + ) + if self.save_robot_world_local_maps: + self.raw_robot_world_local_maps[self.trigger_count] = ( + CoverageEnvUtils.get_raw_robot_world_local_maps(self.env, self.env_params) + ) self.comm_maps[count] = CoverageEnvUtils.get_communication_maps( - self.env, self.env_params, self.cnn_map_size - ) + self.env, self.env_params, self.cnn_map_size + ) self.edge_weights[count] = CoverageEnvUtils.get_weights( - self.env, self.env_params - ) + self.env, self.env_params + ) if self.save_objective: self.objectives[count] = self.env.GetObjectiveValue() @@ -303,37 +330,52 @@ def trigger_post_processing(self): if self.trigger_start_idx > self.num_dataset - 1: return trigger_end_idx = min( - self.num_dataset, self.trigger_start_idx + self.trigger_size - ) + self.num_dataset, self.trigger_start_idx + self.trigger_size + ) raw_local_maps = self.raw_local_maps[ - 0: trigger_end_idx - self.trigger_start_idx - ] + 0: trigger_end_idx - self.trigger_start_idx + ] raw_local_maps = raw_local_maps.to(self.device) resized_local_maps = CoverageEnvUtils.resize_maps( - raw_local_maps, self.cnn_map_size - ) + raw_local_maps, self.cnn_map_size + ) self.local_maps[self.trigger_start_idx: trigger_end_idx] = ( - resized_local_maps.view( - -1, self.num_robots, self.cnn_map_size, self.cnn_map_size - ) - .cpu() - .clone() - ) + resized_local_maps.view( + -1, self.num_robots, self.cnn_map_size, self.cnn_map_size + ) + .cpu() + .clone() + ) + if self.save_robot_world_local_maps: + raw_robot_world_local_maps = self.raw_robot_world_local_maps[ + 0: trigger_end_idx - self.trigger_start_idx + ] + raw_robot_world_local_maps = raw_robot_world_local_maps.to(self.device) + resized_robot_world_local_maps = CoverageEnvUtils.resize_maps( + raw_robot_world_local_maps, self.cnn_map_size + ) + self.robot_world_local_maps[self.trigger_start_idx: trigger_end_idx] = ( + resized_robot_world_local_maps.view( + -1, self.num_robots, self.cnn_map_size, self.cnn_map_size + ) + .cpu() + .clone() + ) raw_obstacle_maps = self.raw_obstacle_maps[ - 0: trigger_end_idx - self.trigger_start_idx - ] + 0: trigger_end_idx - self.trigger_start_idx + ] raw_obstacle_maps = raw_obstacle_maps.to(self.device) resized_obstacle_maps = CoverageEnvUtils.resize_maps( - raw_obstacle_maps, self.cnn_map_size - ) + raw_obstacle_maps, self.cnn_map_size + ) self.obstacle_maps[self.trigger_start_idx: trigger_end_idx] = ( - resized_obstacle_maps.view( - -1, self.num_robots, self.cnn_map_size, self.cnn_map_size - ) - .cpu() - .clone() - ) + resized_obstacle_maps.view( + -1, self.num_robots, self.cnn_map_size, self.cnn_map_size + ) + .cpu() + .clone() + ) self.trigger_start_idx = trigger_end_idx @@ -395,8 +437,8 @@ def save_tensor_split(self, tensor, name, as_sparse=False): tensor = tensor.cpu() train_tensor = tensor[0: self.train_size].clone() validation_tensor = tensor[ - self.train_size: self.train_size + self.validation_size - ].clone() + self.train_size: self.train_size + self.validation_size + ].clone() test_tensor = tensor[self.train_size + self.validation_size:].clone() if as_sparse: @@ -417,11 +459,11 @@ def save_dataset(self): if self.split_dataset: self.train_size = int( - self.num_dataset * self.config["DataSetSplit"]["TrainRatio"] - ) + self.num_dataset * self.config["DataSetSplit"]["TrainRatio"] + ) self.validation_size = int( - self.num_dataset * self.config["DataSetSplit"]["ValRatio"] - ) + self.num_dataset * self.config["DataSetSplit"]["ValRatio"] + ) self.test_size = self.num_dataset - self.train_size - self.validation_size # Make sure the folder exists @@ -437,6 +479,8 @@ def save_dataset(self): self.save_tensor(self.robot_positions, "robot_positions.pt") self.save_tensor(self.local_maps, "local_maps.pt", as_sparse) + if self.save_robot_world_local_maps: + self.save_tensor(self.robot_world_local_maps, "robot_world_local_maps.pt", as_sparse) self.save_tensor(self.obstacle_maps, "obstacle_maps.pt", as_sparse) self.save_tensor(self.edge_weights, "edge_weights.pt", as_sparse) @@ -452,22 +496,22 @@ def save_dataset(self): if self.config["NormalizeQ"]: normalized_actions, actions_mean, actions_std = self.normalize_tensor( - self.actions, is_symmetric=True, zero_mean=True - ) + self.actions, is_symmetric=True, zero_mean=True + ) self.save_tensor(normalized_actions, "normalized_actions.pt") torch.save(actions_mean, self.dataset_dir_path / "actions_mean.pt") torch.save(actions_std, self.dataset_dir_path / "actions_std.pt") if self.save_objective: normalize_objectives, objectives_mean, objectives_std = self.normalize_tensor( - self.objectives - ) + self.objectives + ) self.save_tensor(normalize_objectives, "normalized_objectives.pt") torch.save( - objectives_mean, self.dataset_dir_path / "objectives_mean.pt" - ) + objectives_mean, self.dataset_dir_path / "objectives_mean.pt" + ) torch.save( - objectives_std, self.dataset_dir_path / "objectives_std.pt" - ) + objectives_std, self.dataset_dir_path / "objectives_std.pt" + ) # coverage_features, coverage_features_mean, coverage_features_std = ( # self.normalize_tensor(self.coverage_features) @@ -499,34 +543,37 @@ def print_tensor_sizes(self, file=sys.stdout): print("Tensor sizes:", file=file) print("Actions:", self.get_tensor_byte_size_mb(self.actions), file=file) print( - "Robot positions:", - self.get_tensor_byte_size_mb(self.robot_positions), - file=file, - ) + "Robot positions:", + self.get_tensor_byte_size_mb(self.robot_positions), + file=file, + ) print( - "Raw local maps:", - self.get_tensor_byte_size_mb(self.raw_local_maps), - file=file, - ) + "Raw local maps:", + self.get_tensor_byte_size_mb(self.raw_local_maps), + file=file, + ) print( - "Raw obstacle maps:", - self.get_tensor_byte_size_mb(self.raw_obstacle_maps), - file=file, - ) + "Raw obstacle maps:", + self.get_tensor_byte_size_mb(self.raw_obstacle_maps), + file=file, + ) print("Local maps:", self.get_tensor_byte_size_mb( self.local_maps), file=file) + if self.save_robot_world_local_maps: + print("Robot world local maps: ", self.get_tensor_byte_size_mb( + self.robot_world_local_maps), file=file) print( - "Obstacle maps:", - self.get_tensor_byte_size_mb(self.obstacle_maps), - file=file, - ) + "Obstacle maps:", + self.get_tensor_byte_size_mb(self.obstacle_maps), + file=file, + ) print("Comm maps:", self.get_tensor_byte_size_mb( self.comm_maps), file=file) print( - "Coverage features:", - self.get_tensor_byte_size_mb(self.coverage_features), - file=file, - ) + "Coverage features:", + self.get_tensor_byte_size_mb(self.coverage_features), + file=file, + ) if __name__ == "__main__": From a891fc7fa7fc7a3bf3ee7233e3cb422357aa3eea Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Mon, 2 Jun 2025 18:01:38 -0400 Subject: [PATCH 26/35] large sensor size optimization --- cppsrc/core/include/CoverageControl/robot_model.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/robot_model.h b/cppsrc/core/include/CoverageControl/robot_model.h index a11e5160..c49ff20c 100644 --- a/cppsrc/core/include/CoverageControl/robot_model.h +++ b/cppsrc/core/include/CoverageControl/robot_model.h @@ -66,6 +66,7 @@ class RobotModel { Point2 global_start_position_, global_current_position_, noisy_global_current_position_; Point2 local_start_position_, local_current_position_, noisy_local_current_position_; double normalization_factor_ = 0; + bool is_clairvoyant_ = false; MapType robot_map_; //!< Stores what the robot has seen. Has the same //!< reference as world map. @@ -143,6 +144,9 @@ class RobotModel { } noise_normal_dist_ = std::normal_distribution{0, noise_sigma_}; } + if (params_.pSensorSize >= params_.pWorldMapSize) { + is_clairvoyant_ = true; + } normalization_factor_ = world_idf_->GetNormalizationFactor(); global_current_position_ = global_start_position_; @@ -160,6 +164,10 @@ class RobotModel { ClearRobotMap(); + if (is_clairvoyant_) { + robot_map_ = world_idf_->GetWorldMap(); + } + if (params_.pUpdateExplorationMap == true) { exploration_map_ = MapType::Constant(params_.pRobotMapSize, params_.pRobotMapSize, 1); @@ -240,7 +248,7 @@ class RobotModel { if (params_.pUpdateSensorView == true) { UpdateSensorView(); } - if (params_.pUpdateRobotMap == true) { + if (params_.pUpdateRobotMap == true and not is_clairvoyant_) { UpdateRobotMap(); } } @@ -308,7 +316,7 @@ class RobotModel { if (params_.pUpdateSensorView == true) { UpdateSensorView(); } - if (params_.pUpdateRobotMap == true) { + if (params_.pUpdateRobotMap == true and not is_clairvoyant_) { UpdateRobotMap(); } if (params_.pUpdateExplorationMap == true) { From 437f8c3b80c3acb1816da22a509c0301d938a02c Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 3 Jun 2025 05:03:57 -0400 Subject: [PATCH 27/35] improve clairvoyance --- .../include/CoverageControl/coverage_system.h | 82 ++------------ .../include/CoverageControl/robot_model.h | 16 +-- cppsrc/core/src/coverage_system.cpp | 103 ++++++++++++++++-- 3 files changed, 108 insertions(+), 93 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h index 12882a11..c2f2b7e7 100644 --- a/cppsrc/core/include/CoverageControl/coverage_system.h +++ b/cppsrc/core/include/CoverageControl/coverage_system.h @@ -70,6 +70,7 @@ class CoverageSystem { std::vector robots_; //!< Vector of robots of type RobotModel double normalization_factor_ = 0; //!< Normalization factor for the world IDF Voronoi voronoi_; //!< Voronoi object + bool is_clairvoyant_ = false; //!< If true, no need to update maps every step std::vector voronoi_cells_; //!< Voronoi cells for each robot mutable std::random_device rd_; //!< Random device for random number generation @@ -78,7 +79,8 @@ class CoverageSystem { std::uniform_real_distribution<> distrib_pts_; //!< Uniform distribution for generating random points PointVector robot_global_positions_; //!< Global positions of the robots - PointVector noisy_robot_global_positions_; //!< Global noisy positions of the robots + PointVector + noisy_robot_global_positions_; //!< Global noisy positions of the robots MapType system_map_; //!< System map contains explored and unexplored locations MapType @@ -102,33 +104,7 @@ class CoverageSystem { void InitSetup(); //! Update the exploration map, explored IDF map, and system map - void UpdateSystemMap() { - // This is not necessarily thread safe. Do NOT parallelize this for loop - for (size_t i = 0; i < num_robots_; ++i) { - MapUtils::MapBounds index, offset; - MapUtils::ComputeOffsets(params_.pResolution, robot_global_positions_[i], - params_.pSensorSize, params_.pWorldMapSize, - index, offset); - explored_idf_map_.block(index.left + offset.left, - index.bottom + offset.bottom, offset.width, - offset.height) = - GetRobotSensorView(i).block(offset.left, offset.bottom, offset.width, - offset.height); - exploration_map_.block( - index.left + offset.left, index.bottom + offset.bottom, offset.width, - offset.height) = MapType::Zero(offset.width, offset.height); - } - system_map_ = explored_idf_map_ - exploration_map_; - /* exploration_ratio_ = 1.0 - - * (double)(exploration_map_.sum())/(params_.pWorldMapSize * - * params_.pWorldMapSize); */ - /* weighted_exploration_ratio_ = - * (double)(explored_idf_map_.sum())/(total_idf_weight_); */ - /* std::cout << "Exploration: " << exploration_ratio_ << " Weighted: " << - * weighted_exploration_ratio_ << std::endl; */ - /* std::cout << "Diff: " << (exploration_map_.count() - - * exploration_map_.sum()) << std::endl; */ - } + void UpdateSystemMap(); //! Execute updates after a step for robot_id (avoid using this function, use //! PostStepCommands() instead). @@ -139,20 +115,8 @@ class CoverageSystem { //! of the system void PostStepCommands(); - //! Compute the adjacency matrix for communication - void ComputeAdjacencyMatrix(); - //! Update the positions of all robots from the RobotModel objects - void UpdateRobotPositions() { - for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { - robot_global_positions_[iRobot] = - robots_[iRobot].GetGlobalCurrentPosition(); - if (params_.pAddNoisePositions) { - noisy_robot_global_positions_[iRobot] = - robots_[iRobot].GetNoisyGlobalCurrentPosition(); - } - } - } + void UpdateRobotPositions(); //! Update the neighbors of all robots void UpdateNeighbors(); @@ -337,36 +301,10 @@ class CoverageSystem { return 0; } - //! Add noise to the given point and ensure within bounds - Point2 AddNoise(Point2 const pt) const; - //! Check if the robot is oscillating about its current position //! \warning This function is dependent on the size of the robot positions //! history - bool CheckOscillation(size_t const robot_id) const { - if (params_.pCheckOscillations == false) { - return false; - } - if (robot_positions_history_[robot_id].size() < 3) { - return false; - } - auto const &history = robot_positions_history_[robot_id]; - Point2 const last_pos = history.back(); - auto it_end = std::next(history.crbegin(), - std::min(6, static_cast(history.size()) - 1)); - bool flag = false; - int count = 0; - std::for_each(history.crbegin(), it_end, - [last_pos, &count](Point2 const &pt) { - if ((pt - last_pos).norm() < kLargeEps) { - ++count; - } - }); - if (count > 2) { - flag = true; - } - return flag; - } + bool CheckOscillation(size_t const robot_id) const; void CheckRobotID(size_t const id) const { if (id >= num_robots_) { @@ -374,13 +312,7 @@ class CoverageSystem { } } - void ComputeVoronoiCells() { - UpdateRobotPositions(); - voronoi_ = Voronoi(robot_global_positions_, GetWorldMap(), - Point2(params_.pWorldMapSize, params_.pWorldMapSize), - params_.pResolution); - voronoi_cells_ = voronoi_.GetVoronoiCells(); - } + void ComputeVoronoiCells(); /*! * Step a robot towards a given goal diff --git a/cppsrc/core/include/CoverageControl/robot_model.h b/cppsrc/core/include/CoverageControl/robot_model.h index c49ff20c..855b4803 100644 --- a/cppsrc/core/include/CoverageControl/robot_model.h +++ b/cppsrc/core/include/CoverageControl/robot_model.h @@ -144,7 +144,7 @@ class RobotModel { } noise_normal_dist_ = std::normal_distribution{0, noise_sigma_}; } - if (params_.pSensorSize >= params_.pWorldMapSize) { + if (params_.pSensorSize >= 2 * params_.pWorldMapSize) { is_clairvoyant_ = true; } @@ -166,19 +166,20 @@ class RobotModel { if (is_clairvoyant_) { robot_map_ = world_idf_->GetWorldMap(); + sensor_view_ = robot_map_; } - if (params_.pUpdateExplorationMap == true) { + if (params_.pUpdateExplorationMap == true and not is_clairvoyant_) { exploration_map_ = MapType::Constant(params_.pRobotMapSize, params_.pRobotMapSize, 1); local_exploration_map_ = - MapType::Constant(params_.pRobotMapSize, params_.pRobotMapSize, 1); + MapType::Constant(params_.pLocalMapSize, params_.pLocalMapSize, 1); UpdateExplorationMap(); } else { exploration_map_ = MapType::Constant(params_.pRobotMapSize, params_.pRobotMapSize, 0); local_exploration_map_ = - MapType::Constant(params_.pRobotMapSize, params_.pRobotMapSize, 0); + MapType::Constant(params_.pLocalMapSize, params_.pLocalMapSize, 0); } time_step_dist_ = @@ -245,7 +246,7 @@ class RobotModel { robot_map_ = MapType::Zero(params_.pRobotMapSize, params_.pRobotMapSize); } - if (params_.pUpdateSensorView == true) { + if (params_.pUpdateSensorView == true and not is_clairvoyant_) { UpdateSensorView(); } if (params_.pUpdateRobotMap == true and not is_clairvoyant_) { @@ -313,13 +314,13 @@ class RobotModel { noisy_local_current_position_ = noisy_global_current_position_ - global_start_position_; } - if (params_.pUpdateSensorView == true) { + if (params_.pUpdateSensorView == true and not is_clairvoyant_) { UpdateSensorView(); } if (params_.pUpdateRobotMap == true and not is_clairvoyant_) { UpdateRobotMap(); } - if (params_.pUpdateExplorationMap == true) { + if (params_.pUpdateExplorationMap == true and not is_clairvoyant_) { UpdateExplorationMap(); } } @@ -370,6 +371,7 @@ class RobotModel { return world_local_map_; } + const MapType &GetFullExplorationMap() const { return exploration_map_; } const MapType &GetExplorationMap() { /* local_exploration_map_ = MapType::Constant(params_.pLocalMapSize, * params_.pLocalMapSize, 0); */ diff --git a/cppsrc/core/src/coverage_system.cpp b/cppsrc/core/src/coverage_system.cpp index 01473214..8c529286 100644 --- a/cppsrc/core/src/coverage_system.cpp +++ b/cppsrc/core/src/coverage_system.cpp @@ -32,7 +32,6 @@ #include "CoverageControl/cgal/polygon_utils.h" #include "CoverageControl/coverage_system.h" -#include "CoverageControl/plotter.h" namespace CoverageControl { @@ -129,9 +128,12 @@ CoverageSystem::CoverageSystem(Parameters const ¶ms, } robots_.reserve(robot_positions.size()); num_robots_ = robot_positions.size(); - if(params_.pNumRobots != static_cast(num_robots_)) { - std::cerr << "Number of robots in the file does not match the number of robots in the parameters\n"; - std::cerr << "Number of robots in the file: " << num_robots_ << " Number of robots in the parameters: " << params_.pNumRobots << std::endl; + if (params_.pNumRobots != static_cast(num_robots_)) { + std::cerr << "Number of robots in the file does not match the number of " + "robots in the parameters\n"; + std::cerr << "Number of robots in the file: " << num_robots_ + << " Number of robots in the parameters: " << params_.pNumRobots + << std::endl; exit(1); } for (Point2 const &pos : robot_positions) { @@ -223,15 +225,50 @@ void CoverageSystem::InitSetup() { relative_positions_neighbors_[iRobot].reserve(num_robots_); neighbor_ids_[iRobot].reserve(num_robots_); } + if (params_.pSensorSize >= 2 * params_.pWorldMapSize) { + is_clairvoyant_ = true; + explored_idf_map_ = GetWorldMap(); + exploration_map_ = + MapType::Constant(params_.pWorldMapSize, params_.pWorldMapSize, 0); + system_map_ = explored_idf_map_ - exploration_map_; + } PostStepCommands(); } +void CoverageSystem::UpdateSystemMap() { + // This is not necessarily thread safe. Do NOT parallelize this for loop + for (size_t i = 0; i < num_robots_; ++i) { + MapUtils::MapBounds index, offset; + MapUtils::ComputeOffsets(params_.pResolution, robot_global_positions_[i], + params_.pSensorSize, params_.pWorldMapSize, index, + offset); + explored_idf_map_.block(index.left + offset.left, + index.bottom + offset.bottom, offset.width, + offset.height) = + GetRobotSensorView(i).block(offset.left, offset.bottom, offset.width, + offset.height); + exploration_map_.block( + index.left + offset.left, index.bottom + offset.bottom, offset.width, + offset.height) = MapType::Zero(offset.width, offset.height); + } + system_map_ = explored_idf_map_ - exploration_map_; + /* exploration_ratio_ = 1.0 - + * (double)(exploration_map_.sum())/(params_.pWorldMapSize * + * params_.pWorldMapSize); */ + /* weighted_exploration_ratio_ = + * (double)(explored_idf_map_.sum())/(total_idf_weight_); */ + /* std::cout << "Exploration: " << exploration_ratio_ << " Weighted: " << + * weighted_exploration_ratio_ << std::endl; */ + /* std::cout << "Diff: " << (exploration_map_.count() - + * exploration_map_.sum()) << std::endl; */ +} + void CoverageSystem::PostStepCommands(size_t robot_id) { robot_global_positions_[robot_id] = robots_[robot_id].GetGlobalCurrentPosition(); UpdateNeighbors(); - if (params_.pUpdateSystemMap) { + if (params_.pUpdateSystemMap and not is_clairvoyant_) { MapUtils::MapBounds index, offset; MapUtils::ComputeOffsets( params_.pResolution, robot_global_positions_[robot_id], @@ -258,7 +295,7 @@ void CoverageSystem::PostStepCommands(size_t robot_id) { void CoverageSystem::PostStepCommands() { UpdateRobotPositions(); UpdateNeighbors(); - if (params_.pUpdateSystemMap) { + if (params_.pUpdateSystemMap and not is_clairvoyant_) { UpdateSystemMap(); } for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { @@ -272,12 +309,23 @@ void CoverageSystem::PostStepCommands() { } } +void CoverageSystem::UpdateRobotPositions() { + for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { + robot_global_positions_[iRobot] = + robots_[iRobot].GetGlobalCurrentPosition(); + if (params_.pAddNoisePositions) { + noisy_robot_global_positions_[iRobot] = + robots_[iRobot].GetNoisyGlobalCurrentPosition(); + } + } +} + void CoverageSystem::UpdateNeighbors() { for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { relative_positions_neighbors_[iRobot].clear(); neighbor_ids_[iRobot].clear(); } - PointVector robot_global_positions = GetRobotPositions(); // Can be noisy + PointVector robot_global_positions = GetRobotPositions(); // Can be noisy for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { for (size_t jRobot = iRobot + 1; jRobot < num_robots_; ++jRobot) { Point2 relative_pos = @@ -292,6 +340,39 @@ void CoverageSystem::UpdateNeighbors() { } } +bool CoverageSystem::CheckOscillation(size_t const robot_id) const { + if (params_.pCheckOscillations == false) { + return false; + } + if (robot_positions_history_[robot_id].size() < 3) { + return false; + } + auto const &history = robot_positions_history_[robot_id]; + Point2 const last_pos = history.back(); + auto it_end = std::next(history.crbegin(), + std::min(6, static_cast(history.size()) - 1)); + bool flag = false; + int count = 0; + std::for_each(history.crbegin(), it_end, + [last_pos, &count](Point2 const &pt) { + if ((pt - last_pos).norm() < kLargeEps) { + ++count; + } + }); + if (count > 2) { + flag = true; + } + return flag; +} + +void CoverageSystem::ComputeVoronoiCells() { + UpdateRobotPositions(); + voronoi_ = Voronoi(robot_global_positions_, GetWorldMap(), + Point2(params_.pWorldMapSize, params_.pWorldMapSize), + params_.pResolution); + voronoi_cells_ = voronoi_.GetVoronoiCells(); +} + bool CoverageSystem::StepRobotToGoal(int const robot_id, Point2 const &goal, double const speed_factor) { Point2 curr_pos = robots_[robot_id].GetGlobalCurrentPosition(); @@ -401,9 +482,9 @@ void CoverageSystem::RenderRecordedMap(std::string const &dir_name, params_.pCommunicationRange); auto iPlotterVoronoi = plotter_voronoi; iPlotterVoronoi.SetPlotName("voronoi_map", i); - iPlotterVoronoi.PlotMap(plotter_data_[i].world_map, plotter_data_[i].positions, - plotter_data_[i].voronoi, - plotter_data_[i].positions_history); + iPlotterVoronoi.PlotMap( + plotter_data_[i].world_map, plotter_data_[i].positions, + plotter_data_[i].voronoi, plotter_data_[i].positions_history); } bool ffmpeg_call = system(("ffmpeg -y -r 30 -i " + frame_dir + @@ -546,7 +627,7 @@ void CoverageSystem::PlotRobotSystemMap(std::string const &dir_name, } void CoverageSystem::PlotRobotLocalMap(std::string const &dir_name, - int const &robot_id, int const &step) { + int const &robot_id, int const &step) { Plotter plotter(dir_name, params_.pLocalMapSize * params_.pResolution, params_.pResolution); plotter.SetScale(params_.pPlotScale); From ca8b4daa06b158a9cd59125b4ea8afaffdc73a55 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 3 Jun 2025 05:04:29 -0400 Subject: [PATCH 28/35] add EveryNumSteps --- params/eval.toml | 1 + python/evaluators/eval.py | 40 ++++++++++++++++++++------------------- 2 files changed, 22 insertions(+), 19 deletions(-) diff --git a/params/eval.toml b/params/eval.toml index 81bce591..1fae13df 100644 --- a/params/eval.toml +++ b/params/eval.toml @@ -4,6 +4,7 @@ EnvironmentConfig = "${CoverageControl_ws}/lpac/params/coverage_control_params.t EnvironmentDataDir = "${CoverageControl_ws}/lpac/envs/" # Absolute location NumEnvironments = 2 NumSteps = 600 +EveryNumSteps = 5 [[Controllers]] Name = "lpac" diff --git a/python/evaluators/eval.py b/python/evaluators/eval.py index c68d3c5b..8295aa8c 100644 --- a/python/evaluators/eval.py +++ b/python/evaluators/eval.py @@ -52,6 +52,7 @@ def __init__(self, in_config): self.num_features = self.cc_params.pNumGaussianFeatures self.num_envs = self.config["NumEnvironments"] self.num_steps = self.config["NumSteps"] + self.every_num_steps = self.config["EveryNumSteps"] self.columns = [ BarColumn(bar_width=None), @@ -66,7 +67,8 @@ def __init__(self, in_config): ] def evaluate(self, save=True): - cost_data = np.zeros((self.num_controllers, self.num_envs, self.num_steps)) + total_samples = self.num_steps // self.every_num_steps + cost_data = np.zeros((self.num_controllers, self.num_envs, total_samples)) with Progress(*self.columns, expand=True) as progress: task = progress.add_task( @@ -94,7 +96,7 @@ def evaluate(self, save=True): robot_init_pos = env_main.GetRobotPositions(force_no_noise=True) for controller_id in range(self.num_controllers): - step_count = 0 + sample_count = 0 env = CoverageSystem(self.cc_params, world_idf, robot_init_pos) if self.controllers_configs[controller_id]["Type"] == "Learning": @@ -105,30 +107,30 @@ def evaluate(self, save=True): self.controllers_configs[controller_id], self.cc_params, env ) initial_objective_value = env.GetObjectiveValue() - cost_data[controller_id, env_count, step_count] = ( + cost_data[controller_id, env_count, sample_count] = ( env.GetObjectiveValue() / initial_objective_value ) - step_count = step_count + 1 + sample_count = sample_count + 1 - while step_count < self.num_steps: + for step_count in range(1, self.num_steps): converged = controller.step(env) - objective_value = env.GetObjectiveValue() - normalized_objective_value = ( - objective_value / initial_objective_value - ) - cost_data[controller_id, env_count, step_count] = ( - normalized_objective_value - ) - - step_count = step_count + 1 - - if converged: - cost_data[controller_id, env_count, step_count:] = ( + if step_count % self.every_num_steps == 0: + objective_value = env.GetObjectiveValue() + normalized_objective_value = ( + objective_value / initial_objective_value + ) + cost_data[controller_id, env_count, sample_count] = ( normalized_objective_value ) - step_count = self.num_steps + sample_count = sample_count + 1 + + if converged: + cost_data[controller_id, env_count, sample_count:] = ( + normalized_objective_value + ) + step_count = self.num_steps + sample_count = total_samples - if (step_count) % 10 == 0 or step_count == self.num_steps: info = f"Controller {controller_id}/{self.num_controllers}: {controller.name} " progress.update( From e339ce6ff227f4cc09d3c90fba2ada1ed68153a4 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 3 Jun 2025 05:04:40 -0400 Subject: [PATCH 29/35] update parameters --- .../extern/tomlplusplus/toml.hpp | 216 +++++++-- .../core/include/CoverageControl/parameters.h | 13 +- cppsrc/core/src/parameters.cpp | 441 +++++++++--------- params/coverage_control_params.toml | 18 +- params/data_params.toml | 3 + params/learning_params.toml | 1 + 6 files changed, 424 insertions(+), 268 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/extern/tomlplusplus/toml.hpp b/cppsrc/core/include/CoverageControl/extern/tomlplusplus/toml.hpp index 0599bf5e..ef3515b1 100644 --- a/cppsrc/core/include/CoverageControl/extern/tomlplusplus/toml.hpp +++ b/cppsrc/core/include/CoverageControl/extern/tomlplusplus/toml.hpp @@ -212,6 +212,14 @@ #endif #endif +#ifndef TOML_NVCC +#ifdef __NVCOMPILER_MAJOR__ +#define TOML_NVCC __NVCOMPILER_MAJOR__ +#else +#define TOML_NVCC 0 +#endif +#endif + #ifndef TOML_ARCH_ITANIUM #if defined(__ia64__) || defined(__ia64) || defined(_IA64) || defined(__IA64__) || defined(_M_IA64) #define TOML_ARCH_ITANIUM 1 @@ -420,6 +428,7 @@ #endif // TOML_ALWAYS_INLINE +#ifndef TOML_ALWAYS_INLINE #ifdef _MSC_VER #define TOML_ALWAYS_INLINE __forceinline #elif TOML_GCC || TOML_CLANG || TOML_HAS_ATTR(__always_inline__) @@ -429,8 +438,10 @@ #else #define TOML_ALWAYS_INLINE inline #endif +#endif // TOML_NEVER_INLINE +#ifndef TOML_NEVER_INLINE #ifdef _MSC_VER #define TOML_NEVER_INLINE TOML_DECLSPEC(noinline) #elif TOML_CUDA // https://gitlab.gnome.org/GNOME/glib/-/issues/2555 @@ -443,19 +454,27 @@ #ifndef TOML_NEVER_INLINE #define TOML_NEVER_INLINE #endif +#endif // MSVC attributes +#ifndef TOML_ABSTRACT_INTERFACE #define TOML_ABSTRACT_INTERFACE TOML_DECLSPEC(novtable) +#endif +#ifndef TOML_EMPTY_BASES #define TOML_EMPTY_BASES TOML_DECLSPEC(empty_bases) +#endif // TOML_TRIVIAL_ABI +#ifndef TOML_TRIVIAL_ABI #if TOML_CLANG || TOML_HAS_ATTR(__trivial_abi__) #define TOML_TRIVIAL_ABI TOML_ATTR(__trivial_abi__) #else #define TOML_TRIVIAL_ABI #endif +#endif // TOML_NODISCARD +#ifndef TOML_NODISCARD #if TOML_CPP >= 17 && TOML_HAS_CPP_ATTR(nodiscard) >= 201603 #define TOML_NODISCARD [[nodiscard]] #elif TOML_CLANG || TOML_GCC || TOML_HAS_ATTR(__warn_unused_result__) @@ -463,13 +482,16 @@ #else #define TOML_NODISCARD #endif +#endif // TOML_NODISCARD_CTOR +#ifndef TOML_NODISCARD_CTOR #if TOML_CPP >= 17 && TOML_HAS_CPP_ATTR(nodiscard) >= 201907 #define TOML_NODISCARD_CTOR [[nodiscard]] #else #define TOML_NODISCARD_CTOR #endif +#endif // pure + const #ifndef TOML_PURE @@ -519,6 +541,7 @@ #endif // TOML_ASSUME +#ifndef TOML_ASSUME #ifdef _MSC_VER #define TOML_ASSUME(expr) __assume(expr) #elif TOML_ICC || TOML_CLANG || TOML_HAS_BUILTIN(__builtin_assume) @@ -530,8 +553,10 @@ #else #define TOML_ASSUME(expr) static_cast(0) #endif +#endif // TOML_UNREACHABLE +#ifndef TOML_UNREACHABLE #ifdef _MSC_VER #define TOML_UNREACHABLE __assume(0) #elif TOML_ICC || TOML_CLANG || TOML_GCC || TOML_HAS_BUILTIN(__builtin_unreachable) @@ -539,6 +564,7 @@ #else #define TOML_UNREACHABLE static_cast(0) #endif +#endif // TOML_LIKELY #if TOML_CPP >= 20 && TOML_HAS_CPP_ATTR(likely) >= 201803 @@ -1065,6 +1091,10 @@ // 256 is crazy high! if you're hitting this limit with real input, TOML is probably the wrong tool for the job... #endif +#ifndef TOML_MAX_DOTTED_KEYS_DEPTH +#define TOML_MAX_DOTTED_KEYS_DEPTH 1024 +#endif + #ifdef TOML_CHAR_8_STRINGS #if TOML_CHAR_8_STRINGS #error TOML_CHAR_8_STRINGS was removed in toml++ 2.0.0; all value setters and getters now work with char8_t strings implicitly. @@ -1103,6 +1133,20 @@ TOML_ENABLE_WARNINGS; #define TOML_ENABLE_FLOAT16 0 #endif +#ifndef TOML_DISABLE_CONDITIONAL_NOEXCEPT_LAMBDA +#define TOML_DISABLE_CONDITIONAL_NOEXCEPT_LAMBDA 0 +#endif + +#ifndef TOML_DISABLE_NOEXCEPT_NOEXCEPT +#define TOML_DISABLE_NOEXCEPT_NOEXCEPT 0 + #ifdef _MSC_VER + #if _MSC_VER <= 1943 // Up to Visual Studio 2022 Version 17.13.6 + #undef TOML_DISABLE_NOEXCEPT_NOEXCEPT + #define TOML_DISABLE_NOEXCEPT_NOEXCEPT 1 + #endif + #endif +#endif + #if !defined(TOML_FLOAT_CHARCONV) && (TOML_GCC || TOML_CLANG || (TOML_ICC && !TOML_ICC_CL)) // not supported by any version of GCC or Clang as of 26/11/2020 // not supported by any version of ICC on Linux as of 11/01/2021 @@ -2600,6 +2644,60 @@ TOML_NAMESPACE_START return lhs; } }; + + TOML_NODISCARD + constexpr optional get_line(std::string_view doc, source_index line_num) noexcept + { + if (line_num == 0) + { + // Invalid line number. Should be greater than zero. + return {}; + } + + // The position of the first character of the specified line. + const auto begin_of_line = [doc, line_num]() -> std::size_t + { + if (line_num == 1) + { + return 0; + } + + const auto num_chars_of_doc = doc.size(); + std::size_t current_line_num{ 1 }; + + for (std::size_t i{}; i < num_chars_of_doc; ++i) + { + if (doc[i] == '\n') + { + ++current_line_num; + + if (current_line_num == line_num) + { + return i + 1; + } + } + } + return std::string_view::npos; + }(); + + if (begin_of_line >= doc.size()) + { + return {}; + } + + if (const auto end_of_line = doc.find('\n', begin_of_line); end_of_line != std::string_view::npos) + { + const auto num_chars_of_line = end_of_line - begin_of_line; + + // Trim an optional trailing carriage return. + return doc.substr(begin_of_line, + ((num_chars_of_line > 0) && (doc[end_of_line - 1] == '\r')) ? num_chars_of_line - 1 + : num_chars_of_line); + } + + // Return the last line. Apparently this doc has no trailing line break character at the end. + return doc.substr(begin_of_line); + } } TOML_NAMESPACE_END; @@ -3570,7 +3668,7 @@ TOML_NAMESPACE_START { TOML_NODISCARD TOML_ALWAYS_INLINE - path operator"" _tpath(const char* str, size_t len) + path operator""_tpath(const char* str, size_t len) { return path(std::string_view{ str, len }); } @@ -3614,6 +3712,16 @@ TOML_PUSH_WARNINGS; #undef max #endif +// workaround for this: https://github.com/marzer/tomlplusplus/issues/220 +#if TOML_NVCC +#define TOML_NVCC_WORKAROUND \ + { \ + return {}; \ + } +#else +#define TOML_NVCC_WORKAROUND = 0 +#endif + TOML_NAMESPACE_START { class TOML_ABSTRACT_INTERFACE TOML_EXPORTED_CLASS node @@ -3734,10 +3842,10 @@ TOML_NAMESPACE_START TOML_EXPORTED_MEMBER_FUNCTION virtual ~node() noexcept; - TOML_PURE_GETTER + TOML_NODISCARD virtual bool is_homogeneous(node_type ntype, node*& first_nonmatch) noexcept = 0; - TOML_PURE_GETTER + TOML_NODISCARD virtual bool is_homogeneous(node_type ntype, const node*& first_nonmatch) const noexcept = 0; TOML_PURE_GETTER @@ -3756,16 +3864,16 @@ TOML_NAMESPACE_START } TOML_PURE_GETTER - virtual node_type type() const noexcept = 0; + virtual node_type type() const noexcept TOML_NVCC_WORKAROUND; TOML_PURE_GETTER - virtual bool is_table() const noexcept = 0; + virtual bool is_table() const noexcept TOML_NVCC_WORKAROUND; TOML_PURE_GETTER virtual bool is_array() const noexcept = 0; TOML_PURE_GETTER - virtual bool is_array_of_tables() const noexcept = 0; + virtual bool is_array_of_tables() const noexcept TOML_NVCC_WORKAROUND; TOML_PURE_GETTER virtual bool is_value() const noexcept = 0; @@ -3820,6 +3928,8 @@ TOML_NAMESPACE_START return is_time(); else if constexpr (std::is_same_v) return is_date_time(); + + TOML_UNREACHABLE; } TOML_PURE_GETTER @@ -3902,6 +4012,8 @@ TOML_NAMESPACE_START return as_time(); else if constexpr (std::is_same_v) return as_date_time(); + + TOML_UNREACHABLE; } template @@ -3930,6 +4042,8 @@ TOML_NAMESPACE_START return as_time(); else if constexpr (std::is_same_v) return as_date_time(); + + TOML_UNREACHABLE; } template @@ -4207,6 +4321,8 @@ TOML_IMPL_NAMESPACE_START } TOML_IMPL_NAMESPACE_END; +#undef TOML_NVCC_WORKAROUND + #ifdef _MSC_VER #pragma pop_macro("min") #pragma pop_macro("max") @@ -4388,7 +4504,7 @@ TOML_NAMESPACE_START return node_->is_homogeneous(ntype, first_nonmatch); } - TOML_NODISCARD + TOML_PURE_GETTER bool is_homogeneous(node_type ntype) const noexcept { return node_ ? node_->is_homogeneous(ntype) : false; @@ -4982,8 +5098,11 @@ TOML_NAMESPACE_START (impl::value_variadic_ctor_allowed, impl::remove_cvref...>::value), typename... Args) TOML_NODISCARD_CTOR - explicit value(Args&&... args) noexcept(noexcept(value_type( - impl::native_value_maker...>::make(static_cast(args)...)))) + explicit value(Args&&... args) +#if !TOML_DISABLE_NOEXCEPT_NOEXCEPT + noexcept(noexcept(value_type( + impl::native_value_maker...>::make(static_cast(args)...)))) +#endif : val_(impl::native_value_maker...>::make(static_cast(args)...)) { #if TOML_LIFETIME_HOOKS @@ -5074,7 +5193,7 @@ TOML_NAMESPACE_START return ntype == node_type::none || ntype == impl::node_type_of; } - TOML_PURE_GETTER + TOML_NODISCARD bool is_homogeneous(node_type ntype, node*& first_nonmatch) noexcept final { if (ntype != node_type::none && ntype != impl::node_type_of) @@ -5085,7 +5204,7 @@ TOML_NAMESPACE_START return true; } - TOML_PURE_GETTER + TOML_NODISCARD bool is_homogeneous(node_type ntype, const node*& first_nonmatch) const noexcept final { if (ntype != node_type::none && ntype != impl::node_type_of) @@ -5599,11 +5718,11 @@ TOML_NAMESPACE_START "Retrieving values as wide-character strings with node::value_exact() is only " "supported on Windows with TOML_ENABLE_WINDOWS_COMPAT enabled."); - static_assert((is_native || can_represent_native)&&!is_cvref, + static_assert((is_native || can_represent_native) && !is_cvref, TOML_SA_VALUE_EXACT_FUNC_MESSAGE("return type of node::value_exact()")); // prevent additional compiler error spam when the static_assert fails by gating behind if constexpr - if constexpr ((is_native || can_represent_native)&&!is_cvref) + if constexpr ((is_native || can_represent_native) && !is_cvref) { if (type() == node_type_of) return { this->get_value_exact() }; @@ -5621,7 +5740,7 @@ TOML_NAMESPACE_START static_assert(!is_wide_string || TOML_ENABLE_WINDOWS_COMPAT, "Retrieving values as wide-character strings with node::value() is only " "supported on Windows with TOML_ENABLE_WINDOWS_COMPAT enabled."); - static_assert((is_native || can_represent_native || can_partially_represent_native)&&!is_cvref, + static_assert((is_native || can_represent_native || can_partially_represent_native) && !is_cvref, TOML_SA_VALUE_FUNC_MESSAGE("return type of node::value()")); // when asking for strings, dates, times and date_times there's no 'fuzzy' conversion @@ -6334,11 +6453,11 @@ TOML_NAMESPACE_START TOML_EXPORTED_MEMBER_FUNCTION bool is_homogeneous(node_type ntype) const noexcept final; - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXPORTED_MEMBER_FUNCTION bool is_homogeneous(node_type ntype, node*& first_nonmatch) noexcept final; - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXPORTED_MEMBER_FUNCTION bool is_homogeneous(node_type ntype, const node*& first_nonmatch) const noexcept final; @@ -6751,7 +6870,10 @@ TOML_NAMESPACE_START static_cast(static_cast(arr)[i]) .visit( [&]([[maybe_unused]] auto&& elem) // +// Define this macro as a workaround to compile errors caused by a bug in MSVC's "legacy lambda processor". +#if !TOML_DISABLE_CONDITIONAL_NOEXCEPT_LAMBDA noexcept(for_each_is_nothrow_one::value) +#endif { using elem_ref = for_each_elem_ref; static_assert(std::is_reference_v); @@ -7023,8 +7145,8 @@ TOML_NAMESPACE_START { using raw_elem_type = impl::remove_cvref; using elem_type = std::conditional_t, // - impl::emplaced_type_of, - raw_elem_type>; + impl::emplaced_type_of, + raw_elem_type>; using type = impl::remove_cvref>; static_assert(impl::is_native || impl::is_one_of, @@ -7061,8 +7183,8 @@ TOML_NAMESPACE_START { using raw_elem_type = impl::remove_cvref; using elem_type = std::conditional_t, // - impl::emplaced_type_of, - raw_elem_type>; + impl::emplaced_type_of, + raw_elem_type>; static constexpr auto moving_node_ptr = std::is_same_v // && sizeof...(Args) == 1u // @@ -7677,11 +7799,11 @@ TOML_NAMESPACE_START TOML_EXPORTED_MEMBER_FUNCTION bool is_homogeneous(node_type ntype) const noexcept final; - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXPORTED_MEMBER_FUNCTION bool is_homogeneous(node_type ntype, node*& first_nonmatch) noexcept final; - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXPORTED_MEMBER_FUNCTION bool is_homogeneous(node_type ntype, const node*& first_nonmatch) const noexcept final; @@ -8113,7 +8235,10 @@ TOML_NAMESPACE_START static_cast(*kvp.second) .visit( [&]([[maybe_unused]] auto&& v) // +// Define this macro as a workaround to compile errors caused by a bug in MSVC's "legacy lambda processor". +#if !TOML_DISABLE_CONDITIONAL_NOEXCEPT_LAMBDA noexcept(for_each_is_nothrow_one::value) +#endif { using value_ref = for_each_value_ref; static_assert(std::is_reference_v); @@ -8416,6 +8541,8 @@ TOML_NAMESPACE_START } return iterator{ ipos }; } + + TOML_UNREACHABLE; } TOML_CONSTRAINED_TEMPLATE((is_key_or_convertible || impl::is_wide_string), @@ -9598,7 +9725,7 @@ TOML_NAMESPACE_START TOML_NODISCARD TOML_ALWAYS_INLINE - parse_result operator"" _toml(const char* str, size_t len) + parse_result operator""_toml(const char* str, size_t len) { return parse(std::string_view{ str, len }); } @@ -9607,7 +9734,7 @@ TOML_NAMESPACE_START TOML_NODISCARD TOML_ALWAYS_INLINE - parse_result operator"" _toml(const char8_t* str, size_t len) + parse_result operator""_toml(const char8_t* str, size_t len) { return parse(std::u8string_view{ str, len }); } @@ -10644,6 +10771,11 @@ TOML_IMPL_NAMESPACE_START void TOML_CALLCONV print_to_stream(std::ostream & stream, const source_region& val) { print_to_stream(stream, val.begin); + if (val.begin != val.end) + { + print_to_stream(stream, " to "sv); + print_to_stream(stream, val.end); + } if (val.path) { print_to_stream(stream, " of '"sv); @@ -11813,7 +11945,7 @@ TOML_NAMESPACE_START return true; } - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXTERNAL_LINKAGE bool array::is_homogeneous(node_type ntype, node * &first_nonmatch) noexcept { @@ -11835,7 +11967,7 @@ TOML_NAMESPACE_START return true; } - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXTERNAL_LINKAGE bool array::is_homogeneous(node_type ntype, const node*& first_nonmatch) const noexcept { @@ -12168,7 +12300,7 @@ TOML_NAMESPACE_START return true; } - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXTERNAL_LINKAGE bool table::is_homogeneous(node_type ntype, node * &first_nonmatch) noexcept { @@ -12191,7 +12323,7 @@ TOML_NAMESPACE_START return true; } - TOML_PURE_GETTER + TOML_NODISCARD TOML_EXTERNAL_LINKAGE bool table::is_homogeneous(node_type ntype, const node*& first_nonmatch) const noexcept { @@ -12631,7 +12763,7 @@ TOML_ANON_NAMESPACE_START return value; } }; - static_assert(std::is_trivial_v); + static_assert(std::is_trivially_default_constructible_v && std::is_trivially_copyable_v); static_assert(std::is_standard_layout_v); struct TOML_ABSTRACT_INTERFACE utf8_reader_interface @@ -13508,7 +13640,8 @@ TOML_IMPL_NAMESPACE_START class parser { private: - static constexpr size_t max_nested_values = TOML_MAX_NESTED_VALUES; + static constexpr size_t max_nested_values = TOML_MAX_NESTED_VALUES; + static constexpr size_t max_dotted_keys_depth = TOML_MAX_DOTTED_KEYS_DEPTH; utf8_buffered_reader reader; table root; @@ -14688,7 +14821,7 @@ TOML_IMPL_NAMESPACE_START set_error_and_return_default("'"sv, traits::full_prefix, std::string_view{ digits, length }, - "' is not representable in 64 bits"sv); + "' is not representable as a signed 64-bit integer"sv); // do the thing { @@ -14712,7 +14845,7 @@ TOML_IMPL_NAMESPACE_START set_error_and_return_default("'"sv, traits::full_prefix, std::string_view{ digits, length }, - "' is not representable in 64 bits"sv); + "' is not representable as a signed 64-bit integer"sv); if constexpr (traits::is_signed) { @@ -15529,6 +15662,11 @@ TOML_IMPL_NAMESPACE_START // store segment key_buffer.push_back(key_segment, key_begin, key_end); + if TOML_UNLIKELY(key_buffer.size() > max_dotted_keys_depth) + set_error_and_return_default("exceeded maximum dotted keys depth of "sv, + max_dotted_keys_depth, + " (TOML_MAX_DOTTED_KEYS_DEPTH)"sv); + // eof or no more key to come if (is_eof() || *cp != U'.') break; @@ -16205,16 +16343,10 @@ TOML_ANON_NAMESPACE_START { #if TOML_EXCEPTIONS #define TOML_PARSE_FILE_ERROR(msg, path) \ - throw parse_error{ msg, source_position{}, std::make_shared(std::move(path)) } + throw parse_error(msg, source_position{}, std::make_shared(std::move(path))) #else #define TOML_PARSE_FILE_ERROR(msg, path) \ - return parse_result \ - { \ - parse_error \ - { \ - msg, source_position{}, std::make_shared(std::move(path)) \ - } \ - } + return parse_result(parse_error(msg, source_position{}, std::make_shared(std::move(path)))) #endif std::string file_path_str(file_path); @@ -16223,7 +16355,7 @@ TOML_ANON_NAMESPACE_START std::ifstream file; TOML_OVERALIGNED char file_buffer[sizeof(void*) * 1024u]; file.rdbuf()->pubsetbuf(file_buffer, sizeof(file_buffer)); -#if TOML_WINDOWS +#if TOML_WINDOWS && !(defined(__MINGW32__) || defined(__MINGW64__)) file.open(impl::widen(file_path_str).c_str(), std::ifstream::in | std::ifstream::binary | std::ifstream::ate); #else file.open(file_path_str, std::ifstream::in | std::ifstream::binary | std::ifstream::ate); @@ -16976,7 +17108,6 @@ TOML_ANON_NAMESPACE_START val *= -1.0; } return weight + static_cast(log10(val)) + 1u; - break; } case node_type::boolean: return 5u; @@ -17701,6 +17832,7 @@ TOML_POP_WARNINGS; #undef TOML_NEVER_INLINE #undef TOML_NODISCARD #undef TOML_NODISCARD_CTOR +#undef TOML_NVCC #undef TOML_OPEN_ENUM #undef TOML_OPEN_FLAGS_ENUM #undef TOML_PARSER_TYPENAME diff --git a/cppsrc/core/include/CoverageControl/parameters.h b/cppsrc/core/include/CoverageControl/parameters.h index cdf12718..a2b6fc05 100644 --- a/cppsrc/core/include/CoverageControl/parameters.h +++ b/cppsrc/core/include/CoverageControl/parameters.h @@ -32,11 +32,10 @@ #ifndef CPPSRC_CORE_INCLUDE_COVERAGECONTROL_PARAMETERS_H_ #define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_PARAMETERS_H_ +#include #include #include -#include - namespace CoverageControl { /*! @@ -55,7 +54,7 @@ class Parameters { * \name Environment Parameters * @{ */ - int pNumRobots = 32; //!< Number of robots + int pNumRobots = 32; //!< Number of robots /*! \name IO Parameters * @{ @@ -171,15 +170,15 @@ class Parameters { /*! @} */ /*! @} */ - Parameters() {} + Parameters() = default; - explicit Parameters(std::string const &config_file) + explicit Parameters(std::string const& config_file) : config_file_{config_file} { ParseParameters(); /* PrintParameters(); */ } - void SetConfig(std::string const &config_file) { + void SetConfig(std::string const& config_file) { config_file_ = config_file; ParseParameters(); } @@ -187,7 +186,9 @@ class Parameters { void PrintParameters() const; private: + void ParseParameters(); + void ValidateParameters(); }; /*! diff --git a/cppsrc/core/src/parameters.cpp b/cppsrc/core/src/parameters.cpp index 4ecdac00..bf7a9280 100644 --- a/cppsrc/core/src/parameters.cpp +++ b/cppsrc/core/src/parameters.cpp @@ -28,277 +28,287 @@ #include #include +#include +#include #include "CoverageControl/extern/tomlplusplus/toml.hpp" #include "CoverageControl/parameters.h" namespace CoverageControl { -void Parameters::ParseParameters() { - std::cout << std::boolalpha; - // std::cout << "Using config file: " << config_file_ << std::endl; - if (not std::filesystem::exists(config_file_)) { - std::cerr << "Could not find config file " << config_file_ << std::endl; - throw std::runtime_error("Could not open config file"); +namespace { + +template +bool SafeExtractValue(const toml::node_view& node, T& target, + const std::string& param_name = "", + bool log_default = true) { + if (auto val = node.value()) { + target = val.value(); + return true; } - - toml::table toml_config; - try { - toml_config = toml::parse_file(config_file_); - } catch (const std::exception& e) { - std::cerr << "Error parsing config file: " << e.what() << std::endl; - std::cerr << "Please check the config file format" << std::endl; - std::cerr << "File: " << config_file_ << std::endl; - throw std::runtime_error("Error parsing config file"); + if (log_default && !param_name.empty()) { + std::cout << param_name << " (default): " << target << std::endl; } + return false; +} - if (toml_config["NumRobots"].value()) { - if (toml_config["NumRobots"].value().value() < 1) { - std::cerr << "NumRobots must be greater than 0" << std::endl; - throw std::runtime_error("NumRobots must be greater than 0"); - } - pNumRobots = toml_config["NumRobots"].value().value(); - } else { - std::cout << "NumRobots (default): " << pNumRobots << std::endl; +bool ValidatePositiveInt(int value, const std::string& param_name) { + if (value <= 0) { + std::cerr << param_name << " must be greater than 0, got: " << value + << std::endl; + return false; } + return true; +} - auto toml_IO = toml_config["IO"]; - if (toml_IO["PlotScale"].value()) { - pPlotScale = toml_IO["PlotScale"].value().value(); - } else { - std::cout << "PlotScale (default): " << pPlotScale << std::endl; +bool ValidatePositiveDouble(double value, const std::string& param_name) { + if (value <= 0.0) { + std::cerr << param_name << " must be greater than 0.0, got: " << value + << std::endl; + return false; } + return true; +} - auto toml_EnvironmentMaps = toml_config["Environment"]["Maps"]; - - if (toml_EnvironmentMaps) { - auto toml_Resolution = toml_EnvironmentMaps["Resolution"].value(); - auto toml_WorldMapSize = toml_EnvironmentMaps["WorldMapSize"].value(); - auto toml_RobotMapSize = toml_EnvironmentMaps["RobotMapSize"].value(); - auto toml_LocalMapSize = toml_EnvironmentMaps["LocalMapSize"].value(); - - if (toml_Resolution) { - pResolution = toml_Resolution.value(); - } - - if (toml_WorldMapSize) { - pWorldMapSize = toml_WorldMapSize.value(); - } - if (toml_RobotMapSize) { - pRobotMapSize = toml_RobotMapSize.value(); - } - if (toml_LocalMapSize) { - pLocalMapSize = toml_LocalMapSize.value(); - } +bool ValidateRange(double value, double min, double max, + const std::string& param_name) { + if (value < min || value > max) { + std::cerr << param_name << " must be between " << min << " and " << max + << ", got: " << value << std::endl; + return false; + } + return true; +} +} // namespace - auto toml_EnvironmentMapsUpdateSettings = - toml_EnvironmentMaps["UpdateSettings"]; - - if (toml_EnvironmentMapsUpdateSettings) { - auto toml_UpdateRobotMap = - toml_EnvironmentMapsUpdateSettings["UpdateRobotMap"].value(); - auto toml_UpdateSensorView = - toml_EnvironmentMapsUpdateSettings["UpdateSensorView"].value(); - auto toml_UpdateExplorationMap = - toml_EnvironmentMapsUpdateSettings["UpdateExplorationMap"] - .value(); - auto toml_UpdateSystemMap = - toml_EnvironmentMapsUpdateSettings["UpdateSystemMap"].value(); - - if (toml_UpdateRobotMap) { - pUpdateRobotMap = toml_UpdateRobotMap.value(); - } - if (toml_UpdateSensorView) { - pUpdateSensorView = toml_UpdateSensorView.value(); - } - if (toml_UpdateExplorationMap) { - pUpdateExplorationMap = toml_UpdateExplorationMap.value(); - } - if (toml_UpdateSystemMap) { - pUpdateSystemMap = toml_UpdateSystemMap.value(); - } - } +void Parameters::ParseParameters() { + std::cout << std::boolalpha; + // std::cout << "Using config file: " << config_file_ << std::endl; + if (not std::filesystem::exists(config_file_)) { + throw std::runtime_error("Config file does not exist: " + config_file_); } - auto toml_EnvironmentIDF = toml_config["Environment"]["IDF"]; + toml::table config; + try { + config = toml::parse_file(config_file_); + } catch (const toml::parse_error& e) { + std::cerr << "TOML parse error in " << config_file_ << ":\n" + << " Line " << e.source().begin.line << ", Column " + << e.source().begin.column << ": " << e.description() + << std::endl; + throw std::runtime_error("Failed to parse TOML config file"); + } catch (const std::exception& e) { + std::cerr << "Error reading config file " << config_file_ << ": " + << e.what() << std::endl; + throw std::runtime_error("Failed to read config file"); + } - if (toml_EnvironmentIDF) { - if (toml_EnvironmentIDF["NumGaussianFeatures"].value()) { - pNumGaussianFeatures = toml_EnvironmentIDF["NumGaussianFeatures"].value().value(); - } else { - std::cout << "NumGaussianFeatures (default): " << pNumGaussianFeatures << std::endl; - } - auto toml_TruncationBND = - toml_EnvironmentIDF["TruncationBND"].value(); - auto toml_Norm = toml_EnvironmentIDF["Norm"].value(); - auto toml_MinSigma = toml_EnvironmentIDF["MinSigma"].value(); - auto toml_MaxSigma = toml_EnvironmentIDF["MaxSigma"].value(); - auto toml_MinPeak = toml_EnvironmentIDF["MinPeak"].value(); - auto toml_MaxPeak = toml_EnvironmentIDF["MaxPeak"].value(); - auto toml_UnknownImportance = - toml_EnvironmentIDF["UnknownImportance"].value(); - auto toml_RobotMapUseUnknownImportance = - toml_EnvironmentIDF["RobotMapUseUnknownImportance"].value(); - - if (toml_TruncationBND) { - pTruncationBND = toml_TruncationBND.value(); - } - if (toml_Norm) { - pNorm = toml_Norm.value(); - } - if (toml_MinSigma) { - pMinSigma = toml_MinSigma.value(); - } - if (toml_MaxSigma) { - pMaxSigma = toml_MaxSigma.value(); - } - if (toml_MinPeak) { - pMinPeak = toml_MinPeak.value(); - } - if (toml_MaxPeak) { - pMaxPeak = toml_MaxPeak.value(); + try { + if (SafeExtractValue(config["NumRobots"], pNumRobots, "NumRobots")) { + if (!ValidatePositiveInt(pNumRobots, "NumRobots")) { + throw std::runtime_error("Invalid NumRobots value"); + } } - if (toml_EnvironmentIDF["NumPolygons"].value()) { - pNumPolygons = toml_EnvironmentIDF["NumPolygons"].value().value(); - } else { - std::cout << "NumPolygons (default): " << pNumPolygons << std::endl; + if (auto env_maps = config["Environment"]["Maps"]) { + SafeExtractValue(env_maps["Resolution"], pResolution, "Resolution"); + SafeExtractValue(env_maps["WorldMapSize"], pWorldMapSize, "WorldMapSize"); + SafeExtractValue(env_maps["RobotMapSize"], pRobotMapSize, "RobotMapSize"); + SafeExtractValue(env_maps["LocalMapSize"], pLocalMapSize, "LocalMapSize"); + + // UpdateSettings subsection + if (auto update_settings = env_maps["UpdateSettings"]) { + SafeExtractValue(update_settings["UpdateRobotMap"], pUpdateRobotMap, + "UpdateRobotMap"); + SafeExtractValue(update_settings["UpdateSensorView"], pUpdateSensorView, + "UpdateSensorView"); + SafeExtractValue(update_settings["UpdateExplorationMap"], + pUpdateExplorationMap, "UpdateExplorationMap"); + SafeExtractValue(update_settings["UpdateSystemMap"], pUpdateSystemMap, + "UpdateSystemMap"); + } } - if (toml_EnvironmentIDF["MaxVertices"].value()) { - pMaxVertices = toml_EnvironmentIDF["MaxVertices"].value().value(); - } else { - std::cout << "MaxVertices (default): " << pMaxVertices << std::endl; + // Environment.IDF section + if (auto env_idf = config["Environment"]["IDF"]) { + SafeExtractValue(env_idf["NumGaussianFeatures"], pNumGaussianFeatures, + "NumGaussianFeatures"); + SafeExtractValue(env_idf["TruncationBND"], pTruncationBND, + "TruncationBND"); + SafeExtractValue(env_idf["Norm"], pNorm, "Norm"); + SafeExtractValue(env_idf["MinSigma"], pMinSigma, "MinSigma"); + SafeExtractValue(env_idf["MaxSigma"], pMaxSigma, "MaxSigma"); + SafeExtractValue(env_idf["MinPeak"], pMinPeak, "MinPeak"); + SafeExtractValue(env_idf["MaxPeak"], pMaxPeak, "MaxPeak"); + SafeExtractValue(env_idf["NumPolygons"], pNumPolygons, "NumPolygons"); + SafeExtractValue(env_idf["MaxVertices"], pMaxVertices, "MaxVertices"); + SafeExtractValue(env_idf["PolygonRadius"], pPolygonRadius, + "PolygonRadius"); + SafeExtractValue(env_idf["UnknownImportance"], pUnknownImportance, + "UnknownImportance"); + SafeExtractValue(env_idf["RobotMapUseUnknownImportance"], + pRobotMapUseUnknownImportance, + "RobotMapUseUnknownImportance"); + } + + if (auto io_section = config["IO"]) { + SafeExtractValue(io_section["PlotScale"], pPlotScale, "PlotScale"); + } + if (auto robot_model = config["RobotModel"]) { + SafeExtractValue(robot_model["SensorSize"], pSensorSize, "SensorSize"); + SafeExtractValue(robot_model["CommunicationRange"], pCommunicationRange, + "CommunicationRange"); + SafeExtractValue(robot_model["MaxRobotSpeed"], pMaxRobotSpeed, + "MaxRobotSpeed"); + SafeExtractValue(robot_model["RobotInitDist"], pRobotInitDist, + "RobotInitDist"); + SafeExtractValue(robot_model["RobotPosHistorySize"], pRobotPosHistorySize, + "RobotPosHistorySize"); + SafeExtractValue(robot_model["TimeStep"], pTimeStep, "TimeStep"); + + // AddNoise subsection + if (auto add_noise = robot_model["AddNoise"]) { + SafeExtractValue(add_noise["AddNoisePositions"], pAddNoisePositions, + "AddNoisePositions"); + SafeExtractValue(add_noise["PositionsNoiseSigmaMin"], + pPositionsNoiseSigmaMin, "PositionsNoiseSigmaMin"); + SafeExtractValue(add_noise["PositionsNoiseSigmaMax"], + pPositionsNoiseSigmaMax, "PositionsNoiseSigmaMax"); + } } + if (auto algorithm = config["Algorithm"]) { + SafeExtractValue(algorithm["EpisodeSteps"], pEpisodeSteps, + "EpisodeSteps"); + SafeExtractValue(algorithm["CheckOscillations"], pCheckOscillations, + "CheckOscillations"); + + // Global-CVT subsection + if (auto global_cvt = algorithm["Global-CVT"]) { + SafeExtractValue(global_cvt["LloydMaxIterations"], pLloydMaxIterations, + "LloydMaxIterations"); + SafeExtractValue(global_cvt["LloydNumTries"], pLloydNumTries, + "LloydNumTries"); + } - if (toml_EnvironmentIDF["PolygonRadius"].value()) { - pPolygonRadius = toml_EnvironmentIDF["PolygonRadius"].value().value(); - } else { - std::cout << "PolygonRadius (default): " << pPolygonRadius << std::endl; + // Exploration subsection + if (auto exploration = algorithm["Exploration"]) { + SafeExtractValue(exploration["NumFrontiers"], pNumFrontiers, + "NumFrontiers"); + } } - if (toml_UnknownImportance) { - pUnknownImportance = toml_UnknownImportance.value(); - } - if (toml_RobotMapUseUnknownImportance) { - pRobotMapUseUnknownImportance = toml_RobotMapUseUnknownImportance.value(); - } + } catch (const std::exception& e) { + std::cerr << "Error parsing parameters: " << e.what() << std::endl; + throw; } + ValidateParameters(); +} - auto toml_RobotModel = toml_config["RobotModel"]; - - if (toml_RobotModel) { - auto toml_SensorSize = toml_RobotModel["SensorSize"].value(); - auto toml_CommunicationRange = - toml_RobotModel["CommunicationRange"].value(); - auto toml_MaxRobotSpeed = toml_RobotModel["MaxRobotSpeed"].value(); - auto toml_RobotInitDist = toml_RobotModel["RobotInitDist"].value(); - auto toml_RobotPosHistorySize = - toml_RobotModel["RobotPosHistorySize"].value(); - auto toml_TimeStep = toml_RobotModel["TimeStep"].value(); - - if (toml_SensorSize) { - pSensorSize = toml_SensorSize.value(); - } - if (toml_CommunicationRange) { - pCommunicationRange = toml_CommunicationRange.value(); - } - if (toml_MaxRobotSpeed) { - pMaxRobotSpeed = toml_MaxRobotSpeed.value(); - } - if (toml_RobotInitDist) { - pRobotInitDist = toml_RobotInitDist.value(); - } - if (toml_RobotPosHistorySize) { - pRobotPosHistorySize = toml_RobotPosHistorySize.value(); - } - if (toml_TimeStep) { - pTimeStep = toml_TimeStep.value(); - } +void Parameters::ValidateParameters() { + std::vector errors; + + // Validate positive integers + if (!ValidatePositiveInt(pNumRobots, "NumRobots")) + errors.push_back("NumRobots"); + if (!ValidatePositiveInt(pWorldMapSize, "WorldMapSize")) + errors.push_back("WorldMapSize"); + if (!ValidatePositiveInt(pRobotMapSize, "RobotMapSize")) + errors.push_back("RobotMapSize"); + if (!ValidatePositiveInt(pLocalMapSize, "LocalMapSize")) + errors.push_back("LocalMapSize"); + if (!ValidatePositiveInt(pSensorSize, "SensorSize")) + errors.push_back("SensorSize"); + if (!ValidatePositiveInt(pEpisodeSteps, "EpisodeSteps")) + errors.push_back("EpisodeSteps"); + + // Validate positive doubles + if (!ValidatePositiveDouble(pResolution, "Resolution")) + errors.push_back("Resolution"); + if (!ValidatePositiveDouble(pCommunicationRange, "CommunicationRange")) + errors.push_back("CommunicationRange"); + if (!ValidatePositiveDouble(pMaxRobotSpeed, "MaxRobotSpeed")) + errors.push_back("MaxRobotSpeed"); + if (!ValidatePositiveDouble(pTimeStep, "TimeStep")) + errors.push_back("TimeStep"); + + // Validate ranges + if (!ValidateRange(pPlotScale, 0.1, 10.0, "PlotScale")) + errors.push_back("PlotScale"); + + // Validate sensor size is even + if (pSensorSize % 2 != 0) { + std::cerr << "SensorSize must be even, got: " << pSensorSize << std::endl; + errors.push_back("SensorSize"); } - if (toml_RobotModel["AddNoise"]) { - auto toml_AddNoisePositions = - toml_RobotModel["AddNoise"]["AddNoisePositions"].value(); - auto toml_PositionsNoiseSigmaMin = - toml_RobotModel["AddNoise"]["PositionsNoiseSigmaMin"].value(); - auto toml_PositionsNoiseSigmaMax = - toml_RobotModel["AddNoise"]["PositionsNoiseSigmaMax"].value(); - if (toml_AddNoisePositions) { - pAddNoisePositions = toml_AddNoisePositions.value(); - } - if (toml_PositionsNoiseSigmaMin) { - pPositionsNoiseSigmaMin = toml_PositionsNoiseSigmaMin.value(); - } - if (toml_PositionsNoiseSigmaMax) { - pPositionsNoiseSigmaMax = toml_PositionsNoiseSigmaMax.value(); - } + // Validate sigma ranges + if (pMinSigma >= pMaxSigma) { + std::cerr << "MinSigma (" << pMinSigma << ") must be less than MaxSigma (" + << pMaxSigma << ")" << std::endl; + errors.push_back("Sigma range"); } - auto toml_Algorithm = toml_config["Algorithm"]; - - if (toml_Algorithm) { - auto toml_EpisodeSteps = toml_Algorithm["EpisodeSteps"].value(); - if (toml_EpisodeSteps) { - pEpisodeSteps = toml_EpisodeSteps.value(); - } - if (toml_Algorithm["CheckOscillations"].value()) { - pCheckOscillations = - toml_Algorithm["CheckOscillations"].value().value(); - } + // Validate peak ranges + if (pMinPeak >= pMaxPeak) { + std::cerr << "MinPeak (" << pMinPeak << ") must be less than MaxPeak (" + << pMaxPeak << ")" << std::endl; + errors.push_back("Peak range"); + } - auto toml_LloydMaxIterations = - toml_Algorithm["Global-CVT"]["LloydMaxIterations"].value(); - auto toml_LloydNumTries = - toml_Algorithm["Global-CVT"]["LloydNumTries"].value(); - if (toml_LloydMaxIterations) { - pLloydMaxIterations = toml_LloydMaxIterations.value(); - } - if (toml_LloydNumTries) { - pLloydNumTries = toml_LloydNumTries.value(); - } + // Validate noise sigma ranges + if (pAddNoisePositions && + pPositionsNoiseSigmaMin >= pPositionsNoiseSigmaMax) { + std::cerr << "PositionsNoiseSigmaMin (" << pPositionsNoiseSigmaMin + << ") must be less than PositionsNoiseSigmaMax (" + << pPositionsNoiseSigmaMax << ")" << std::endl; + errors.push_back("Noise sigma range"); + } - auto toml_NumFrontiers = - toml_Algorithm["Exploration"]["NumFrontiers"].value(); - if (toml_NumFrontiers) { - pNumFrontiers = toml_NumFrontiers.value(); - } + // Validate speed constraint + double max_displacement_per_step = pMaxRobotSpeed * pTimeStep / pResolution; + if (max_displacement_per_step >= pSensorSize / 2.0) { + std::cerr << "Robot speed constraint violated: MaxRobotSpeed * TimeStep / " + "Resolution (" + << max_displacement_per_step << ") must be < SensorSize/2 (" + << pSensorSize / 2.0 << ")" << std::endl; + errors.push_back("Speed constraint"); } } void Parameters::PrintParameters() const { + std::cout << "\n=== CoverageControl Parameters ===" << std::endl; + + std::cout << "\n[Environment]" << std::endl; std::cout << "NumRobots: " << pNumRobots << std::endl; + std::cout << "NumGaussianFeatures: " << pNumGaussianFeatures << std::endl; std::cout << "NumPolygons: " << pNumPolygons << std::endl; std::cout << "MaxVertices: " << pMaxVertices << std::endl; std::cout << "PolygonRadius: " << pPolygonRadius << std::endl; + std::cout << "\n[IO]" << std::endl; std::cout << "PlotScale: " << pPlotScale << std::endl; + std::cout << "\n[Maps]" << std::endl; std::cout << "Resolution: " << pResolution << std::endl; std::cout << "WorldMapSize: " << pWorldMapSize << std::endl; std::cout << "RobotMapSize: " << pRobotMapSize << std::endl; std::cout << "LocalMapSize: " << pLocalMapSize << std::endl; - std::cout << "UpdateRobotMap: " << pUpdateRobotMap << std::endl; std::cout << "UpdateSensorView: " << pUpdateSensorView << std::endl; std::cout << "UpdateExplorationMap: " << pUpdateExplorationMap << std::endl; std::cout << "UpdateSystemMap: " << pUpdateSystemMap << std::endl; - std::cout << "NumGaussianFeatures: " << pNumGaussianFeatures << std::endl; + std::cout << "\n[IDF Parameters]" << std::endl; std::cout << "TruncationBND: " << pTruncationBND << std::endl; std::cout << "Norm: " << pNorm << std::endl; std::cout << "MinSigma: " << pMinSigma << std::endl; std::cout << "MaxSigma: " << pMaxSigma << std::endl; std::cout << "MinPeak: " << pMinPeak << std::endl; std::cout << "MaxPeak: " << pMaxPeak << std::endl; - - std::cout << "pNumPolygons: " << pNumPolygons << std::endl; - std::cout << "pMaxVertices: " << pMaxVertices << std::endl; - std::cout << "pPolygonRadius: " << pPolygonRadius << std::endl; - std::cout << "UnknownImportance: " << pUnknownImportance << std::endl; std::cout << "RobotMapUseUnknownImportance: " << pRobotMapUseUnknownImportance << std::endl; + std::cout << "\n[Robot Model]" << std::endl; std::cout << "SensorSize: " << pSensorSize << std::endl; std::cout << "CommunicationRange: " << pCommunicationRange << std::endl; std::cout << "MaxRobotSpeed: " << pMaxRobotSpeed << std::endl; @@ -306,14 +316,21 @@ void Parameters::PrintParameters() const { std::cout << "RobotPosHistorySize: " << pRobotPosHistorySize << std::endl; std::cout << "TimeStep: " << pTimeStep << std::endl; + std::cout << "\n[Noise Parameters]" << std::endl; std::cout << "AddNoisePositions: " << pAddNoisePositions << std::endl; - std::cout << "PositionsNoiseSigmaMin: " << pPositionsNoiseSigmaMin << std::endl; - std::cout << "PositionsNoiseSigmaMax: " << pPositionsNoiseSigmaMax << std::endl; + std::cout << "PositionsNoiseSigmaMin: " << pPositionsNoiseSigmaMin + << std::endl; + std::cout << "PositionsNoiseSigmaMax: " << pPositionsNoiseSigmaMax + << std::endl; + std::cout << "\n[Algorithm]" << std::endl; std::cout << "EpisodeSteps: " << pEpisodeSteps << std::endl; std::cout << "CheckOscillations: " << pCheckOscillations << std::endl; std::cout << "LloydMaxIterations: " << pLloydMaxIterations << std::endl; std::cout << "LloydNumTries: " << pLloydNumTries << std::endl; std::cout << "NumFrontiers: " << pNumFrontiers << std::endl; + + std::cout << "\n=================================" << std::endl; } -} /* namespace CoverageControl */ + +} // namespace CoverageControl diff --git a/params/coverage_control_params.toml b/params/coverage_control_params.toml index 0d961b17..917d8d67 100644 --- a/params/coverage_control_params.toml +++ b/params/coverage_control_params.toml @@ -1,11 +1,6 @@ # Specify parameters for coverage control in toml format NumRobots = 32 -NumFeatures = 32 - -NumPolygons = 0 -MaxVertices = 10 -PolygonRadius = 64 [IO] # Determines the quality of the plot and videos @@ -27,13 +22,12 @@ WorldMapSize = 1024 RobotMapSize = 1024 # Local map is used for computing mass. Actual area would be LocalMapSize * Resolution -# Should be greater than CommunicationRange so that they can form different channels of the same image. LocalMapSize = 256 # Map update settings [Environment.Maps.UpdateSettings] -UpdateRobotMap = true # Set UpdateRobotMap to false for centralized known world +UpdateRobotMap = true UpdateSensorView = true UpdateExplorationMap = false UpdateSystemMap = true @@ -41,6 +35,8 @@ UpdateSystemMap = true # Parameters for the IDF [Environment.IDF] +NumGaussianFeatures = 32 + # Bivariate Normal Distribution truncated after TruncationBND * sigma # Makes it more realistic and reduces the number of erfc evaluations TruncationBND = 2 @@ -54,6 +50,11 @@ MaxSigma = 60 MinPeak = 6 MaxPeak = 10 +# Polygonal features +NumPolygons = 0 +MaxVertices = 10 +PolygonRadius = 64 + # Add importance to unknown regions UnknownImportance = 0.5 # fraction of the largest imaportance of a grid cell RobotMapUseUnknownImportance = false @@ -80,7 +81,8 @@ TimeStep = 0.2 # in seconds # Add noise to the robot's position and sensor readings AddNoisePositions = false -PositionsNoiseSigma = 0.1 +PositionsNoiseSigmaMin = 1.0 +PositionsNoiseSigmaMax = 2.0 # Parameters for the standard baseline coverage control algorithms [Algorithm] diff --git a/params/data_params.toml b/params/data_params.toml index feedabb4..93dc0da0 100644 --- a/params/data_params.toml +++ b/params/data_params.toml @@ -22,6 +22,9 @@ ConvergedDataRatio = 0.02 # Resizing of maps and Sparsification of tensors are triggered every TriggerPostProcessing dataset # This should be set based on RAM resources available on the system TriggerPostProcessing = 100 +SaveObjective = false +TimeStep = 5 # Overrides TimeStep in coverge_control_params.toml +SaveRobotWorldLocalMaps = false CNNMapSize = 32 SaveAsSparseQ = true diff --git a/params/learning_params.toml b/params/learning_params.toml index 7e976ab7..08de8fca 100644 --- a/params/learning_params.toml +++ b/params/learning_params.toml @@ -7,6 +7,7 @@ NumWorkers = 4 # Similarly, for the optimizer [LPACModel] Dir = "${CoverageControl_ws}/lpac/models/" +# PreTrainedModel = "model.pt" [CNNModel] Dir = "${CoverageControl_ws}/lpac/models/" # Absolute location From d1585e43d966f38dd31a4e1dd40b18fe38df674e Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Thu, 5 Jun 2025 01:54:26 -0400 Subject: [PATCH 30/35] fix eval --- python/evaluators/eval.py | 63 +++++++++++++++++++++------------------ 1 file changed, 34 insertions(+), 29 deletions(-) diff --git a/python/evaluators/eval.py b/python/evaluators/eval.py index 8295aa8c..e9759947 100644 --- a/python/evaluators/eval.py +++ b/python/evaluators/eval.py @@ -66,9 +66,24 @@ def __init__(self, in_config): TimeElapsedColumn(), ] + def get_normalized_objective_value(self, env, initial_objective_value): + """ + Returns the normalized objective value of the environment + :param env: The environment object + :param initial_objective_value: The initial objective value of the environment + :return: The normalized objective value + """ + objective_value = env.GetObjectiveValue() + return objective_value / initial_objective_value + def evaluate(self, save=True): total_samples = self.num_steps // self.every_num_steps + if self.every_num_steps != 1: + total_samples += 1 cost_data = np.zeros((self.num_controllers, self.num_envs, total_samples)) + progress_update_rate = self.every_num_steps + if self.every_num_steps < 10: + progress_update_rate = self.every_num_steps * (10 // self.every_num_steps) with Progress(*self.columns, expand=True) as progress: task = progress.add_task( @@ -97,6 +112,7 @@ def evaluate(self, save=True): for controller_id in range(self.num_controllers): sample_count = 0 + converged = False env = CoverageSystem(self.cc_params, world_idf, robot_init_pos) if self.controllers_configs[controller_id]["Type"] == "Learning": @@ -107,41 +123,30 @@ def evaluate(self, save=True): self.controllers_configs[controller_id], self.cc_params, env ) initial_objective_value = env.GetObjectiveValue() - cost_data[controller_id, env_count, sample_count] = ( - env.GetObjectiveValue() / initial_objective_value - ) - sample_count = sample_count + 1 + cost_data[controller_id, env_count, sample_count] = self.get_normalized_objective_value(env, initial_objective_value) + sample_count += 1 for step_count in range(1, self.num_steps): converged = controller.step(env) - if step_count % self.every_num_steps == 0: - objective_value = env.GetObjectiveValue() - normalized_objective_value = ( - objective_value / initial_objective_value - ) - cost_data[controller_id, env_count, sample_count] = ( - normalized_objective_value - ) - sample_count = sample_count + 1 - - if converged: - cost_data[controller_id, env_count, sample_count:] = ( - normalized_objective_value + if (step_count + 1) % self.every_num_steps == 0: + normalized_objective_value = self.get_normalized_objective_value(env, initial_objective_value) + cost_data[controller_id, env_count, sample_count] = normalized_objective_value + sample_count += 1 + if (step_count + 1) % progress_update_rate == 0: + info = f"Controller {controller_id}/{self.num_controllers}: {controller.name} " + progress.update( + task, + info=info, + step_count=step_count+1, + obj=normalized_objective_value, ) - step_count = self.num_steps - sample_count = total_samples - - info = f"Controller {controller_id}/{self.num_controllers}: {controller.name} " - - progress.update( - task, - info=info, - step_count=step_count, - obj=normalized_objective_value, - ) - progress.refresh() + progress.refresh() if converged: + cost_data[controller_id, env_count, sample_count:] = self.get_normalized_objective_value(env, initial_objective_value) + step_count = self.num_steps + sample_count = total_samples + break if controller_id == self.num_controllers - 1: From 0d05a95c5631b099ee0e419e332c545a4e24e9b5 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Mon, 9 Jun 2025 11:00:20 -0400 Subject: [PATCH 31/35] Add const GetObjectiveValue --- cppsrc/core/include/CoverageControl/coverage_system.h | 10 +++++++++- cppsrc/core/src/coverage_system.cpp | 4 ++-- cppsrc/python_bindings/core_binds.h | 1 + 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h index c2f2b7e7..42c05006 100644 --- a/cppsrc/core/include/CoverageControl/coverage_system.h +++ b/cppsrc/core/include/CoverageControl/coverage_system.h @@ -438,7 +438,8 @@ class CoverageSystem { return weighted_exploration_ratio; } - PointVector GetRelativePositonsNeighbors(size_t const robot_id); + PointVector GetRelativePositonsNeighbors (size_t const robot_id) const; + std::vector GetNeighborIDs(size_t const robot_id) const { return neighbor_ids_[robot_id]; } @@ -513,6 +514,13 @@ class CoverageSystem { return communication_maps; } + auto GetObjectiveValueConst() const { + auto voronoi = Voronoi(robot_global_positions_, GetWorldMap(), + Point2(params_.pWorldMapSize, params_.pWorldMapSize), + params_.pResolution); + return voronoi.GetSumIDFSiteDistSqr(); + } + auto GetObjectiveValue() { ComputeVoronoiCells(); return voronoi_.GetSumIDFSiteDistSqr(); diff --git a/cppsrc/core/src/coverage_system.cpp b/cppsrc/core/src/coverage_system.cpp index 8c529286..8885e4ee 100644 --- a/cppsrc/core/src/coverage_system.cpp +++ b/cppsrc/core/src/coverage_system.cpp @@ -679,8 +679,8 @@ void CoverageSystem::PlotRobotCommunicationMaps(std::string const &dir_name, plotter_y.PlotMap(robot_communication_maps.second); } -PointVector CoverageSystem::GetRelativePositonsNeighbors( - size_t const robot_id) { +PointVector CoverageSystem::GetRelativePositonsNeighbors ( + size_t const robot_id) const { return relative_positions_neighbors_[robot_id]; } diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index 2da02bd6..7eb17e7f 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -350,6 +350,7 @@ void pyCoverageControl_core_coverage_system(py::module &m) { .def("GetSystemMap", &CoverageSystem::GetSystemMap, py::return_value_policy::reference_internal) .def("GetObjectiveValue", &CoverageSystem::GetObjectiveValue) + .def("GetObjectiveValueConst", &CoverageSystem::GetObjectiveValueConst) .def("GetNormalizationFactor", &CoverageSystem::GetNormalizationFactor) .def("PlotSystemMap", py::overload_cast( &CoverageSystem::PlotSystemMap, py::const_)) From f419e91d0b8e37b98feca6534d7302575bcf8db9 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 10 Jun 2025 14:47:59 -0400 Subject: [PATCH 32/35] add datestamp --- utils/docker/build_all_images.sh | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/utils/docker/build_all_images.sh b/utils/docker/build_all_images.sh index 6ade67dc..b6952898 100644 --- a/utils/docker/build_all_images.sh +++ b/utils/docker/build_all_images.sh @@ -10,11 +10,15 @@ if [ "$#" -ne 1 ]; then print_usage fi +DATESTAMP=$(date -u +'%Y%m%dZ') +echo "DATESTAMP: ${DATESTAMP}" + + build_image() { echo "Building image $2" TAG_NAME=$2 ALL_BUILD_ARGS="--no-cache --build-arg CUDA_VERSION=${CUDA_VERSION} --build-arg PYTHON_VERSION=${PYTHON_VERSION} --build-arg PYTORCH_VERSION=${PYTORCH_VERSION} --build-arg IMAGE_TYPE=${IMAGE_TYPE} --build-arg UBUNTU_VERSION=${UBUNTU_VERSION} --build-arg ROS_DISTRO=${ROS_DISTRO}" - DOCKER_BUILDKIT=1 docker buildx build --push ${ALL_BUILD_ARGS} $3 -t ${1}:${TAG_NAME} . + DOCKER_BUILDKIT=1 docker buildx build --push ${ALL_BUILD_ARGS} $3 -t ${1}:${TAG_NAME}-${DATESTAMP} . if [ $? -ne 0 ]; then echo "Failed to build image $2" exit 1 @@ -41,6 +45,7 @@ build_image $1 $TAG_NAME "--target ros2" TAG_NAME=jammy-torch${PYTORCH_VERSION} build_image $1 $TAG_NAME "--target base" +IMAGE_TYPE="arm64" TAG_NAME=arm64-jammy-torch${PYTORCH_VERSION}-humble build_image $1 $TAG_NAME "--platform linux/arm64 --target ros2" @@ -61,8 +66,9 @@ IMAGE_TYPE="cpu" TAG_NAME=noble-torch${PYTORCH_VERSION}-jazzy build_image $1 $TAG_NAME "--target ros2" -TAG_NAME=noble-torch${PYTORCH_VERSION} -build_image $1 $TAG_NAME "--target base -t ${1}:latest" +# TAG_NAME=noble-torch${PYTORCH_VERSION} +# build_image $1 $TAG_NAME "--target base -t ${1}:latest" +IMAGE_TYPE="arm64" TAG_NAME=arm64-noble-torch${PYTORCH_VERSION}-jazzy build_image $1 $TAG_NAME "--platform linux/arm64 --target ros2" From 272aae91b807d0ae81d2389be48405d317af1207 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Sun, 15 Jun 2025 18:45:52 -0400 Subject: [PATCH 33/35] add support for goals --- .../include/CoverageControl/coverage_system.h | 2 + cppsrc/core/src/coverage_system.cpp | 19 ++ cppsrc/python_bindings/core_binds.h | 3 + params/learning_params.toml | 1 + .../algorithms/controllers.py | 9 +- python/coverage_control/io_utils.py | 209 ++++++++++++------ .../nn/data_loaders/data_loader_utils.py | 34 +-- .../nn/data_loaders/loaders.py | 8 +- python/data_generation/data_generation.py | 19 ++ python/training/train_lpac.py | 10 +- 10 files changed, 226 insertions(+), 88 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h index 42c05006..92be5710 100644 --- a/cppsrc/core/include/CoverageControl/coverage_system.h +++ b/cppsrc/core/include/CoverageControl/coverage_system.h @@ -334,6 +334,8 @@ class CoverageSystem { */ bool StepRobotsToGoals(PointVector const &goals, PointVector &actions); + bool StepRobotsToRelativeGoals(PointVector const &goals); + void ClearRobotMaps() { for (size_t i = 0; i < num_robots_; ++i) { robots_[i].ClearRobotMap(); diff --git a/cppsrc/core/src/coverage_system.cpp b/cppsrc/core/src/coverage_system.cpp index 8885e4ee..c713f44c 100644 --- a/cppsrc/core/src/coverage_system.cpp +++ b/cppsrc/core/src/coverage_system.cpp @@ -373,6 +373,25 @@ void CoverageSystem::ComputeVoronoiCells() { voronoi_cells_ = voronoi_.GetVoronoiCells(); } +bool CoverageSystem::StepRobotsToRelativeGoals(PointVector const &goals) { + for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { + Point2 action = Point2(0, 0); + Point2 diff = goals[iRobot]; + double dist = diff.norm(); + double speed = dist / params_.pTimeStep; + speed = std::min(params_.pMaxRobotSpeed, speed); + Point2 direction(diff); + direction.normalize(); + action = speed * direction; + if (robots_[iRobot].StepControl(direction, speed)) { + std::cerr << "Control incorrect\n"; + return 1; + } + } + PostStepCommands(); + return 0; +} + bool CoverageSystem::StepRobotToGoal(int const robot_id, Point2 const &goal, double const speed_factor) { Point2 curr_pos = robots_[robot_id].GetGlobalCurrentPosition(); diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index 7eb17e7f..b5ddf817 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -179,7 +179,9 @@ void pyCoverageControl_core(py::module &m) { py::class_(m, "NearOptimalCVT") .def(py::init()) + .def(py::init()) .def(py::init()) + .def(py::init()) .def("ComputeActions", &NearOptimalCVT::ComputeActions) .def("GetActions", &NearOptimalCVT::GetActions) .def("IsConverged", &NearOptimalCVT::IsConverged) @@ -293,6 +295,7 @@ void pyCoverageControl_core_coverage_system(py::module &m) { .def("StepControl", &CoverageSystem::StepControl) .def("StepAction", &CoverageSystem::StepAction) .def("StepActions", &CoverageSystem::StepActions) + .def("StepRobotsToRelativeGoals", &CoverageSystem::StepRobotsToRelativeGoals) .def("SetLocalRobotPositions", &CoverageSystem::SetLocalRobotPositions) .def("SetLocalRobotPosition", &CoverageSystem::SetLocalRobotPosition) .def("SetGlobalRobotPosition", &CoverageSystem::SetGlobalRobotPosition) diff --git a/params/learning_params.toml b/params/learning_params.toml index 08de8fca..a1892252 100644 --- a/params/learning_params.toml +++ b/params/learning_params.toml @@ -16,6 +16,7 @@ Optimizer = "optimizer.pt" [ModelConfig] UseCommMaps = true +TargetType = "actions" # or goals [GNNBackBone] InputDim = 7 diff --git a/python/coverage_control/algorithms/controllers.py b/python/coverage_control/algorithms/controllers.py index 0e67cfe2..887f3246 100644 --- a/python/coverage_control/algorithms/controllers.py +++ b/python/coverage_control/algorithms/controllers.py @@ -130,6 +130,10 @@ def __init__(self, config: dict, params: Parameters, env: CoverageSystem): self.model = cc_nn.LPAC(self.learning_params).to(self.device) self.model.load_model(IOUtils.sanitize_path(self.config["ModelStateDict"])) + self.target_type = "actions" + if "TargetType" in self.learning_params["ModelConfig"]: + self.target_type = self.learning_params["ModelConfig"]["TargetType"] + self.actions_mean = self.model.actions_mean.to(self.device) self.actions_std = self.model.actions_std.to(self.device) self.model = self.model.to(self.device) @@ -166,7 +170,10 @@ def step(self, env): Objective value and convergence flag """ actions, converged = self.get_actions(env) - env.StepActions(actions) + if self.target_type == "actions": + env.StepActions(actions) + else: + env.StepRobotsToRelativeGoals(actions) # Check if actions are all zeros (1e-12) return converged diff --git a/python/coverage_control/io_utils.py b/python/coverage_control/io_utils.py index c5fe1b67..19b1dd67 100644 --- a/python/coverage_control/io_utils.py +++ b/python/coverage_control/io_utils.py @@ -28,105 +28,184 @@ import os import sys -import pathlib - +from pathlib import Path +from typing import Union, Dict, Any import torch import yaml -if sys.version_info[1] < 11: - import tomli as tomllib -else: +# Handle tomllib import based on Python version +if sys.version_info >= (3, 11): import tomllib +else: + try: + import tomli as tomllib + except ImportError: + tomllib = None + ## @ingroup python_api class IOUtils: """ - Class provides the following utility functions: - - load_tensor - - load_yaml - - load_toml + Class provides utility functions for loading data from various file formats. + + Methods: + - load_tensor: Load PyTorch tensors from files + - load_yaml: Load YAML configuration files + - load_toml: Load TOML configuration files + - sanitize_path: Sanitize and normalize file paths """ @staticmethod def sanitize_path(path_str: str) -> str: """ - Function to sanitize a path string + Sanitize and normalize a path string. + + Args: + path_str (str): Path string to sanitize + + Returns: + Path: Normalized pathlib.Path object """ return os.path.normpath(os.path.expanduser(os.path.expandvars(path_str))) @staticmethod - def load_tensor(path: (str, pathlib.Path)) -> torch.tensor: + def load_tensor(path: Union[str, Path]) -> torch.Tensor: """ - Function to load a tensor from a file - Can load tensors from jit script format files - + Load a PyTorch tensor from a file. + + Supports loading tensors from both regular PyTorch saves and JIT script modules. + Args: - path (str): Path to the file - + path (Union[str, Path]): Path to the tensor file + Returns: - tensor: The loaded tensor - None: If the file does not exist - + torch.Tensor: The loaded tensor + Raises: FileNotFoundError: If the file does not exist + RuntimeError: If the file cannot be loaded or contains no tensor data """ - - if isinstance(path, pathlib.Path): - path = str(path) - # Throw error if path does not exist - path = IOUtils.sanitize_path(path) - - if not os.path.exists(path): - raise FileNotFoundError(f"IOUtils::load_tensor: File not found: {path}") - # Load data - data = torch.load(path, weights_only=True) - # Extract tensor if data is in jit script format - - if isinstance(data, torch.jit.ScriptModule): - tensor = list(data.parameters())[0] + # Convert to Path object and sanitize + if isinstance(path, str): + path = IOUtils.sanitize_path(path) + elif isinstance(path, Path): + path = IOUtils.sanitize_path(str(path)) else: - tensor = data - - return tensor - + raise TypeError(f"Path must be str or Path, got {type(path)}") + path = Path(path) + + # Check if file exists + if not path.is_file(): + raise FileNotFoundError(f"IOUtils::load_tensor: File not found: {path}") + + try: + # Load data with weights_only for security + data = torch.load(path, weights_only=True, map_location='cpu') + + # Extract tensor based on data type + if isinstance(data, torch.jit.ScriptModule): + # Get the first parameter from JIT script module + params = list(data.parameters()) + if not params: + raise RuntimeError(f"No parameters found in JIT script module: {path}") + tensor = params[0] + elif isinstance(data, torch.Tensor): + tensor = data + elif isinstance(data, dict) and 'tensor' in data: + # Handle case where tensor is stored in a dictionary + tensor = data['tensor'] + else: + raise RuntimeError(f"Unsupported data type in file: {type(data)}") + + return tensor + + except Exception as e: + if isinstance(e, (FileNotFoundError, RuntimeError)): + raise + raise RuntimeError(f"Failed to load tensor from {path}: {str(e)}") + @staticmethod - def load_yaml(path: str) -> dict: + def load_yaml(path: Union[str, Path]) -> Dict[str, Any]: """ - Function to load a yaml file - + Load data from a YAML file. + Args: - path (str): Path to the file - + path (Union[str, Path]): Path to the YAML file + Returns: - data: The loaded data - + Dict[str, Any]: The loaded data as a dictionary + Raises: FileNotFoundError: If the file does not exist + yaml.YAMLError: If the YAML file is malformed """ - - path = IOUtils.sanitize_path(path) - # Throw error if path does not exist - - if not os.path.exists(path): - raise FileNotFoundError(f"IOUtils::load_yaml File not found: {path}") - # Load data - with open(path, "r") as f: - data = yaml.load(f, Loader=yaml.SafeLoader) - - return data - + # Convert to Path object and sanitize + if isinstance(path, str): + path = IOUtils.sanitize_path(path) + elif isinstance(path, Path): + path = IOUtils.sanitize_path(str(path)) + else: + raise TypeError(f"Path must be str or Path, got {type(path)}") + + path = Path(path) + # Check if file exists + if not path.is_file(): + raise FileNotFoundError(f"IOUtils::load_yaml: File not found: {path}") + + try: + with open(path, "r", encoding="utf-8") as f: + data = yaml.safe_load(f) + return data if data is not None else {} + + except yaml.YAMLError as e: + raise yaml.YAMLError(f"Failed to parse YAML file {path}: {str(e)}") + except Exception as e: + raise RuntimeError(f"Failed to load YAML file {path}: {str(e)}") + @staticmethod - def load_toml(path: str) -> dict: # Throw error if path does not exist + def load_toml(path: Union[str, Path]) -> Dict[str, Any]: """ - Function to load a toml file + Load data from a TOML file. + + Args: + path (Union[str, Path]): Path to the TOML file + + Returns: + Dict[str, Any]: The loaded data as a dictionary + + Raises: + FileNotFoundError: If the file does not exist + ImportError: If tomllib/tomli is not available + tomllib.TOMLDecodeError: If the TOML file is malformed """ - path = IOUtils.sanitize_path(path) - - if not os.path.exists(path): + # Check if tomllib is available + if tomllib is None: + raise ImportError( + "TOML support requires 'tomli' package for Python < 3.11. " + "Install with: pip install tomli" + ) + + # Convert to Path object and sanitize + if isinstance(path, str): + path = IOUtils.sanitize_path(path) + elif isinstance(path, Path): + path = IOUtils.sanitize_path(str(path)) + else: + raise TypeError(f"Path must be str or Path, got {type(path)}") + + path = Path(path) + # Check if file exists + if not path.is_file(): raise FileNotFoundError(f"IOUtils::load_toml: File not found: {path}") - # Load data - with open(path, "rb") as f: - data = tomllib.load(f) + + try: + with open(path, "rb") as f: + data = tomllib.load(f) + return data + + except tomllib.TOMLDecodeError as e: + raise tomllib.TOMLDecodeError(f"Failed to parse TOML file {path}: {str(e)}") + except Exception as e: + raise RuntimeError(f"Failed to load TOML file {path}: {str(e)}") - return data diff --git a/python/coverage_control/nn/data_loaders/data_loader_utils.py b/python/coverage_control/nn/data_loaders/data_loader_utils.py index 0d27787e..80977f47 100644 --- a/python/coverage_control/nn/data_loaders/data_loader_utils.py +++ b/python/coverage_control/nn/data_loaders/data_loader_utils.py @@ -19,6 +19,7 @@ # You should have received a copy of the GNU General Public License along with # CoverageControl library. If not, see . +from pathlib import Path import torch import torch_geometric from coverage_control import IOUtils @@ -108,29 +109,32 @@ def load_features( ) @staticmethod - def load_actions(path: str) -> tuple[torch.tensor, torch.tensor, torch.tensor]: + def load_targets(path: str, target_type: str) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: """ - Function to load normalized actions + Function to load normalized targets - The actions are stored as tensors in the following format: - - {path}/normalized_actions.pt: Normalized actions - - {path}/../actions_mean.pt: Mean of the actions - - {path}/../actions_std.pt: Standard deviation of the actions + The targets = actions|goals are stored as tensors in the following format: + - {path}/normalized_targets.pt: Normalized targets + - {path}/../targets_mean.pt: Mean of the targets + - {path}/../targets_std.pt: Standard deviation of the targets Args: - path (str): Path to the directory containing the actions + path (str): Path to the directory containing the targets Returns: - actions: The loaded actions - actions_mean: Mean of the actions - actions_std: Standard deviation of the actions + targets: The loaded targets + targets_mean: Mean of the targets + targets_std: Standard deviation of the targets """ - actions = IOUtils.load_tensor(f"{path}/normalized_actions.pt") - actions_mean = IOUtils.load_tensor(f"{path}/../actions_mean.pt") - actions_std = IOUtils.load_tensor(f"{path}/../actions_std.pt") - - return actions, actions_mean, actions_std + path_ = Path(path) + if target_type not in ["actions", "goals"]: + raise ValueError(f"Valid target_type actions or goals. Given: {target_type}") + targets = IOUtils.load_tensor(path_ / f"normalized_{target_type}.pt") + targets_mean = IOUtils.load_tensor(path_.parent / f"{target_type}_mean.pt") + targets_std = IOUtils.load_tensor(path_.parent / f"{target_type}_std.pt") + + return targets, targets_mean, targets_std @staticmethod def load_robot_positions(path: str) -> torch.tensor: diff --git a/python/coverage_control/nn/data_loaders/loaders.py b/python/coverage_control/nn/data_loaders/loaders.py index a515ae16..e7e51c63 100644 --- a/python/coverage_control/nn/data_loaders/loaders.py +++ b/python/coverage_control/nn/data_loaders/loaders.py @@ -89,7 +89,7 @@ def load_data(self): # self.targets, self.targets_mean, self.targets_std = DataLoaderUtils.load_features(f"{self.data_dir}/{self.stage}", self.output_dim) self.targets, self.targets_mean, self.targets_std = ( - DataLoaderUtils.load_actions(f"{self.data_dir}/{self.stage}") + DataLoaderUtils.load_targets(f"{self.data_dir}/{self.stage}") ) self.targets = self.targets.view(-1, self.targets.shape[2]) @@ -99,7 +99,7 @@ class CNNGNNDataset(Dataset): Dataset for hybrid CNN-GNN training """ - def __init__(self, data_dir, stage, use_comm_map, world_size): + def __init__(self, data_dir, stage, use_comm_map, world_size, target_type="actions"): super().__init__(None, None, None, None) self.stage = stage @@ -108,7 +108,7 @@ def __init__(self, data_dir, stage, use_comm_map, world_size): self.dataset_size = self.maps.shape[0] self.targets, self.targets_mean, self.targets_std = ( - DataLoaderUtils.load_actions(f"{data_dir}/{stage}") + DataLoaderUtils.load_targets(f"{data_dir}/{stage}", target_type) ) self.edge_weights = DataLoaderUtils.load_edge_weights(f"{data_dir}/{stage}") @@ -155,7 +155,7 @@ def __init__(self, data_dir, stage, output_dim): self.features = DataLoaderUtils.load_features(f"{data_dir}/{stage}", output_dim) self.dataset_size = self.features[0].shape[0] self.targets, self.targets_mean, self.targets_std = ( - DataLoaderUtils.load_actions(f"{data_dir}/{stage}") + DataLoaderUtils.load_targets(f"{data_dir}/{stage}") ) self.edge_weights = DataLoaderUtils.load_edge_weights(f"{data_dir}/{stage}") diff --git a/python/data_generation/data_generation.py b/python/data_generation/data_generation.py index d7027f7f..4c943925 100644 --- a/python/data_generation/data_generation.py +++ b/python/data_generation/data_generation.py @@ -57,6 +57,7 @@ from coverage_control import CoverageEnvUtils from coverage_control.algorithms import ClairvoyantCVT from coverage_control.algorithms import CentralizedCVT +from coverage_control.algorithms import NearOptimalCVT # @ingroup python_api @@ -146,6 +147,7 @@ def __init__(self, args): # Initialize tensors self.actions = torch.zeros((self.num_dataset, self.num_robots, 2)) + self.goals = torch.zeros((self.num_dataset, self.num_robots, 2)) self.robot_positions = torch.zeros( (self.num_dataset, self.num_robots, 2)) self.raw_local_maps = torch.zeros( @@ -246,6 +248,9 @@ def run_data_generation(self): if self.algorithm == "CentralizedCVT": self.alg = CentralizedCVT( self.env_params, self.num_robots, self.env, self.force_no_noise) + elif self.algorithm == "NearOptimalCVT": + self.alg = NearOptimalCVT( + self.env_params, self.num_robots, self.env, self.force_no_noise) else: self.alg = ClairvoyantCVT( self.env_params, self.num_robots, self.env, self.force_no_noise) @@ -285,8 +290,14 @@ def step_with_save(self): self.alg.ComputeActions() converged = self.alg.IsConverged() actions = self.alg.GetActions() + goals = self.alg.GetGoals() + robot_pos = self.env.GetRobotPositions() + for i, pos in enumerate(robot_pos): + goals[i] -= pos + count = self.dataset_count self.actions[count] = CoverageEnvUtils.to_tensor(actions) + self.goals[count] = CoverageEnvUtils.to_tensor(goals) self.robot_positions[count] = CoverageEnvUtils.get_robot_positions( self.env) self.coverage_features[count] = CoverageEnvUtils.get_voronoi_features( @@ -490,6 +501,7 @@ def save_dataset(self): # torch.save(range_val, self.dataset_dir / 'comm_maps_range.pt') self.save_tensor(self.actions, "actions.pt") + self.save_tensor(self.goals, "goals.pt") self.save_tensor(self.coverage_features, "coverage_features.pt") if self.save_objective: self.save_tensor(self.objectives, "objectives.pt") @@ -498,9 +510,15 @@ def save_dataset(self): normalized_actions, actions_mean, actions_std = self.normalize_tensor( self.actions, is_symmetric=True, zero_mean=True ) + normalized_goals, goals_mean, goals_std = self.normalize_tensor( + self.goals, is_symmetric=True, zero_mean=True + ) self.save_tensor(normalized_actions, "normalized_actions.pt") torch.save(actions_mean, self.dataset_dir_path / "actions_mean.pt") torch.save(actions_std, self.dataset_dir_path / "actions_std.pt") + self.save_tensor(normalized_goals, "normalized_goals.pt") + torch.save(goals_mean, self.dataset_dir_path / "goals_mean.pt") + torch.save(goals_std, self.dataset_dir_path / "goals_std.pt") if self.save_objective: normalize_objectives, objectives_mean, objectives_std = self.normalize_tensor( self.objectives @@ -542,6 +560,7 @@ def print_tensor_sizes(self, file=sys.stdout): # Set to two decimal places print("Tensor sizes:", file=file) print("Actions:", self.get_tensor_byte_size_mb(self.actions), file=file) + print("Goals:", self.get_tensor_byte_size_mb(self.goals), file=file) print( "Robot positions:", self.get_tensor_byte_size_mb(self.robot_positions), diff --git a/python/training/train_lpac.py b/python/training/train_lpac.py index 8c789d1e..328ac331 100644 --- a/python/training/train_lpac.py +++ b/python/training/train_lpac.py @@ -53,8 +53,12 @@ ) model.load_model(lpac_pretrained_model) -train_dataset = CNNGNNDataset(data_dir, "train", use_comm_map, world_size) -val_dataset = CNNGNNDataset(data_dir, "val", use_comm_map, world_size) +target_type = "actions" +if "TargetType" in config["ModelConfig"]: + target_type = config["ModelConfig"]["TargetType"] + +train_dataset = CNNGNNDataset(data_dir, "train", use_comm_map, world_size, target_type) +val_dataset = CNNGNNDataset(data_dir, "val", use_comm_map, world_size, target_type) # Check if buffer exists @@ -104,7 +108,7 @@ trainer.train() -test_dataset = CNNGNNDataset(data_dir, "test", use_comm_map, world_size) +test_dataset = CNNGNNDataset(data_dir, "test", use_comm_map, world_size, target_type) test_loader = torch_geometric.loader.DataLoader( test_dataset, batch_size=batch_size, shuffle=False, num_workers=num_workers ) From e95cc375474c48e09121e1d32c237953e3617705 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Tue, 17 Jun 2025 22:42:48 -0400 Subject: [PATCH 34/35] add pretty printing --- cppsrc/python_bindings/core_binds.h | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index b5ddf817..1564374e 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -50,6 +50,7 @@ #include #include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -81,7 +82,17 @@ void pyCoverageControl_core(py::module &m) { py::bind_vector>>(m, "DblVectorVector"); py::bind_vector>(m, "intVector"); - py::bind_vector(m, "PointVector"); + py::bind_vector(m, "PointVector") + .def("__str__", [](const PointVector& v) { + std::stringstream ss; + ss << "PointVector(["; + for (size_t i = 0; i < v.size(); ++i) { + if (i > 0) ss << ", "; + ss << "(" << v[i].x() << ", " << v[i].y() << ")"; + } + ss << "])"; + return ss.str(); + }); py::bind_vector>(m, "Point3Vector"); py::bind_vector>(m, "MapTypeVector"); From 535bf946f5df455579ebbcca503ef259e299f174 Mon Sep 17 00:00:00 2001 From: Saurav Agarwal Date: Wed, 18 Jun 2025 00:20:40 -0400 Subject: [PATCH 35/35] add const get robot positions and update printing --- .../include/CoverageControl/coverage_system.h | 13 ++++ cppsrc/python_bindings/core_binds.h | 63 ++++++++++++++----- 2 files changed, 60 insertions(+), 16 deletions(-) diff --git a/cppsrc/core/include/CoverageControl/coverage_system.h b/cppsrc/core/include/CoverageControl/coverage_system.h index 92be5710..f2380920 100644 --- a/cppsrc/core/include/CoverageControl/coverage_system.h +++ b/cppsrc/core/include/CoverageControl/coverage_system.h @@ -461,6 +461,19 @@ class CoverageSystem { return robot_global_positions_; } + PointVector GetRobotPositionsConst(bool force_no_noise = false) const { + PointVector robot_global_positions{num_robots_}; + for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { + robot_global_positions[iRobot] = + robots_[iRobot].GetGlobalCurrentPosition(); + if (params_.pAddNoisePositions and not force_no_noise) { + robot_global_positions[iRobot] = + robots_[iRobot].GetNoisyGlobalCurrentPosition(); + } + } + return robot_global_positions; + } + Point2 GetRobotPosition(int const robot_id, bool force_no_noise = false) const { if (params_.pAddNoisePositions and not force_no_noise) { diff --git a/cppsrc/python_bindings/core_binds.h b/cppsrc/python_bindings/core_binds.h index 1564374e..a6b33dae 100644 --- a/cppsrc/python_bindings/core_binds.h +++ b/cppsrc/python_bindings/core_binds.h @@ -49,8 +49,10 @@ #include #include -#include +#include +#include #include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -82,17 +84,37 @@ void pyCoverageControl_core(py::module &m) { py::bind_vector>>(m, "DblVectorVector"); py::bind_vector>(m, "intVector"); + auto formatPointVector = [](const PointVector &vec) { + std::stringstream ss; + double max_val = 0.0; + bool has_negative = false; + for (const auto &p : vec) { + max_val = std::max(max_val, std::abs(p.x())); + max_val = std::max(max_val, std::abs(p.y())); + if (p.x() < 0.0 || p.y() < 0.0) { + has_negative = true; + } + } + + int width = (max_val == 0.0) ? 1 : static_cast(std::log10(max_val)) + 1; + width += 1 + 2; // decimal point + precision + if (has_negative) { + width += 1; // negative sign + } + + ss << std::fixed << std::setprecision(2); + ss << "PointVector([\n"; + for (size_t i = 0; i < vec.size(); ++i) { + if (i > 0) ss << ",\n"; + ss << "[" << std::setw(width) << vec[i].x() << ", " << std::setw(width) << vec[i].y() << "]"; + } + ss << "\n])"; + return ss.str(); + }; + py::bind_vector(m, "PointVector") - .def("__str__", [](const PointVector& v) { - std::stringstream ss; - ss << "PointVector(["; - for (size_t i = 0; i < v.size(); ++i) { - if (i > 0) ss << ", "; - ss << "(" << v[i].x() << ", " << v[i].y() << ")"; - } - ss << "])"; - return ss.str(); - }); + .def("__repr__", formatPointVector) + .def("__str__", formatPointVector); py::bind_vector>(m, "Point3Vector"); py::bind_vector>(m, "MapTypeVector"); @@ -192,7 +214,8 @@ void pyCoverageControl_core(py::module &m) { .def(py::init()) .def(py::init()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def("ComputeActions", &NearOptimalCVT::ComputeActions) .def("GetActions", &NearOptimalCVT::GetActions) .def("IsConverged", &NearOptimalCVT::IsConverged) @@ -203,7 +226,8 @@ void pyCoverageControl_core(py::module &m) { .def(py::init()) .def(py::init()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def("ComputeActions", &ClairvoyantCVT::ComputeActions) .def("IsConverged", &ClairvoyantCVT::IsConverged) .def("GetActions", &ClairvoyantCVT::GetActions) @@ -214,7 +238,8 @@ void pyCoverageControl_core(py::module &m) { .def(py::init()) .def(py::init()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def("ComputeActions", &DecentralizedCVT::ComputeActions) .def("IsConverged", &DecentralizedCVT::IsConverged) .def("GetActions", &DecentralizedCVT::GetActions) @@ -224,7 +249,8 @@ void pyCoverageControl_core(py::module &m) { .def(py::init()) .def(py::init()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def("ComputeActions", &CentralizedCVT::ComputeActions) .def("IsConverged", &CentralizedCVT::IsConverged) .def("GetActions", &CentralizedCVT::GetActions) @@ -306,7 +332,8 @@ void pyCoverageControl_core_coverage_system(py::module &m) { .def("StepControl", &CoverageSystem::StepControl) .def("StepAction", &CoverageSystem::StepAction) .def("StepActions", &CoverageSystem::StepActions) - .def("StepRobotsToRelativeGoals", &CoverageSystem::StepRobotsToRelativeGoals) + .def("StepRobotsToRelativeGoals", + &CoverageSystem::StepRobotsToRelativeGoals) .def("SetLocalRobotPositions", &CoverageSystem::SetLocalRobotPositions) .def("SetLocalRobotPosition", &CoverageSystem::SetLocalRobotPosition) .def("SetGlobalRobotPosition", &CoverageSystem::SetGlobalRobotPosition) @@ -321,6 +348,10 @@ void pyCoverageControl_core_coverage_system(py::module &m) { py::arg("force_no_noise") = false) .def("GetRobotPositions", &CoverageSystem::GetRobotPositions, "Get Positions of Robots", py::arg("force_no_noise") = false) + .def("GetRobotPositionsConst", &CoverageSystem::GetRobotPositionsConst, + py::return_value_policy::copy, + "Get Positions of Robots const", + py::arg("force_no_noise") = false) .def("GetRobotLocalMap", &CoverageSystem::GetRobotLocalMap, py::return_value_policy::reference_internal) .def("GetRobotWorldMap", &CoverageSystem::GetRobotWorldMap, pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy