add code when last-feedback-msgs-stamp is not updated

pr2eus/robot-interface.l

@@ -525,7 +525,14 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
             (cacts (cond
                     (ctype (gethash ctype controller-table))
                     (t     controller-actions))))
-        (while (some #'(lambda (x) (<= (send (ros::time- (send x :last-feedback-msg-stamp) tm) :to-sec) 0)) cacts)
+        (while (some #'(lambda (x)
+                         (and
+                          (<= (send (ros::time- (send x :last-feedback-msg-stamp) tm) :to-sec) 0)
+                          (let ((goal (send (x . ros::comm-state) :action-goal)))
+                            (>= (send (ros::time- (ros::time+ (send goal :header :stamp)
+                                                              (send (car (last (send goal :goal :trajectory :points))) :time_from_start))
+                                                  (ros::time-now)) :to-sec) 0))))
+                     cacts)
           (send self :spin-once) ;; need to wait for feedback
           (send self :ros-wait 0.005))))
     (while (null (send self :interpolating-smoothp time-to-finish ctype))