fix logic errors in Combined Scheduler

src/decmcts.py

@@ -143,20 +143,20 @@ class CombinedSchedule:
                     ao = copy.deepcopy(self.col_table[ct, ct_r])
 
             # apply t_diff and add allowed offset for when other robot is done
-            allowed_offsets.append([(-1e6, t_0 - self.end_times[r][-1]-self.safe_time)] + \
+            allowed_offsets.append([(-1e6, t_0 - self.end_times[r][-1]-10)] + \
                 [(a+t_diff, b+t_diff) for a,b in ao if a+t_diff<=0])
 
         # easy for n = 2
         if self.n == 2:
             # if collision forces this contour to start after other arm is finished their last contour, reject
-            if t_0 + allowed_offsets[0][-1][1] > self.end_times[1-robot][-1] \
+            if t_0 - allowed_offsets[0][-1][1] > self.end_times[1-robot][-1] \
                     and not (self.end_times[robot][-1] == 0 and self.end_times[1-robot][-1] == 0) \
                     and not force:
                 # reject contour if it cannot be parallelised with current plan
                 self.start_next_contour[robot] = self.end_times[1-robot][-1] + self.safe_time
                 return False
 
-            t_wait = max(trav_time, -min(0, allowed_offsets[0][-1][1]))
+            t_wait = -min(0, allowed_offsets[0][-1][1])
             self.trajs[robot].append(ct)
             self.start_times[robot].append(t_0 + t_wait)
             self.end_times[robot].append(self.start_times[robot][-1] + ct.time[-1])
@@ -250,6 +250,7 @@ def createCombinedSchedule(data, states):
                 for r in rs:
                     if len(cts_left[r]) != 0:
                         robot = r
+                        break
 
             # if rejected, switch robots (if possible) and force next contour
             else:
@@ -575,7 +576,7 @@ def decmcts(
 if __name__ == "__main__":
 
     # Read gcode file
-    model = "Top-ascii"
+    model = "prop_v2-ascii"
     tracker = tu.get_tracker(model)
 
     # 3arms simulation
@@ -590,4 +591,4 @@ if __name__ == "__main__":
     sim_env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
 
     # decmcts plan first n contours
-    plan = decmcts(model, sim_env, tracker, nth_layer=50, interactive=False, nodes_per_layer=1000)
+    plan = decmcts(model, sim_env, tracker, nth_layer=50, interactive=False, nodes_per_layer=3000)