Merge pull request #578 from furushchev/last-status-msg

roseus: add :last-status-msg method for simple-action-client

roseus/euslisp/actionlib.l

@@ -15,7 +15,7 @@
 (unless (find-package "ROSACTIONLIB") (make-package "ROSACTIONLIB"))
 (defclass ros::simple-action-client
   :super ros::object
-  :slots (name-space action-spec simple-state comm-state status-stamp
+  :slots (name-space action-spec simple-state comm-state status-stamp last-status-msg
                      action-goal-class action-result-class action-feedback-class
                      action-done-cb action-active-cb action-feedback-cb
                      goal_id groupname seq-count))
@@ -24,6 +24,7 @@
   (:goal-status-cb
    (msg)
    ;;(ros::ros-debug "[~A] status-cb (seq:~A ~A)" name-space (send msg :header :seq) (mapcar #'(lambda (s) (format nil "id:~A, status:~A" (send s :goal_id :id) (goal-status-to-string (send s :status)))) (send msg :status_list)))
+   (setq last-status-msg msg)
    (let ((m (send comm-state :find-status-by-goal-id msg)))
      (when m
        (let ((com-state (send comm-state :update-status msg))
@@ -58,6 +59,7 @@
    (msg)
    (let (dummy-msg)
      (ros::ros-debug "[~A] reuslt-cb ~A ~A" name-space (send msg :status :goal_id :id) (goal-status-to-string (send msg :status :status)))
+     (if (null goal_id) (return-from :action-result-cb nil))
      (unless (send comm-state :update-result msg)
        (ros::ros-warn "[~A] action-result-cb may recieved old client's goal" name-space)
        (ros::ros-warn "     expected goal id ~A" (and (send comm-state :action-goal) (send (send comm-state :action-goal) :goal_id :id)))
@@ -232,6 +234,7 @@
          (setq state actionlib_msgs::GoalStatus::*active*)))
       state))
   (:get-goal-status-text () (send (send comm-state :latest-goal-status) :text))
+  (:last-status-msg () last-status-msg)
   (:cancel-all-goals
    ()
    (let (cancel)

roseus/test/simple-client-test.l

@@ -15,7 +15,7 @@
 (setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b)))
 
 (deftest test-simple-client ()
-  (let (c goal)
+  (let (c goal status)
     (setq c (instance ros::simple-action-client :init
                       "reference_simple_action" actionlib::TestAction))
     (warning-message 1 "wait-for-server~%")
@@ -59,6 +59,11 @@
     (assert (equal (send c :get-state) actionlib_msgs::GoalStatus::*succeeded*))
     (assert (equal (send (elt saved-feedback 0) :feedback :feedback) 9))
 
+    ;; check last-status-msg
+    (setq status (send c :last-status-msg))
+    (assert (derivedp status actionlib_msgs::GoalStatusArray))
+    (assert (> (length (send status :status_list)) 0))
+
     (send c :cancel-all-goals)
     ))