check regularly instead of using wait time (#61)

src/decmcts.py

@@ -366,6 +366,8 @@ def decmcts(contour_tracker,
 
             # Trajectory to add and time to wait for it
             traj = combined_schedule.trajs[arm][inds[arm]]
+
+            # TODO THIS VAIRABLE SHOULD BE RELATIVE TO OTHER ARM
             decmcts_wait_time = combined_schedule.start_times[arm][inds[arm]] - \
                     combined_schedule.end_times[arm][inds[arm]-1]
 
@@ -379,17 +381,17 @@ def decmcts(contour_tracker,
                 joint_diff(home, traj.positions[0])
 
             # if there's enough time to get home in the wait period, do that
-            athome = plan.getLastJoint(arm) == home
+            athome = list(plan.getLastJoint(arm)) == home
+            additional_time_spent = 0
             if time_last2home + time_home2next <= decmcts_wait_time:
 
                 # try going to home pos
-                additional_time_spent = 0
                 while not athome:
                     gohome = plan.plan_to(env.home, arm)
 
                     # if no travel found, stay at last pos
                     if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
-                        print(arm, "waiting, returning to home")
+                        print(arm, "returning to home to wait")
                         athome = True
                     else:
                         print("There's enough time to get home but couldn't find a path - Unfortunate")
@@ -404,13 +406,13 @@ def decmcts(contour_tracker,
                             # can't go home anymore
                             break
 
-            if plan.getLastJoint(arm) == home:
-                # stay put at home until it's time to move
-                wait_time = decmcts_wait_time - time_last2home - time_home2next - additional_time_spent
-                if wait_time > 0:
-                    stayput = tu.JTrajectory([home, home],[0,wait_time], arm)
-                    if plan.appendTrajectory([stayput], arm)!=0:
-                        raise Exception("cannot stay home, now we got issues")
+            #if list(plan.getLastJoint(arm)) == home:
+            #    # stay put at home until it's time to move
+            #    wait_time = decmcts_wait_time - time_last2home - time_home2next - additional_time_spent
+            #    if wait_time > 0:
+            #        stayput = tu.JTrajectory([home, home],[0,wait_time], arm)
+            #        if plan.appendTrajectory([stayput], arm)!=0:
+            #            raise Exception("cannot stay home, now we got issues")
 
             # Following commented out since we will wait anyway if not possible, and
             #  the time should be short since no tenough time to get home
@@ -500,7 +502,7 @@ if __name__ == "__main__":
                    n_contours=None,
                    c_traj_save_file=c_traj_file,
                    gui=True,
-                   mcts_nodes=15
+                   mcts_nodes=5
                    )
     visualiseCombinedSchedule(cs)
     plan.visualise()

src/utils/plan.py

@@ -155,24 +155,23 @@ class Plan:
         for r in range(self.n):
             for t in range(1,len(self.trajs[r])):
                 if self.trajs[r][t].contour is not None:
-                    print(t)
                     ax.plot([self.cumul_time[r][t]- self.trajs[r][t].time[-1],
                              self.cumul_time[r][t]],
                             [r,r],
                             'bo-',
                             linewidth=2)
 
-                    #ax.plot([self.cumul_time[r][t]- self.trajs[r][t].time[-1],
-                    #         self.cumul_time[r][t]- self.trajs[r][t].time[-1]],
-                    #        [0,self.n-1],
-                    #        'g--',
-                    #        linewidth=0.5)
-
-                    #ax.plot([self.cumul_time[r][t],
-                    #         self.cumul_time[r][t]],
-                    #        [0,self.n-1],
-                    #        'r--',
-                    #        linewidth=0.5)
+                    ax.plot([self.cumul_time[r][t]- self.trajs[r][t].time[-1],
+                             self.cumul_time[r][t]- self.trajs[r][t].time[-1]],
+                            [0,self.n-1],
+                            'g--',
+                            linewidth=0.5)
+
+                    ax.plot([self.cumul_time[r][t],
+                             self.cumul_time[r][t]],
+                            [0,self.n-1],
+                            'r--',
+                            linewidth=0.5)
         plt.xlabel("Time")
         plt.ylabel("Robots")
         plt.show()