Merge pull request #673 from Affonso-Gui/fix_actionilb_preempt_status

Fix case function misuse in simple-action-server :set-preempted

roseus/euslisp/actionlib.l

@@ -225,10 +225,10 @@
   (:get-state ()
     (let (state)
       (setq state (send (send comm-state :latest-goal-status) :status))
-      (case state
-        (actionlib_msgs::GoalStatus::*recalling*
+      (cond
+        ((= state actionlib_msgs::GoalStatus::*recalling*)
          (setq state actionlib_msgs::GoalStatus::*pending*))
-        (actionlib_msgs::GoalStatus::*preempting*
+        ((= state actionlib_msgs::GoalStatus::*preempting*)
          (setq state actionlib_msgs::GoalStatus::*active*)))
       state))
   (:get-goal-status-text () (send (send comm-state :latest-goal-status) :text))
@@ -350,11 +350,13 @@
   (:set-preempted
    (&optional (msg (send self :result)) (text ""))
    (ros::ros-info ";; Cancel Goal ~A" (send goal-id :id))
-   (case status
-     ((actionlib_msgs::GoalStatus::*pending* actionlib_msgs::GoalStatus::*recalling*)
+   (cond
+     ((or (= status actionlib_msgs::GoalStatus::*pending*)
+          (= status actionlib_msgs::GoalStatus::*recalling*))
       (setq status actionlib_msgs::GoalStatus::*recalled*)
       )
-     ((actionlib_msgs::GoalStatus::*active* actionlib_msgs::GoalStatus::*preepmpting*)
+     ((or (= status actionlib_msgs::GoalStatus::*active*)
+          (= status actionlib_msgs::GoalStatus::*preempting*))
       (setq status actionlib_msgs::GoalStatus::*preempted*)
       ))
    (send self :publish-result msg text))

roseus/test/test-simple-server-cancel.l

@@ -0,0 +1,37 @@
+#!/usr/bin/env roseus
+;;;
+;;;
+
+(require :unittest "lib/llib/unittest.l")
+(ros::load-ros-package "actionlib")
+;;;
+;;;
+(init-unit-test)
+
+(defun cancel-cb (server goal)
+  (send server :set-preempted))
+
+(deftest test-server-cancel ()
+  (let* ((server (instance ros::simple-action-server :init
+                           "/cancel_test" actionlib::TestAction
+                           :execute-cb #'cancel-cb))
+         (client (instance ros::simple-action-client :init
+                           "/cancel_test" actionlib::TestAction))
+         (goal (instance actionlib::TestActionGoal :init)))
+
+    (send client :wait-for-server)
+    (send client :send-goal goal)
+    ;; wait until the message is received
+    (while (not (send server :is-active))
+      (ros::sleep)
+      (send server :spin-once))
+    (send server :worker)
+    (send client :spin-once)
+    (ros::ros-info "action server returned with status: ~S" (send client :get-state))
+    (assert (equal actionlib_msgs::GoalStatus::*PREEMPTED*
+                   (send client :get-state)))))))
+
+(ros::roseus "server_cancel")
+(run-all-tests)
+
+(exit)

roseus/test/test-simple-server-cancel.test

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="simple_server_cancel" pkg="roseus" type="roseus" args="$(find roseus)/test/test-simple-server-cancel.l" />
+</launch>