diff --git a/.travis.yml b/.travis.yml
index 978283a3..76172676 100644
--- a/.travis.yml
+++ b/.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
diff --git a/pddl/pddl_planner/demos/2011_saito/solve-knock-door.l b/pddl/pddl_planner/demos/2011_saito/solve-knock-door.l
index da7b8743..8dd9b45f 100755
--- a/pddl/pddl_planner/demos/2011_saito/solve-knock-door.l
+++ b/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))
diff --git a/pddl/pddl_planner/demos/2011_saito/solve-taking-elevator.l b/pddl/pddl_planner/demos/2011_saito/solve-taking-elevator.l
index 9d93cda7..17f1fc8f 100755
--- a/pddl/pddl_planner/demos/2011_saito/solve-taking-elevator.l
+++ b/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")
diff --git a/pddl/pddl_planner/src/eus-pddl.l b/pddl/pddl_planner/src/eus-pddl.l
index f85101da..1a9d0795 100644
--- a/pddl/pddl_planner/src/eus-pddl.l
+++ b/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)
diff --git a/pddl/pddl_planner/test/2011_saito_knock_door.test b/pddl/pddl_planner/test/2011_saito_knock_door.test
index dbb5bafb..d308c1e4 100644
--- a/pddl/pddl_planner/test/2011_saito_knock_door.test
+++ b/pddl/pddl_planner/test/2011_saito_knock_door.test
@@ -5,7 +5,7 @@
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)']
+
+
+
+ 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']
+
+
diff --git a/pddl/pddl_planner/test/2011_saito_simple.test b/pddl/pddl_planner/test/2011_saito_simple.test
index e95771c8..064f600a 100644
--- a/pddl/pddl_planner/test/2011_saito_simple.test
+++ b/pddl/pddl_planner/test/2011_saito_simple.test
@@ -4,7 +4,7 @@
topics:
@@ -13,4 +13,16 @@
sequence_action: ['(KNOCK DOOR)', '(MOVE ROOM-FRONT ROOM-INSIDE)', '(WIPE DESK)']
+
+
+
+ topics:
+ - name: /server_name/smach/container_structure
+ timeout: 10
+ children: ['(knock door)', '(move room-front room-inside)', '(wipe desk)']
+ container_outcomes: ['goal0']
+
+
diff --git a/pddl/pddl_planner/test/2013_fridge_demo.test b/pddl/pddl_planner/test/2013_fridge_demo.test
index 20ac83f3..ddb17a00 100644
--- a/pddl/pddl_planner/test/2013_fridge_demo.test
+++ b/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)']
+
+
+
+ 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']
+
+
diff --git a/pddl/pddl_planner/test/search_object.test b/pddl/pddl_planner/test/search_object.test
index 2b88cc5b..a38a7963 100644
--- a/pddl/pddl_planner/test/search_object.test
+++ b/pddl/pddl_planner/test/search_object.test
@@ -1,6 +1,6 @@
-
+
@@ -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)']
+
+
+
+ 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']
+
+
diff --git a/pddl/pddl_planner/test/simple_failure_torelant.test b/pddl/pddl_planner/test/simple_failure_torelant.test
index ccf7827f..d7d2a60d 100644
--- a/pddl/pddl_planner/test/simple_failure_torelant.test
+++ b/pddl/pddl_planner/test/simple_failure_torelant.test
@@ -17,4 +17,16 @@
sequence_action: ['(move-to elevator)', '(look elevator-button)', '(push-button elevator-button)']
+
+
+
+ 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']
+
+
diff --git a/pddl/pddl_planner/test/structuretest b/pddl/pddl_planner/test/structuretest
new file mode 100755
index 00000000..d9ec79bf
--- /dev/null
+++ b/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:
+
+
+
+ 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
+
+
+
+Author: Kentaro Wada
+Modified for SmachContainerStructure by
+"""
+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)