add test to check smach output

.travis.yml

@@ -15,6 +15,8 @@ services:
 env:
   global:
     - USE_DOCKER=true
+    - CATKIN_PARALLEL_TEST_JOBS="-p1"
+    - ROS_PARALLEL_TEST_JOBS="-j1"
   matrix:
     - CHECK_PYTHON3_COMPILE=true
     - CHECK_PYTHON2_COMPILE=true

pddl/pddl_planner/demos/2011_saito/solve-knock-door.l

@@ -202,7 +202,7 @@
   (format t ";; open action [~a]~%" obj) t)
 (defun PR2_ACTION::check_open (obj)
   (format t ";; check if open [~a]~%" obj)
-  (< 0.8 (rand))) ;; 20% success
+  (< 0.8 (random 1.0))) ;; 20% success
 (defun PR2_ACTION::wipe (obj)
   (format t ";; wiping [~a]       ~%" obj) t)
 (defun PR2_ACTION::pick (obj)
@@ -218,6 +218,7 @@
 (load "package://roseus_smach/src/pddl2smach.l")
 
 ;; global data is not used (nil)
+;; currently pddl-smach supports only success/failure branch, but this sample has multi-goal branch. So this sample does not work well with smach.
 (exec-smach-with-spin (convert-smach *graph*) nil :hz 1.0)
 
 (if *exit-on-end* (ros::exit))

pddl/pddl_planner/demos/2011_saito/solve-taking-elevator.l

@@ -1,5 +1,7 @@
 #!/usr/bin/env roseus
 
+(warn "~%!!!!! THIS DEMO IS INCOMPLETE !!!!!~%")
+
 (load "package://pddl_planner/src/pddl-result-graph.l")
 (load "package://pddl_planner/src/eus-pddl-client.l")
 

pddl/pddl_planner/src/eus-pddl.l

@@ -678,8 +678,11 @@
                 (ret (mapcar #'(lambda(obj)
                                  (if (equal (caddr v) (cdr obj))
                                      (multiple-value-bind (va ar) param
-                                       (send self :check-state st (caddr ss) (list (append va (list (car v))) (append ar (list (car obj))))))
-                                   nil)
+                                       (send self :check-state
+                                             st
+                                             (if (= (length v) 3) (caddr ss) (list (car ss) (if (eq '- (cadr v)) (cdddr v) (cdr v)) (caddr ss)))
+                                             (list (append va (list (car v))) (append ar (list (car obj))))))
+                                     nil)
                                  )
                              objects)))
            (null (every #'null ret))))
@@ -688,8 +691,11 @@
                 (ret (mapcar #'(lambda(obj)
                                  (if (equal (caddr v) (cdr obj))
                                      (multiple-value-bind (va ar) param
-                                       (send self :check-state st (caddr ss) (list (append va (list (car v))) (append ar (list (car obj))))))
-                                   t)
+                                       (send self :check-state
+                                             st
+                                             (if (= (length v) 3) (caddr ss) (list (car ss) (if (eq '- (cadr v)) (cdddr v) (cdr v)) (caddr ss)))
+                                             (list (append va (list (car v))) (append ar (list (car obj))))))
+                                     t)
                                  )
                              objects)))
            (null (some #'null ret))))
@@ -782,9 +788,12 @@
         ('forall
          (let ((v (cadr ee)))
            (dolist (obj objects)
-             (when (equal (caddr v) (cdr obj))
+             (when (equal (cadr (memq '- v)) (cdr obj))
                (multiple-value-bind (va ar) param
-                 (setq tmp-st (send self :change-state tmp-st (caddr ee) (list (append va (list (car v))) (append ar (list (car obj)))))))))
+                 (setq tmp-st (send self :change-state
+                                    tmp-st
+                                    (if (= (length v) 3) (caddr ee) (list (car ee) (if (eq '- (cadr v)) (cdddr v) (cdr v)) (caddr ee)))
+                                    (list (append va (list (car v))) (append ar (list (car obj)))))))))
            tmp-st))
         ('when
          (case (caadr ee)

pddl/pddl_planner/test/2011_saito_knock_door.test

@@ -5,7 +5,7 @@
   </include>
 
   <test name="pddl_result_test"
-        test-name="2011_saito_knock_door"
+        test-name="2011_saito_knock_door_pddl"
         pkg="pddl_planner" type="pddlresulttest">
     <rosparam>
       topics:
@@ -23,4 +23,16 @@
           sequence_action: ['(MOVE RM73A3-CENTER RM73B2-DOORFRONT)', '(OPEN RM73B2-DOOR)', '(CHECK_OPEN RM73B2-DOOR)', '(MOVE RM73B2-DOORFRONT RM73A3-CENTER)', '(MOVE RM73A3-CENTER RM73B2-TABLEFRONT)', '(PICK PLASTIC-BOTTLE)', '(MOVE RM73B2-TABLEFRONT RM73A3-CENTER)', '(MOVE RM73A3-CENTER TRASHBOXFRONT)', '(PUT PLASTIC-BOTTLE TRASHBOX)', '(MOVE TRASHBOXFRONT RM73A3-CENTER)']
     </rosparam>
   </test>
+
+  <test name="structure_test"
+        test-name="2011_saito_knock_door_smach"
+        pkg="pddl_planner" type="structuretest">
+    <rosparam>
+      topics:
+        - name: /server_name/smach/container_structure
+          timeout: 10
+          children: ['(move rm73a3-center rm73b2-doorfront)', '(open rm73b2-door)', '(check_open rm73b2-door)', '(move rm73b2-doorfront rm73a3-center)', '(move rm73a3-center rm73b2-tablefront)', '(pick plastic-bottle)', '(wipe rm73b2-table)', '(move rm73b2-tablefront rm73a3-center)', '(move rm73a3-center trashboxfront)', '(put plastic-bottle trashbox)', '(move trashboxfront rm73a3-center)', '(move rm73a3-center shopfront)', '(buy sandwitch)', '(move shopfront rm73a3-center)', '(put sandwitch saito-table)']
+          container_outcomes: ['goal2', 'goal1', 'goal0']
+    </rosparam>
+  </test>
 </launch>

pddl/pddl_planner/test/2011_saito_simple.test

@@ -4,7 +4,7 @@
   </include>
 
   <test name="pddl_result_test"
-        test-name="2011_saito_simple"
+        test-name="2011_saito_simple_pddl"
         pkg="pddl_planner" type="pddlresulttest">
     <rosparam>
       topics:
@@ -13,4 +13,16 @@
           sequence_action: ['(KNOCK DOOR)', '(MOVE ROOM-FRONT ROOM-INSIDE)', '(WIPE DESK)']
     </rosparam>
   </test>
+
+  <test name="structure_test"
+        test-name="2011_saito_simple_smach"
+        pkg="pddl_planner" type="structuretest">
+    <rosparam>
+      topics:
+        - name: /server_name/smach/container_structure
+          timeout: 10
+          children: ['(knock door)', '(move room-front room-inside)', '(wipe desk)']
+          container_outcomes: ['goal0']
+    </rosparam>
+  </test>
 </launch>

pddl/pddl_planner/test/2013_fridge_demo.test

@@ -27,4 +27,16 @@
           sequence_action: ['(MOVE-TO START FRONTFRIDGE)', '(OPEN-DOOR)', '(MOVE-TO FRONTFRIDGE PREGRASP)', '(GRASP-OBJECT CAN)', '(MOVE-TO PREGRASP START)', '(MOVE-TO START SOMEWHERE)', '(TRY-CLOSE)', '(MOVE-RECOVERLY)', '(MOVE-TO FRONTFRIDGE START)']
     </rosparam>
   </test>
+
+  <test name="structure_test"
+        test-name="2013_fridge_demo_smach"
+        pkg="pddl_planner" type="structuretest">
+    <rosparam>
+      topics:
+        - name: /server_name/smach/container_structure
+          timeout: 10
+          children: ['(move-to start frontfridge)', '(open-door)', '(move-to frontfridge pregrasp)', '(grasp-object can)', '(move-to pregrasp start)', '(move-to start somewhere)', '(try-close)', '(move-recoverly)', '(move-to frontfridge start)', '(move-recoverly)', '(close-door)', '(move-to preclose start)', '(try-close)']
+          container_outcomes: ['goal0']
+    </rosparam>
+  </test>
 </launch>

pddl/pddl_planner/test/search_object.test

@@ -1,6 +1,6 @@
 <launch>
   <include file="$(find pddl_planner)/demos/search_object/demo_search_object.launch">
-    <arg name="display_graph" default="false" />
+    <arg name="display_graph" default="true" />
     <arg name="launch_prefix" default="$(find pddl_planner)/test/repeat.sh 5 " />
   </include>
 
@@ -24,4 +24,16 @@
           sequence_action: ['(move-to boxa)', '(open-box boxa)', '(detect_f boxa)', '(close-box boxa)', '(move-to boxb)', '(open-box boxb)', '(detect_f boxb)', '(close-box boxb)', '(move-to boxc)', '(open-box boxc)', '(detect_f boxc)', '(close-box boxc)', '(move-to end)']
     </rosparam>
   </test>
+
+  <test name="structure_test"
+        test-name="search_object_smach"
+        pkg="pddl_planner" type="structuretest">
+    <rosparam>
+      topics:
+        - name: /server_name/smach/container_structure
+          timeout: 10
+          children: ['(move-to boxa)', '(open-box boxa)', '(detect boxa)', '(grasp boxa)', '(close-box boxa)', '(move-to end)', '(close-box boxa)', '(move-to boxb)', '(open-box boxb)', '(detect boxb)', '(grasp boxb)', '(close-box boxb)', '(move-to end)', '(close-box boxb)', '(move-to boxc)', '(open-box boxc)', '(detect boxc)', '(grasp boxc)', '(close-box boxc)', '(move-to end)', '(close-box boxc)', '(move-to end)']
+          container_outcomes: ['goal3', 'goal2', 'goal1', 'goal0']
+    </rosparam>
+  </test>
 </launch>

pddl/pddl_planner/test/simple_failure_torelant.test

@@ -17,4 +17,16 @@
           sequence_action: ['(move-to elevator)', '(look elevator-button)', '(push-button elevator-button)']
     </rosparam>
   </test>
+
+  <test name="structure_test"
+        test-name="simple_failure_smach"
+        pkg="pddl_planner" type="structuretest">
+    <rosparam>
+      topics:
+        - name: /server_name/smach/container_structure
+          timeout: 10
+          children: ['(move-to elevator)', '(look elevator-button)', '(push-button elevator-button)', '(move-to elevator)']
+          container_outcomes: ['goal0']
+    </rosparam>
+  </test>
 </launch>

pddl/pddl_planner/test/structuretest

@@ -0,0 +1,204 @@
+#!/usr/bin/env python
+###############################################################################
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2016, Kentaro Wada.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+###############################################################################
+
+"""
+Integration test node that subscribes to smach_msgs/SmachContainerStructure
+topic (/server_name/smach/container_structure) and verifies it's contents
+
+below parameters must be set:
+
+<test name="structuretest"
+      test-name="structuretest"
+      pkg="pddl_planner" type="structuretest">
+  <rosparam>
+    topics:
+      - name: a topic name
+        timeout: timeout for the topic
+        children: contens of children
+        internal_outcomes: contens of internal_outcomes
+        outcomes_from: contens of coutcomes_from
+        outcomes_to: contens of outcomes_to
+        container_outcomes: contens of container_outcomes
+      - name: another topic name
+        timeout: timeout for the topic
+        children: contens of children
+        internal_outcomes: contens of internal_outcomes
+        outcomes_from: contens of coutcomes_from
+        outcomes_to: contens of outcomes_to
+        container_outcomes: contens of container_outcomes
+  </rosparam>
+</test>
+
+Author: Kentaro Wada <[email protected]>
+Modified for SmachContainerStructure by <[email protected]>
+"""
+from __future__ import print_function
+
+import sys
+import time
+import unittest
+
+import rospy
+import rostopic
+
+from smach_msgs.msg import SmachContainerStructure
+
+PKG = 'pddl_planner'
+NAME = 'structtest'
+
+
+class StructureChecker(object):
+    def __init__(self, topic_name, timeout, struct):
+        self.topic_name = topic_name
+        self.deadline = rospy.Time.now() + rospy.Duration(timeout)
+        self.struct = struct
+        self.msg = None
+        rospy.loginfo("subscribing {}".format(topic_name))
+        rospy.loginfo(" - children: {}".format(struct.children))
+        rospy.loginfo(" - internal_outcomes: {}".format(struct.internal_outcomes))
+        rospy.loginfo(" - outcomes_from: {}".format(struct.outcomes_from))
+        rospy.loginfo(" - outcomes_to: {}".format(struct.outcomes_to))
+        rospy.loginfo(" - container_outcomes: {}".format(struct.container_outcomes))
+        self.sub = rospy.Subscriber(topic_name, SmachContainerStructure, self._callback)
+
+    def _callback(self, msg):
+        message_matched = True
+
+        # skip tailing spaces
+        msg.children = map(lambda x: x.strip(), msg.children)
+        msg.internal_outcomes = map(lambda x: x.strip(), msg.internal_outcomes)
+        msg.outcomes_from = map(lambda x: x.strip(), msg.outcomes_from)
+        msg.outcomes_to = map(lambda x: x.strip(), msg.outcomes_to)
+        msg.container_outcomes = map(lambda x: x.strip(), msg.container_outcomes)
+        rospy.loginfo("received msg")
+        if not self.struct.children == None:
+            rospy.loginfo(" - children: {}".format(msg.children))
+            if not set(msg.children) == set(self.struct.children):
+                rospy.loginfo("  expecting: {}".format(self.struct.children))
+                message_matched = False
+        if not self.struct.internal_outcomes == None:
+            rospy.loginfo(" - internal_outcomes: {}".format(msg.internal_outcomes))
+            if not set(msg.internal_outcomes) == set(self.struct.internal_outcomes):
+                rospy.loginfo("       expecting    : {}".format(self.struct.internal_outcomes))
+                message_matched = False
+        if not self.struct.outcomes_from == None:
+            rospy.loginfo(" - outcomes_from: {}".format(msg.outcomes_from))
+            if not set(msg.outcomes_from) == set(self.struct.outcomes_from):
+                rospy.loginfo("       expecting: {}".format(self.struct.outcomes_from))
+                message_matched = False
+        if not self.struct.outcomes_to == None:
+            rospy.loginfo(" - outcomes_to: {}".format(msg.outcomes_to))
+            if not set(msg.outcomes_to) == set(self.struct.outcomes_to):
+                rospy.loginfo("     expecting: {}".format(self.struct.outcomes_to))
+                message_matched = False
+        if not self.struct.container_outcomes == None:
+            rospy.loginfo(" - container_outcomes: {}".format(msg.container_outcomes))
+            if not set(msg.container_outcomes) == set(self.struct.container_outcomes):
+                rospy.loginfo("            expecting: {}".format(self.struct.container_outcomes))
+                message_matched = False
+
+        if message_matched:
+            self.msg = msg
+
+    def assert_published(self):
+        if self.msg:
+            return True
+        if rospy.Time.now() > self.deadline:
+            return False
+        return None
+
+
+class StructureTest(unittest.TestCase):
+    def __init__(self, *args):
+        super(self.__class__, self).__init__(*args)
+        rospy.init_node(NAME)
+        # scrape rosparam
+        self.topics = []
+        params = rospy.get_param('~topics', [])
+        for param in params:
+            if 'name' not in param:
+                self.fail("'name' field in rosparam is required but not specified.")
+            topic = {'timeout': 10, 'children': None, 'internal_outcomes': None, 'outcomes_from': None, 'outcomes_to': None, 'container_outcomes': None}
+            topic.update(param)
+            self.topics.append(topic)
+        # check if there is at least one topic
+        if not self.topics:
+            self.fail('No topic is specified in rosparam.')
+
+    def test_publish(self):
+        """Test topics are published and messages come"""
+        use_sim_time = rospy.get_param('/use_sim_time', False)
+        t_start = time.time()
+        while not rospy.is_shutdown() and \
+                use_sim_time and (rospy.Time.now() == rospy.Time(0)):
+            rospy.logwarn_throttle(
+                1, '/use_sim_time is specified and rostime is 0, /clock is published?')
+            if time.time() - t_start > 10:
+                self.fail('Timed out (10s) of /clock publication.')
+            # must use time.sleep because /clock isn't yet published, so rospy.sleep hangs.
+            time.sleep(0.1)
+        # subscribe topics
+        checkers = []
+        for topic in self.topics:
+            topic_name = topic['name']
+            timeout = topic['timeout']
+            struct = SmachContainerStructure()
+            struct.children = topic['children']
+            struct.internal_outcomes = topic['internal_outcomes']
+            struct.outcomes_from = topic['outcomes_from']
+            struct.outcomes_to = topic['outcomes_to']
+            struct.container_outcomes = topic['container_outcomes']
+
+            checkers.append(StructureChecker(topic_name, timeout, struct))
+        deadline = max(checker.deadline for checker in checkers)
+        # assert
+        finished_topics = []
+        while not rospy.is_shutdown():
+            if len(self.topics) == len(finished_topics):
+                break
+            for checker in checkers:
+                if checker.topic_name in finished_topics:
+                    continue  # skip topic testing has finished
+                ret = checker.assert_published()
+                if ret is None:
+                    continue  # skip if there is no test result
+                finished_topics.append(checker.topic_name)
+                assert ret, 'Topic [%s] is not published' % (checker.topic_name)
+            rospy.sleep(0.01)
+
+
+if __name__ == '__main__':
+    import rostest
+    rostest.run(PKG, NAME, StructureTest, sys.argv)