From 9be477067e31c76567d6e96a49f6266de15546e6 Mon Sep 17 00:00:00 2001 From: Yuqian Jiang Date: Mon, 8 May 2023 12:50:21 -0500 Subject: [PATCH] BWI planning to noetic --- .../scripts/populate_with_map | 2 +- .../src/bwi_planning_common/door_function.py | 2 +- .../bwi_planning_common/location_function.py | 42 ++++++++----------- .../bwi_planning_common/object_function.py | 2 +- knowledge_representation | 2 +- .../actasp/include/actasp/reasoners/Clingo.h | 4 ++ .../libplan_execution/PlanExecutorNode.cpp | 6 +++ 7 files changed, 32 insertions(+), 28 deletions(-) diff --git a/bwi_knowledge_representation/scripts/populate_with_map b/bwi_knowledge_representation/scripts/populate_with_map index efda658a..6a200b1b 100755 --- a/bwi_knowledge_representation/scripts/populate_with_map +++ b/bwi_knowledge_representation/scripts/populate_with_map @@ -81,7 +81,7 @@ def populate(ltmc, files_path): def read_yaml_from_file(file_path): with open(file_path, 'r') as stream: try: - contents = yaml.load(stream) + contents = yaml.load(stream, Loader=yaml.Loader) return contents except yaml.YAMLError: print("File found at " + file_path + ", but cannot be parsed by YAML parser.") diff --git a/bwi_planning_common/src/bwi_planning_common/door_function.py b/bwi_planning_common/src/bwi_planning_common/door_function.py index 90f3f833..e4d98046 100644 --- a/bwi_planning_common/src/bwi_planning_common/door_function.py +++ b/bwi_planning_common/src/bwi_planning_common/door_function.py @@ -139,7 +139,7 @@ def readDoorsFromFile(self): approach_pt_1, approach_pt_2) self.draw_door[door_key] = True - except yaml.YAMLError, KeyError: + except (yaml.YAMLError, KeyError): rospy.logerr("File found at " + self.door_file + ", but cannot be parsed by YAML parser. I'm starting doors from scratch.") stream.close() diff --git a/bwi_planning_common/src/bwi_planning_common/location_function.py b/bwi_planning_common/src/bwi_planning_common/location_function.py index 1ed7c1ab..e0f46e21 100644 --- a/bwi_planning_common/src/bwi_planning_common/location_function.py +++ b/bwi_planning_common/src/bwi_planning_common/location_function.py @@ -16,16 +16,6 @@ from .utils import clearLayoutAndFixHeight, getLocationsImageFileLocationFromDataDirectory, \ scalePoint, scalePolygon, getConnectivityFileLocationFromDataDirectory - -def makeConnectivityMap(polygons): - connectivity = {} - for polygon in polygons: - potential_neighbors = copy.copy(polygons) - potential_neighbors.remove(polygon) - connectivity[polygon] = checkConnectivity(polygon, potential_neighbors) - return connectivity - - def checkConnectivity(polygon, other_polygons): neighbors = [] inflated_polygon = inflatePolygon(polygon) @@ -44,13 +34,13 @@ def inflatePolygon(polygon): def makeNeighborsString(polygon, all_locations): - poly_to_name = {poly: name for name, poly in all_locations.items()} - neighbors = checkConnectivity(polygon, all_locations.values()) + neighbors = [] + inflated_polygon = inflatePolygon(polygon) + for potential_neighbor_name, potential_neighbor in all_locations.items(): + if polygon != potential_neighbor and not potential_neighbor.intersected(inflated_polygon).isEmpty(): + neighbors.append(potential_neighbor_name) # Don't allow the polygon to border on itself - if polygon in neighbors: - neighbors.remove(polygon) - as_names = [poly_to_name[p] for p in neighbors] - return "[" + ", ".join(as_names) + "]" + return "[" + ", ".join(neighbors) + "]" class LocationFunction(object): @@ -132,16 +122,20 @@ def saveConfiguration(self): self.writeConnectivityToFile() def writeConnectivityToFile(self): - connectivity = makeConnectivityMap(self.locations.values()) - out_dict = {} - poly_to_name = {poly: name for name, poly in self.locations.items()} - - for location_poly, neighbor_poly in connectivity.items(): - sorted_list = sorted([poly_to_name[p] for p in neighbor_poly]) - out_dict[poly_to_name[location_poly]] = sorted_list + connectivity = {} + #poly_to_name = {poly: name for name, poly in self.locations.items()} + for name, polygon in self.locations.items(): + potential_neighbors = copy.copy(list(self.locations.values())) + potential_neighbors.remove(polygon) + neighbors = [] + inflated_polygon = inflatePolygon(polygon) + for potential_neighbor_name, potential_neighbor in self.locations.items(): + if name != potential_neighbor_name and not potential_neighbor.intersected(inflated_polygon).isEmpty(): + neighbors.append(potential_neighbor_name) + connectivity[name] = sorted(neighbors) stream = open(self.connectivity_file, 'w') - yaml.dump(out_dict, stream) + yaml.dump(connectivity, stream) stream.close() diff --git a/bwi_planning_common/src/bwi_planning_common/object_function.py b/bwi_planning_common/src/bwi_planning_common/object_function.py index a9b80816..12c285e9 100644 --- a/bwi_planning_common/src/bwi_planning_common/object_function.py +++ b/bwi_planning_common/src/bwi_planning_common/object_function.py @@ -93,7 +93,7 @@ def readObjectsFromFile(self): object_orientation = -float(object["point"][2]) self.objects[object_key] = Object(object_location, object_orientation) self.draw_object[object_key] = True - except yaml.YAMLError, KeyError: + except (yaml.YAMLError, KeyError): rospy.logerr("File found at " + self.object_file + ", but cannot be parsed by YAML parser. I'm starting objects from scratch.") stream.close() diff --git a/knowledge_representation b/knowledge_representation index e17ed543..ff7171a9 160000 --- a/knowledge_representation +++ b/knowledge_representation @@ -1 +1 @@ -Subproject commit e17ed543fc1aca28348b74faa541f279c6cec99c +Subproject commit ff7171a987ef4c453c4b8d4599d281e18c51e794 diff --git a/plan_execution/actasp/include/actasp/reasoners/Clingo.h b/plan_execution/actasp/include/actasp/reasoners/Clingo.h index 98986f0b..5d4e1b70 100644 --- a/plan_execution/actasp/include/actasp/reasoners/Clingo.h +++ b/plan_execution/actasp/include/actasp/reasoners/Clingo.h @@ -31,6 +31,10 @@ struct Clingo { if (ros_distro == "melodic") { return new Clingo5_2(incrementalVar, dirToAllAspFilesInDir(linkDir), copyFiles, actions, max_time); } + + if (ros_distro == "noetic") { + return new Clingo5_2(incrementalVar, dirToAllAspFilesInDir(linkDir), copyFiles, actions, max_time); + } assert(false); } diff --git a/plan_execution/src/libplan_execution/PlanExecutorNode.cpp b/plan_execution/src/libplan_execution/PlanExecutorNode.cpp index 293d109b..3fadb490 100644 --- a/plan_execution/src/libplan_execution/PlanExecutorNode.cpp +++ b/plan_execution/src/libplan_execution/PlanExecutorNode.cpp @@ -44,10 +44,13 @@ PlanExecutorNode::PlanExecutorNode(const string &domain_directory, map(new Reasoner(generator, MAX_N, actionMapToSet(action_map))); + std::cout << "Got Reasoner" << std::endl; auto diagnosticsPath = boost::filesystem::path(domain_directory) / "diagnostics"; if (boost::filesystem::is_directory(diagnosticsPath)) { auto diagnosticReasoner = std::unique_ptr(actasp::Clingo::getQueryGenerator("n", diagnosticsPath.string(), {working_memory_path}, {}, PLANNER_TIMEOUT)); @@ -55,6 +58,9 @@ PlanExecutorNode::PlanExecutorNode(const string &domain_directory, map(new RosActionServerInterfaceObserver(server)); } + + std::cout << "Got observer" << std::endl; + { //need a pointer to the specific type for the observer auto replanner = new ReplanningPlanExecutor(*planningReasoner, *planningReasoner, action_map, resourceManager);