Merge branch 'master' into smach-structure-pub

jsk-ros-pkg · Jan 25, 2023 · 5b9950d · 5b9950d
2 parents 6b373ec + f29889f
commit 5b9950d
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 6 deletions.
diff --git a/roseus/euslisp/actionlib-commstate.l b/roseus/euslisp/actionlib-commstate.l
@@ -1,7 +1,7 @@
 (if (not (find-package "ACTIONLIB_MSGS")) (ros::roseus-add-msgs "actionlib_msgs"))
 
 (defun goal-status-to-string (i)
-  (elt '(actionlib_msgs::GoalStatus::*pending* actionlib_msgs::GoalStatus::*active* actionlib_msgs::GoalStatus::*preempted* actionlib_msgs::GoalStatus::*succeeded* actionlib_msgs::GoalStatus::*aborted* actionlib_msgs::GoalStatus::*rejected* actionlib_msgs::GoalStatus::*preempting* actionlib_msgs::GoalStatus::*recalling* actionlib_msgs::GoalStatus::*recalled* actionlib_msgs::lost) i))
+  (elt '(actionlib_msgs::GoalStatus::*pending* actionlib_msgs::GoalStatus::*active* actionlib_msgs::GoalStatus::*preempted* actionlib_msgs::GoalStatus::*succeeded* actionlib_msgs::GoalStatus::*aborted* actionlib_msgs::GoalStatus::*rejected* actionlib_msgs::GoalStatus::*preempting* actionlib_msgs::GoalStatus::*recalling* actionlib_msgs::GoalStatus::*recalled* actionlib_msgs::GoalStatus::*lost*) i))
 
 ;; state machine transition
 (setq

diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp
@@ -1082,15 +1082,16 @@ pointer ROSEUS_WAIT_FOR_SERVICE(register context *ctx,int n,pointer *argv)
 {
   isInstalledCheck;
   string service;
+  numunion nu;
 
   ckarg2(1,2);
   if (isstring(argv[0])) service = ros::names::resolve((char *)get_string(argv[0]));
   else error(E_NOSTRING);
 
-  int32_t timeout = -1;
+  float timeout = -1;
 
   if( n > 1 )
-    timeout = (int32_t)ckintval(argv[1]);
+    timeout = ckfltval(argv[1]);
 
   bool bSuccess = service::waitForService(service, ros::Duration(timeout));
 

diff --git a/roseus/test/test-add-two-ints.l b/roseus/test/test-add-two-ints.l
@@ -52,9 +52,9 @@
         (warning-message 3 (format nil "[~A] integration failure (~A+~A)=~A(~A)/=~A(~A)~%"
                                    service-call-error
                                    a b (+ a b) (class (+ a b)) (send res :sum) (class (send res :sum)))))
-      (assert (or (< service-call-error 3) (= (+ (send req :a) (send req :b)) (send res :sum)))
-              (format nil "integration failure (~A+~A)=~A(~A)/=~A(~A)"
-                           a b (+ a b) (class (+ a b)) (send res :sum) (class (send res :sum))))
+      (assert (or (< service-call-error 5) (= (+ (send req :a) (send req :b)) (send res :sum)))
+              (format nil "integration failure ~A times ... (~A+~A)=~A(~A)/=~A(~A)"
+                           service-call-error a b (+ a b) (class (+ a b)) (send res :sum) (class (send res :sum))))
       (sys::gc)
       )))
 

diff --git a/roseus_smach/src/state-machine.l b/roseus_smach/src/state-machine.l
@@ -20,6 +20,17 @@
   (:init (&key (parallel nil))
      (setq parallel-action-p parallel)
      self)
+  (:node (name &key (flatten nil))
+    (if (and flatten (send self :sub-sm-node))
+      (find name (send self :nodes :flatten t)
+            :key #'(lambda (x) (send x :name)) :test #'equal)
+      (send-super :node name)))
+  (:nodes (&key (flatten nil))
+    (if (and flatten (send self :sub-sm-node))
+      (append
+        (send-super :nodes)
+        (flatten (send-all (send-all (send self :sub-sm-node) :submachine) :nodes :flatten t)))
+      (send-super :nodes)))
   (:active-state
    (&rest args)
    (let ((node (car args)))