[WIP][pr2eus/robot-interface.l] Add method waiting until last sent motion is finished or given function returns t

pr2eus/robot-interface.l

@@ -672,6 +672,44 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
               (send-all cacts :time-to-finish)))
            (t
             (send-all controller-actions :time-to-finish)))))
+  (:wait-interpolation-until-func
+   (func &optional ctype
+         &key (check-interval 0.01)
+         (post-process #'(lambda () (send self :cancel-angle-vector :controller-type ctype)))
+         return-post-process-ret)
+   "Wait until last sent motion is finished or given function (func) returns t.
+E.g., (send *ri* :wait-interpolation-until-func #'(lambda () (> (abs (aref (send *ri* :state :torque-vector) 1)) 5)))
+- ctype : Controller to be waited
+- check-interval : Interval for checking given function [sec]
+- post-process : If this argument is not nil, function stored in this argument is called just after func returns t. This is useful when you want to stop robot motion just after a condition is satisfied. E.g., robot stops abruptly when this argument is #'(lambda () (send *ri* :cancel-angle-vector)), robot stops smoothly when this argument is #'(lambda () (send *ri* :stop-motion))
+- return-post-process-ret : If this argument is nil, this method returns a list of interpolatingp for all controllers as with :wait-interpolation. If this argument is t, this method returns return value of post-process
+"
+   (while
+     (progn
+       (when (funcall func)
+         (ros::ros-info
+           "wait-interpolation-until-func : Waiting finished because given function returned t")
+         (unless (null post-process)
+           (ros::ros-info "wait-interpolation-until-func : Calling ~A..." post-process)
+           (if return-post-process-ret
+               (return-from :wait-interpolation-until-func (funcall post-process))
+             (funcall post-process)))
+         (return))
+       (if (send self :simulation-modep)  ;; Continuation condition starts
+           (when (send self :interpolatingp ctype)  ;; Simulated robot
+             (unix:usleep (round (* check-interval 1000 1000)))
+             (send self :robot-interface-simulation-callback)
+             (send self :interpolatingp ctype))
+         (let (cacts each-timeout)  ;; Real robot
+           (cond (ctype
+                   (setq cacts (gethash ctype controller-table)))
+                 (t
+                   (setq cacts controller-actions)))
+           (setq each-timeout (/ check-interval (float (length cacts))))
+           (send-all cacts :wait-for-result :timeout each-timeout
+                     :wait-rate (max 100 (/ 1.0 each-timeout)))
+           (send self :interpolatingp ctype)))))  ;; Continuation condition ends
+   (send-all controller-actions :interpolatingp))
   (:stop-motion (&key (stop-time 0) (wait t))
    "Smoothly stops the motion being executed by sending the current joint state to the joint trajectory action goal in stop-time [ms]. Please note that if stop-time is smaller than the :angle-vector min-time (defaulting to 1000 [ms]) the actual interpolation uses min-time instead. Refer to :cancel-angle-vector for a faster stop"
    (when (send self :interpolatingp)

pr2eus/test/pr2-ri-test-arm.l

@@ -210,6 +210,84 @@
     (assert (null ret) "interpolation must be finished")
     ))
 
+(deftest test-wait-interpolation-until-func
+  (let (init-av init-torque func torso-angle ret tm-0 tm-1 tm-diff)
+    (setq init-av (send *pr2* :angle-vector (send *ri* :state :potentio-vector)))
+    (setq init-torque (aref (send *ri* :state :torque-vector) 1))
+    (setq func
+          #'(lambda () (> (abs (- (aref (send *ri* :state :torque-vector) 1) init-torque)) 0.001)))
+    (setq torso-angle (send *pr2* :torso :waist-z :joint-angle))
+    (send *pr2* :reset-pose)
+    (send *pr2* :torso :waist-z :joint-angle torso-angle)  ;; torso is very slow, so we have to keep its angle not to overwrite angle-vector duration
+    (send *pr2* :larm :move-end-pos #f(400 0 0) :world)
+    (send *pr2* :rarm :move-end-pos #f(400 0 0) :world)
+
+    (send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
+    (setq ret (send *ri* :wait-interpolation-until-func func))
+    (ros::ros-warn ":wait-interpolation-until-func returns ~A (= t)" ret)
+    (warning-message 3 ":wait-interpolation-until-func returns ~A (= t)~%" ret)
+    (assert (some #'identity ret) "robot should be interpolating")
+    (setq tm-0 (ros::time-now))
+    (send *ri* :wait-interpolation)
+    (setq tm-1 (ros::time-now))
+    (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec))
+    (ros::ros-warn "duration until stop ~A" tm-diff)
+    (warning-message 3 "duration until stop ~A~%" tm-diff)
+    (assert (< tm-diff 0.5) "robot should stop immediately")
+
+    (send *ri* :angle-vector (send *pr2* :angle-vector) 2000 :larm-controller)
+    (setq ret (send *ri* :wait-interpolation-until-func func :larm-controller))
+    (ros::ros-warn ":wait-interpolation-until-func :larm-controller returns ~A (= t)" ret)
+    (warning-message 3 ":wait-interpolation-until-func :larm-controller returns ~A (= t)~%" ret)
+    (assert (some #'identity ret) "robot should be interpolating")
+    (setq tm-0 (ros::time-now))
+    (send *ri* :wait-interpolation :larm-controller)
+    (setq tm-1 (ros::time-now))
+    (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec))
+    (ros::ros-warn "duration until stop ~A" tm-diff)
+    (warning-message 3 "duration until stop ~A~%" tm-diff)
+    (assert (< tm-diff 0.5) "robot should stop immediately")
+
+    (send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
+    (setq ret (send *ri* :wait-interpolation-until-func func nil :return-post-process-ret t))
+    (ros::ros-warn ":wait-interpolation-until-func :return-post-process-ret t returns ~A (= t)"
+                   ret)
+    (warning-message 3 ":wait-interpolation-until-func :return-post-process-ret t returns ~A (= t)~%"
+                     ret)
+    (assert (or (not (listp ret)) (null ret))
+            "return value of post-process (not a list of interpolatingp) should be returned")
+    (send *ri* :wait-interpolation)
+
+    (send *ri* :angle-vector (send *pr2* :angle-vector) 2000)
+    (setq ret
+          (send *ri* :wait-interpolation-until-func func nil
+                :post-process #'(lambda () (send *ri* :stop-motion :wait nil))))
+    (ros::ros-warn ":wait-interpolation-until-func :stop-motion returns ~A (= t)" ret)
+    (warning-message 3 ":wait-interpolation-until-func :stop-motion returns ~A (= t)~%" ret)
+    (assert (some #'identity ret) "robot should be interpolating")
+    (setq tm-0 (ros::time-now))
+    (send *ri* :wait-interpolation)
+    (setq tm-1 (ros::time-now))
+    (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec))
+    (ros::ros-warn "duration until stop ~A" tm-diff)
+    (warning-message 3 "duration until stop ~A~%" tm-diff)
+    (assert (and (> tm-diff 0.5) (< tm-diff 1.3)) "robot should stop smoothly")
+
+    (send *pr2* :angle-vector init-av)
+    (send *ri* :angle-vector (send *pr2* :angle-vector) 1000)
+    (setq ret (send *ri* :wait-interpolation-until-func func nil :post-process nil))
+    (ros::ros-warn ":wait-interpolation-until-func :post-process nil returns ~A (= t)" ret)
+    (warning-message 3 ":wait-interpolation-until-func :post-process nil returns ~A (= t)~%" ret)
+    (assert (some #'identity ret) "robot should be interpolating")
+    (setq tm-0 (ros::time-now))
+    (send *ri* :wait-interpolation)
+    (setq tm-1 (ros::time-now))
+    (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec))
+    (ros::ros-warn "duration until stop ~A" tm-diff)
+    (warning-message 3 "duration until stop ~A~%" tm-diff)
+    (assert (> tm-diff 0.7) "robot should complete execution")
+    ))
+
 (deftest test-end-coords-interpolation
   (let (tm-0 tm-1 tm-diff)
     (send *pr2* :reset-manip-pose)