BWI planning to noetic

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.")

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()

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()
 
 

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()

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);
 	}
 

plan_execution/src/libplan_execution/PlanExecutorNode.cpp

@@ -44,17 +44,23 @@ PlanExecutorNode::PlanExecutorNode(const string &domain_directory, map<string, A
   fs.open(working_memory_path, ios::out);
   fs.close();
 
+  std::cout << "Got current.asp" << std::endl;
+
   FilteringQueryGenerator *generator = Clingo::getQueryGenerator("n", domain_directory, {working_memory_path},
                                                                  actionMapToSet(action_map),
                                                                  PLANNER_TIMEOUT);
   planningReasoner = unique_ptr<actasp::AspKR>(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::QueryGenerator>(actasp::Clingo::getQueryGenerator("n", diagnosticsPath.string(), {working_memory_path}, {}, PLANNER_TIMEOUT));
     ros_observer = std::unique_ptr<RosActionServerInterfaceObserver>(new ExplainingRosActionServerInterfaceObserver(server, std::move(diagnosticReasoner)));
   } else {
     ros_observer = std::unique_ptr<RosActionServerInterfaceObserver>(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);