Merge pull request #333 from knorth55/fix-state-method

[pr2eus] do not pass :wait-until-update in :state args

pr2eus/robot-interface.l

@@ -871,6 +871,10 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
 - :wait-until-update nil wait until joint_state is updated"
    (send self :update-robot-state
          :wait-until-update (when (position :wait-until-update args) (elt args (+ (position :wait-until-update args) 1))))
+   ;; remove :wait-until-update and its argument
+   (when (position :wait-until-update args)
+     (setq args (append (subseq args 0 (position :wait-until-update args))
+                        (cddr (memq :wait-until-update args)))))
    (unless args (return-from :state))
    (case (car args)
      ((:potentio-vector :angle-vector)
@@ -883,6 +887,8 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
       (let ((mm (find (car args) (remove-if-not #'(lambda (x) (substringp "-VECTOR" (string x))) (send self :methods)))))
         (if mm ;; if xx-vector method exists
             (send* self mm (cdr args))
+          ;; digging arguments in robot-state and return nil if not found
+          ;; (send *ri* :state :odom :pose :x)
           (let ((cur robot-state))
             (dolist (key args cur)
               (setq cur (cdr (assoc key cur))))))

pr2eus/test/pr2-read-state-test.l

@@ -24,6 +24,13 @@
 	  "can not read :potentio-vector")
   (assert (send *ri* :state :torque-vector)
 	  "can not read torque-vector")
+  (assert (send *ri* :state :stamp :wait-until-update nil)
+	  "can not read :stamp :wait-until-update nil")
+  (assert (send *ri* :state :potentio-vector :wait-until-update nil)
+	  "can not read :potentio-vector :wait-until-update nil")
+  (assert (send *ri* :state :torque-vector :wait-until-update nil)
+	  "can not read torque-vector :wait-until-update nil")
+
   (let ((lend (send *tfl* :lookup-transform "/base_footprint" "/l_gripper_tool_frame"
 		    (ros::time 0)))
 	(rend (send *tfl* :lookup-transform "/base_footprint" "/r_gripper_tool_frame"
@@ -65,12 +72,19 @@
   (assert (send *ri* :state :odom :pose))
   (assert (derivedp (send *ri* :state :odom :pose) coordinates))
   (assert (send *ri* :state :odom :velocity))
+  (assert (send *ri* :state :odom :stamp :wait-until-update nil))
+  (assert (send *ri* :state :odom :pose :wait-until-update nil))
+  (assert (derivedp (send *ri* :state :odom :pose :wait-until-update nil)
+                    coordinates))
+  (assert (send *ri* :state :odom :velocity :wait-until-update nil))
   )
 
 (deftest pr2-calc-worldcoords-test ()
   (ros::spin-once)
   (assert (send *ri* :state :worldcoords)
 	  "failed to get robot position from /world frame")
+  (assert (send *ri* :state :worldcoords :wait-until-update nil)
+	  "failed to get robot position from /world frame")
   )
 
 #|