catch failed trajectory and exit prematurely (#17)

src/Controller.py

@@ -323,8 +323,12 @@ class Controller(object):
         if len(msg.status_list)>0:
             self.traj_complete = (msg.status_list[-1].status==3 or
                 msg.status_list[-1].status==2)
-#            if self.traj_complete:
-#                print("Trajectory Execution Completed")
+            if msg.status_list[-1].status >= 4:
+                print("Trajectory has failed to execute, please check ROS MoveIt execution planner to see why")
+                print("STATUS: {}".format(msg.status_list[-1].status))
+                print("For info on status meaning, go to:\n" + \
+                    "http://docs.ros.org/en/melodic/api/actionlib_msgs/html/msg/GoalStatus.html")
+                sys.exit(0)
 
 
 def getR_ET():