successfully converting decMCTS plan to real plan (#49)

src/decmcts.py

@@ -343,7 +343,7 @@ def decmcts(contour_tracker,
 
         # Add contours from combined schedule to real plan
         contours_left = sum([len(l) for l in combined_schedule.trajs])
-        inds = [1] * n_arms
+        inds = [0] * n_arms
         while contours_left>0:
 
             # select trajectory to add next (as per plan not combined schedule)
@@ -418,18 +418,9 @@ def decmcts(contour_tracker,
             i=0
             while not traj_added:
 
-                # create plan to next traj
                 print("---")
+                # create plan to next traj
                 travel_traj = plan.plan_to(traj.positions[0], arm)
-                print(travel_traj)
-                i+=1
-                if travel_traj is None:
-                    print(traj.positions[0])
-                    print(arm)
-                    print(plan.cumul_time)
-                    if i>10:
-                        raise Exception("Failed to create travel traj")
-
                 # try adding traj + contour to planner
                 if travel_traj is not None and plan.appendTrajectory([travel_traj, traj], arm) == 0:
                     # if success, break, else loop again
@@ -448,7 +439,8 @@ def decmcts(contour_tracker,
 
                     # If the cumul time of current arm is greater than that of other arms, send other arms home
                     if plan.cumul_time[arm][-1] == plan.len():
-                        print("current arm has the longest plan, meaning it is ahead of the other arms. Take other arms home now")
+
+                        print("current arm has the longest plan but failing, take other arms home to clear")
                         # send other arms home
                         for other_arm in plan.trajs:
                             if other_arm == arm:
@@ -462,7 +454,7 @@ def decmcts(contour_tracker,
                             gohome = plan.plan_to(env.home, other_arm)
                             if gohome is not None and plan.appendTrajectory([gohome], other_arm)==0:
                                 print("current arm is", arm, ", taking other arm home")
-                                print(arm, "waiting, returning to home")
+                                print("Arm", other_arm, "returning to home")
                                 athome = True
                             else:
                                 raise Exception("Could not go home, need to address logic")