Merge branch 'main' of github.com:StarlingUAS/starling_allocator into…

… main
StarlingUAS · Oct 7, 2022 · d9e470b · d9e470b
2 parents 3b974ed + 9b77e7e
commit d9e470b
Showing 1 changed file with 49 additions and 12 deletions.
diff --git a/starling_allocator/starling_allocator/main.py b/starling_allocator/starling_allocator/main.py
@@ -160,28 +160,65 @@ def create_namespace_trajectory_mapping_nearest(self, req, traj_tuple):
         traj_locations = {k: t.points[0].positions[:3] for (k, t, _) in traj_tuple}
 
         assigned = {}
-        assigned_traj_idx = set()
-        for cn, cl in self.current_locations.items():
-            self.get_logger().info(f"Assigning {cn}")
+        # assigned_traj_idx = set()
+        # for cn, cl in self.current_locations.items():
+        #     self.get_logger().info(f"Assigning {cn}")
+        #     cassin = None
+        #     min_dist = 100000000000
+        #     for k, position in traj_locations.items():
+        #         if k in assigned_traj_idx:
+        #             self.get_logger().info(f"{k} already assigned")
+        #             continue
+        #         dist = np.linalg.norm(np.array(cl) - np.array(position))
+        #         self.get_logger().info(f"Checking traj {k} at dist {dist} from vehicle position {cl} to init traj {position}")
+        #         if dist < min_dist:
+        #             min_dist = dist
+        #             cassin = k
+        #             self.get_logger().info(f"Maybe Assinging traj {k} to {cn}")
+        #     if cassin is None:
+        #         self.get_logger().warn("There are more vehicles than trajectories, all trajectories assigned")
+        #         continue
+        #     self.get_logger().info(f"Assigned vehicle {cn} to traj {cassin}")
+        #     assigned[cn] = traj_tuple[cassin]
+        #     assigned_traj_idx.add(cassin)
+
+        #     blah = {cn: ttp[0] for cn, ttp in assigned.items()}
+        #     self.get_logger().info(f"Assigned: {blah}")
+
+        assigned_vehicles = set()
+        # Trajectory id (k) and traj start location (position)
+        for k, position in traj_locations.items(): 
+
             cassin = None
             min_dist = 100000000000
-            for k, position in traj_locations.items():
-                if k in assigned_traj_idx:
-                    self.get_logger().info(f"{k} already assigned")
+
+            # Vehicle name (cn) and vehicle location (cl)
+            for cn, cl in self.current_locations.items():
+
+                if cn in assigned_vehicles:
+                    self.get_logger().info(f"{cn} already assigned")
                     continue
-                dist = np.linalg.norm(np.array(cl) - np.array(position))
-                self.get_logger().info(f"Checking traj {k} at dist {dist} from vehicle position {cl} to init traj {position}")
+
+                # X-Y plane distance 
+                dist = np.linalg.norm(np.array(cl)[:2] - np.array(position)[:2])
+                self.get_logger().info(f"Checking traj {k} at dist {dist} from {cn} position {cl} to init traj {position}")
                 if dist < min_dist:
                     min_dist = dist
-                    cassin = k
+                    cassin = cn
                     self.get_logger().info(f"Maybe Assinging traj {k} to {cn}")
-            self.get_logger().info(f"Assigned vehicle {cn} to traj {cassin}")
-            assigned[cn] = traj_tuple[cassin]
-            assigned_traj_idx.add(cassin)
+
+            if cassin is None:
+                self.get_logger().warn("There are more trajectories than vehicles, all vehicles assigned")
+                continue
+
+            self.get_logger().info(f"Assigned trajetory {k} to vehicle {cassin}")
+            assigned[cassin] = traj_tuple[k]
+            assigned_vehicles.add(cassin)
 
             blah = {cn: ttp[0] for cn, ttp in assigned.items()}
             self.get_logger().info(f"Assigned: {blah}")
 
+
         return assigned
 
     def create_namespace_trajectory_mapping_random(self, req, traj_tuple):