From a8923e549954f62147ceb1ca51ea38009d841e0c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 21 Jun 2016 01:49:41 +0900 Subject: [PATCH] add code when last-feedback-msgs-stamp is not updated --- pr2eus/robot-interface.l | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index e556c7392..8b9c345c2 100644 --- a/pr2eus/robot-interface.l +++ b/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))