diff --git a/jsk_robot_common/complex_recovery/CMakeLists.txt b/jsk_robot_common/complex_recovery/CMakeLists.txt
new file mode 100644
index 0000000000..7dd42684f9
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/CMakeLists.txt
@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(complex_recovery)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED
+ COMPONENTS
+ costmap_2d
+ nav_core
+ pluginlib
+ roscpp
+ tf2
+ tf2_ros
+)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES complex_recovery
+ CATKIN_DEPENDS
+ costmap_2d
+ nav_core
+ pluginlib
+ roscpp
+ tf2
+ tf2_ros
+)
+
+# Abort if indigo or kinetic
+if ( $ENV{ROS_DISTRO} STREQUAL "indigo" OR $ENV{ROS_DISTRO} STREQUAL "kinetic" )
+ return()
+endif()
+
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+add_library(complex_recovery
+ src/sequential_complex_recovery.cpp
+ src/parallel_complex_recovery.cpp
+ src/utils.cpp
+)
+add_dependencies(complex_recovery
+ ${${PROJECT_NAME}_EXPORTED_TARGETS}
+ ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(complex_recovery
+ ${catkin_LIBRARIES}
+)
+
+install(TARGETS complex_recovery
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+ )
+
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h"
+)
+
+install(FILES complex_recovery_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#
+# Testing
+#
+if (CATKIN_ENABLE_TESTING)
+ find_package(rostest REQUIRED)
+
+ catkin_add_executable_with_gtest(sequential_complex_recovery_test_node
+ tests/sequential_complex_recovery_test_node.cpp
+ )
+ target_link_libraries(sequential_complex_recovery_test_node
+ ${catkin_LIBRARIES}
+ ${PROJECT_NAME}
+ )
+ add_rostest(tests/sequential_complex_recovery.test)
+
+ catkin_add_executable_with_gtest(parallel_complex_recovery_test_node
+ tests/parallel_complex_recovery_test_node.cpp
+ )
+ target_link_libraries(parallel_complex_recovery_test_node
+ ${catkin_LIBRARIES}
+ ${PROJECT_NAME}
+ )
+ add_rostest(tests/parallel_complex_recovery.test)
+endif()
diff --git a/jsk_robot_common/complex_recovery/README.md b/jsk_robot_common/complex_recovery/README.md
new file mode 100644
index 0000000000..fc65f10920
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/README.md
@@ -0,0 +1,67 @@
+# complex_recovery
+
+This package provides recovery behavior plugins which combines multi recoveries to one recovery behavior.
+This is useful for assuring a set of recovery behavior to run at one time.
+
+There are two types of recoveries. One is to run multi recoveries sequentially and another is to run them in parallel.
+
+
+
+## complex_recovery/SequentialComplexRecovery
+
+* `~recovery_behaviors` (list of dictionaries which has `name` and `type` entries, default: None)
+
+Example configuration of `move_base` is like
+
+```yaml
+recovery_behavior_enabled: true
+recovery_behaviors:
+ - name: "speak_then_clear_costmap0"
+ type: "complex_recovery/SequentialComplexRecovery"
+speak_then_clear_costmap0:
+ recovery_behaviors:
+ - name: "speak_and_wait0"
+ type: "speak_and_wait_recovery/SpeakAndWaitRecovery"
+ - name: "clear_costmap0"
+ type: "clear_costmap_recovery/ClearCostmapRecovery"
+ speak_and_wait0:
+ speak_text: "I'm clearing costmap."
+ duration_wait: 5.0
+ duration_timeout: 1.0
+ sound_action: /sound_play
+ clear_costmap0:
+ reset_distance: 1.0
+```
+
+In this case, `speak_and_clear_costmap0` recovery runs `speak_and_wait0` recovery first, then runs `clear_costmap0`.
+So a robot speaks first and then clear its costmap.
+
+## complex_recovery/ParallelComplexRecovery
+
+* `~recovery_behaviors` (list of dictionaries which has `name` and `type` entries, default: None)
+
+Example configuration of `move_base` is like
+
+```yaml
+recovery_behavior_enabled: true
+recovery_behaviors:
+ - name: "speak_and_rotate_costmap0"
+ type: "complex_recovery/SequentialComplexRecovery"
+speak_and_rotate_costmap0:
+ recovery_behaviors:
+ - name: "speak_and_wait0"
+ type: "speak_and_wait_recovery/SpeakAndWaitRecovery"
+ - name: "rotate0"
+ type: "rotate_recovery/RotateRecovery"
+ speak_and_wait0:
+ speak_text: "I'm rotating."
+ duration_wait: 5.0
+ duration_timeout: 1.0
+ sound_action: /sound_play
+ rotate0:
+ sim_granularity: 0.017
+ frequency: 20.0
+```
+
+In this case, `speak_and_rotate_costmap0` recovery runs `speak_and_wait0` and `rotate0` simultaneously.
+So a robot speaks during its rotation.
diff --git a/jsk_robot_common/complex_recovery/complex_recovery_plugins.xml b/jsk_robot_common/complex_recovery/complex_recovery_plugins.xml
new file mode 100644
index 0000000000..52d7a4b4d5
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/complex_recovery_plugins.xml
@@ -0,0 +1,18 @@
+
+
+
+ A recovery behavior that runs other recovery behaviors sequentially.
+
+
+
+
+ A recovery behavior that runs other recovery behaviors in parallel.
+
+
+
diff --git a/jsk_robot_common/complex_recovery/include/complex_recovery/parallel_complex_recovery.h b/jsk_robot_common/complex_recovery/include/complex_recovery/parallel_complex_recovery.h
new file mode 100644
index 0000000000..fc56d9c217
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/include/complex_recovery/parallel_complex_recovery.h
@@ -0,0 +1,32 @@
+#ifndef COMPLEX_RECOVERY_H
+#define COMPLEX_RECOVERY_H
+
+#include
+#include
+#include
+#include
+
+namespace complex_recovery
+{
+
+class ParallelComplexRecovery : public nav_core::RecoveryBehavior
+{
+public:
+ ParallelComplexRecovery();
+ void initialize(
+ std::string name,
+ tf2_ros::Buffer* tf_buffer,
+ costmap_2d::Costmap2DROS* global_costmap,
+ costmap_2d::Costmap2DROS* local_costmap);
+ void runBehavior();
+ ~ParallelComplexRecovery();
+
+private:
+ bool initialized_;
+ std::vector > recovery_behaviors_;
+ std::vector recovery_behavior_names_;
+ pluginlib::ClassLoader recovery_loader_;
+};
+};
+
+#endif
diff --git a/jsk_robot_common/complex_recovery/include/complex_recovery/sequential_complex_recovery.h b/jsk_robot_common/complex_recovery/include/complex_recovery/sequential_complex_recovery.h
new file mode 100644
index 0000000000..507e0ea0ab
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/include/complex_recovery/sequential_complex_recovery.h
@@ -0,0 +1,32 @@
+#ifndef COMPLEX_RECOVERY_H
+#define COMPLEX_RECOVERY_H
+
+#include
+#include
+#include
+#include
+
+namespace complex_recovery
+{
+
+class SequentialComplexRecovery : public nav_core::RecoveryBehavior
+{
+public:
+ SequentialComplexRecovery();
+ void initialize(
+ std::string name,
+ tf2_ros::Buffer*,
+ costmap_2d::Costmap2DROS* global_costmap,
+ costmap_2d::Costmap2DROS* local_costmap);
+ void runBehavior();
+ ~SequentialComplexRecovery();
+
+private:
+ bool initialized_;
+ std::vector > recovery_behaviors_;
+ std::vector recovery_behavior_names_;
+ pluginlib::ClassLoader recovery_loader_;
+};
+};
+
+#endif
diff --git a/jsk_robot_common/complex_recovery/include/complex_recovery/utils.h b/jsk_robot_common/complex_recovery/include/complex_recovery/utils.h
new file mode 100644
index 0000000000..a02ee51eec
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/include/complex_recovery/utils.h
@@ -0,0 +1,21 @@
+#include
+#include
+#include
+#include
+#include
+
+namespace complex_recovery
+{
+
+
+bool loadRecoveryBehaviors(
+ std::string parent_name,
+ ros::NodeHandle& node,
+ pluginlib::ClassLoader& recovery_loader,
+ std::vector >& recovery_behaviors,
+ std::vector& recovery_behavior_names,
+ tf2_ros::Buffer* ptr_tf_buffer,
+ costmap_2d::Costmap2DROS* ptr_global_costmap,
+ costmap_2d::Costmap2DROS* ptr_local_costmap
+ );
+};
diff --git a/jsk_robot_common/complex_recovery/package.xml b/jsk_robot_common/complex_recovery/package.xml
new file mode 100644
index 0000000000..1892a9fc61
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/package.xml
@@ -0,0 +1,30 @@
+
+
+ complex_recovery
+ 1.1.0
+ The complex_recovery package
+
+ Koki Shinjo
+ Kei Okada
+ Koki Shinjo
+
+ BSD
+
+
+ catkin
+
+ costmap_2d
+ nav_core
+ pluginlib
+ roscpp
+ tf2
+ tf2_ros
+
+ rostest
+ speak_and_wait_recovery
+ sound_play
+
+
+
+
+
diff --git a/jsk_robot_common/complex_recovery/src/parallel_complex_recovery.cpp b/jsk_robot_common/complex_recovery/src/parallel_complex_recovery.cpp
new file mode 100644
index 0000000000..80425e0f1b
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/src/parallel_complex_recovery.cpp
@@ -0,0 +1,84 @@
+#include
+#include
+#include
+#include
+
+PLUGINLIB_EXPORT_CLASS(complex_recovery::ParallelComplexRecovery, nav_core::RecoveryBehavior)
+
+namespace complex_recovery
+{
+
+ParallelComplexRecovery::ParallelComplexRecovery():
+ initialized_(false),
+ recovery_loader_("nav_core", "nav_core::RecoveryBehavior")
+{
+}
+
+void ParallelComplexRecovery::initialize(
+ std::string name,
+ tf2_ros::Buffer* tf_buffer,
+ costmap_2d::Costmap2DROS* global_costmap,
+ costmap_2d::Costmap2DROS* local_costmap)
+{
+ if (not initialized_) {
+ ros::NodeHandle private_nh("~/" + name);
+ bool success = loadRecoveryBehaviors(
+ name,
+ private_nh,
+ recovery_loader_,
+ recovery_behaviors_,
+ recovery_behavior_names_,
+ tf_buffer,
+ global_costmap,
+ local_costmap
+ );
+ if ( not success ) {
+ ROS_ERROR("Failed to load behaviors.");
+ } else {
+ ROS_INFO("Behaviors are loaded.");
+ for (auto behavior_name = recovery_behavior_names_.begin(); behavior_name != recovery_behavior_names_.end(); behavior_name++) {
+ ROS_INFO("behavior: %s", behavior_name->c_str());
+ }
+ }
+
+ initialized_ = true;
+ } else {
+ ROS_ERROR("You should not call initialize twice on this object, doing nothing");
+ }
+}
+
+ParallelComplexRecovery::~ParallelComplexRecovery()
+{
+ recovery_behaviors_.clear();
+}
+
+void ParallelComplexRecovery::runBehavior()
+{
+ if (not initialized_)
+ {
+ ROS_ERROR("This object must be initialized before runBehavior is called");
+ return;
+ }
+
+ std::vector> threads;
+
+ ROS_INFO("Start executing behaviors in parallel.");
+ for (auto index = 0; index < recovery_behaviors_.size(); index++) {
+ ROS_INFO("start executing behavior %s", recovery_behavior_names_[index].c_str());
+ threads.push_back(
+ std::shared_ptr(
+ new std::thread(
+ &nav_core::RecoveryBehavior::runBehavior,
+ recovery_behaviors_[index]
+ )));
+ }
+
+ ROS_INFO("Waiting for behaviors to finish.");
+ for (auto thread = threads.begin(); thread != threads.end(); thread++) {
+ (*thread)->join();
+ }
+
+ ROS_INFO("All behaviors have finished.");
+}
+
+};
diff --git a/jsk_robot_common/complex_recovery/src/sequential_complex_recovery.cpp b/jsk_robot_common/complex_recovery/src/sequential_complex_recovery.cpp
new file mode 100644
index 0000000000..4a71196ea1
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/src/sequential_complex_recovery.cpp
@@ -0,0 +1,71 @@
+#include
+#include
+#include
+
+PLUGINLIB_EXPORT_CLASS(complex_recovery::SequentialComplexRecovery, nav_core::RecoveryBehavior)
+
+namespace complex_recovery
+{
+
+SequentialComplexRecovery::SequentialComplexRecovery():
+ initialized_(false),
+ recovery_loader_("nav_core", "nav_core::RecoveryBehavior")
+{
+}
+
+void SequentialComplexRecovery::initialize(
+ std::string name,
+ tf2_ros::Buffer* tf_buffer,
+ costmap_2d::Costmap2DROS* global_costmap,
+ costmap_2d::Costmap2DROS* local_costmap)
+{
+ if (not initialized_) {
+ ros::NodeHandle private_nh("~/" + name);
+ bool success = loadRecoveryBehaviors(
+ name,
+ private_nh,
+ recovery_loader_,
+ recovery_behaviors_,
+ recovery_behavior_names_,
+ tf_buffer,
+ global_costmap,
+ local_costmap
+ );
+ if ( not success ) {
+ ROS_ERROR("Failed to load behaviors.");
+ } else {
+ ROS_INFO("Behaviors are loaded.");
+ for (auto behavior_name = recovery_behavior_names_.begin(); behavior_name != recovery_behavior_names_.end(); behavior_name++) {
+ ROS_INFO("behavior: %s", behavior_name->c_str());
+ }
+ }
+
+ initialized_ = true;
+ } else {
+ ROS_ERROR("You should not call initialize twice on this object, doing nothing");
+ }
+}
+
+SequentialComplexRecovery::~SequentialComplexRecovery()
+{
+ recovery_behaviors_.clear();
+}
+
+void SequentialComplexRecovery::runBehavior()
+{
+ if (not initialized_)
+ {
+ ROS_ERROR("This object must be initialized before runBehavior is called");
+ return;
+ }
+
+ ROS_INFO("Start executing behaviors sequentially.");
+ for (auto index = 0; index < recovery_behaviors_.size(); index++) {
+ ROS_INFO("executing behavior %s", recovery_behavior_names_[index].c_str());
+ recovery_behaviors_[index]->runBehavior();
+ }
+
+ ROS_INFO("All behaviors have finished.");
+}
+
+};
diff --git a/jsk_robot_common/complex_recovery/src/utils.cpp b/jsk_robot_common/complex_recovery/src/utils.cpp
new file mode 100644
index 0000000000..660d91007f
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/src/utils.cpp
@@ -0,0 +1,108 @@
+#include
+
+namespace complex_recovery
+{
+
+/**
+ *
+ * Almost copied from https://github.com/ros-planning/navigation/blob/47c9c629fc93e6c6bbcd3109eb2d5b55efce400d/move_base/src/move_base.cpp#L1019-L1102
+ */
+bool loadRecoveryBehaviors(
+ std::string parent_name,
+ ros::NodeHandle& node,
+ pluginlib::ClassLoader& recovery_loader,
+ std::vector >& recovery_behaviors,
+ std::vector& recovery_behavior_names,
+ tf2_ros::Buffer* ptr_tf_buffer,
+ costmap_2d::Costmap2DROS* ptr_global_costmap,
+ costmap_2d::Costmap2DROS* ptr_local_costmap
+ )
+{
+ XmlRpc::XmlRpcValue behavior_list;
+ if(node.getParam("recovery_behaviors", behavior_list)){
+ if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){
+ for(int i = 0; i < behavior_list.size(); ++i){
+ if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){
+ if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){
+ //check for recovery behaviors with the same name
+ for(int j = i + 1; j < behavior_list.size(); j++){
+ if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){
+ if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){
+ std::string name_i = behavior_list[i]["name"];
+ std::string name_j = behavior_list[j]["name"];
+ if(name_i == name_j){
+ ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.",
+ name_i.c_str());
+ return false;
+ }
+ }
+ }
+ }
+ }
+ else{
+ ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
+ return false;
+ }
+ }
+ else{
+ ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
+ behavior_list[i].getType());
+ return false;
+ }
+ }
+
+ //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
+ for(int i = 0; i < behavior_list.size(); ++i){
+ try{
+ //check if a non fully qualified name has potentially been passed in
+ if(!recovery_loader.isClassAvailable(behavior_list[i]["type"])){
+ std::vector classes = recovery_loader.getDeclaredClasses();
+ for(unsigned int i = 0; i < classes.size(); ++i){
+ if(behavior_list[i]["type"] == recovery_loader.getName(classes[i])){
+ //if we've found a match... we'll get the fully qualified name and break out of the loop
+ ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
+ std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
+ behavior_list[i]["type"] = classes[i];
+ break;
+ }
+ }
+ }
+
+ boost::shared_ptr behavior(recovery_loader.createInstance(behavior_list[i]["type"]));
+
+ //shouldn't be possible, but it won't hurt to check
+ if(behavior.get() == NULL){
+ ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
+ return false;
+ }
+
+ //initialize the recovery behavior with its name
+ std::string base_name = behavior_list[i]["name"];
+ behavior->initialize(
+ parent_name + "/" + base_name,
+ ptr_tf_buffer, ptr_global_costmap, ptr_local_costmap);
+ recovery_behavior_names.push_back(behavior_list[i]["name"]);
+ recovery_behaviors.push_back(behavior);
+ }
+ catch(pluginlib::PluginlibException& ex){
+ ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
+ return false;
+ }
+ }
+ }
+ else{
+ ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
+ behavior_list.getType());
+ return false;
+ }
+ }
+ else{
+ //if no recovery_behaviors are specified, we'll just load the defaults
+ return false;
+ }
+
+ //if we've made it here... we've constructed a recovery behavior list successfully
+ return true;
+}
+
+};
diff --git a/jsk_robot_common/complex_recovery/tests/parallel_complex_recovery.test b/jsk_robot_common/complex_recovery/tests/parallel_complex_recovery.test
new file mode 100644
index 0000000000..17dc02c20c
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/tests/parallel_complex_recovery.test
@@ -0,0 +1,56 @@
+
+
+
+ recovery:
+ recovery_behaviors:
+ - name: 'wait_and_speak_complex0'
+ type: 'complex_recovery/SequentialComplexRecovery'
+ - name: 'wait_and_speak_complex1'
+ type: 'complex_recovery/SequentialComplexRecovery'
+ wait_and_speak_complex0:
+ recovery_behaviors:
+ - name: 'wait_3_secs'
+ type: 'speak_and_wait_recovery/SpeakAndWaitRecovery'
+ - name: 'speak_and_wait0'
+ type: 'speak_and_wait_recovery/SpeakAndWaitRecovery'
+ wait_3_secs:
+ speak_text: ''
+ duration_wait: 3.0
+ duration_timeout: 2.0
+ result_timeout: 1.0
+ speak_and_wait0:
+ speak_text: 'test0'
+ duration_wait: 2.0
+ duration_timeout: 2.0
+ result_timeout: 1.0
+ wait_and_speak_complex1:
+ recovery_behaviors:
+ - name: 'wait_5_secs'
+ type: 'speak_and_wait_recovery/SpeakAndWaitRecovery'
+ - name: 'speak_and_wait1'
+ type: 'speak_and_wait_recovery/SpeakAndWaitRecovery'
+ wait_5_secs:
+ speak_text: ''
+ duration_wait: 5.0
+ duration_timeout: 2.0
+ result_timeout: 1.0
+ speak_and_wait1:
+ speak_text: 'test1'
+ duration_wait: 2.0
+ duration_timeout: 2.0
+ result_timeout: 1.0
+
+
+
+
+
diff --git a/jsk_robot_common/complex_recovery/tests/parallel_complex_recovery_test_node.cpp b/jsk_robot_common/complex_recovery/tests/parallel_complex_recovery_test_node.cpp
new file mode 100644
index 0000000000..3d54f9cad7
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/tests/parallel_complex_recovery_test_node.cpp
@@ -0,0 +1,21 @@
+#include
+#include "complex_recovery/parallel_complex_recovery.h"
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "complex_recovery_simple_node");
+ ros::NodeHandle nh;
+
+ complex_recovery::ParallelComplexRecovery recovery;
+ recovery.initialize(std::string("recovery"),NULL,NULL,NULL);
+
+ ros::Rate rate(10);
+ while (ros::ok()) {
+ ROS_WARN("Spoken a test");
+ recovery.runBehavior();
+ ros::spinOnce();
+ rate.sleep();
+ }
+
+ return 0;
+}
diff --git a/jsk_robot_common/complex_recovery/tests/sequential_complex_recovery.test b/jsk_robot_common/complex_recovery/tests/sequential_complex_recovery.test
new file mode 100644
index 0000000000..78bfd748f2
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/tests/sequential_complex_recovery.test
@@ -0,0 +1,34 @@
+
+
+
+ recovery:
+ recovery_behaviors:
+ - name: 'speak_and_wait0'
+ type: 'speak_and_wait_recovery/SpeakAndWaitRecovery'
+ - name: 'speak_and_wait1'
+ type: 'speak_and_wait_recovery/SpeakAndWaitRecovery'
+ speak_and_wait0:
+ speak_text: 'test0'
+ duration_wait: 2.0
+ duration_timeout: 2.0
+ result_timeout: 1.0
+ speak_and_wait1:
+ speak_text: 'test1'
+ duration_wait: 2.0
+ duration_timeout: 2.0
+ result_timeout: 1.0
+
+
+
+
+
diff --git a/jsk_robot_common/complex_recovery/tests/sequential_complex_recovery_test_node.cpp b/jsk_robot_common/complex_recovery/tests/sequential_complex_recovery_test_node.cpp
new file mode 100644
index 0000000000..618ff5656c
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/tests/sequential_complex_recovery_test_node.cpp
@@ -0,0 +1,21 @@
+#include
+#include "complex_recovery/sequential_complex_recovery.h"
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "complex_recovery_simple_node");
+ ros::NodeHandle nh;
+
+ complex_recovery::SequentialComplexRecovery recovery;
+ recovery.initialize(std::string("recovery"),NULL,NULL,NULL);
+
+ ros::Rate rate(10);
+ while (ros::ok()) {
+ ROS_WARN("Spoken a test");
+ recovery.runBehavior();
+ ros::spinOnce();
+ rate.sleep();
+ }
+
+ return 0;
+}
diff --git a/jsk_robot_common/complex_recovery/tests/test_parallel_complex_recovery.py b/jsk_robot_common/complex_recovery/tests/test_parallel_complex_recovery.py
new file mode 100755
index 0000000000..8d7fea0de8
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/tests/test_parallel_complex_recovery.py
@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+
+from sound_play.msg import SoundRequestResult
+from sound_play.msg import SoundRequestAction
+import rospy
+import actionlib
+import threading
+import sys
+import unittest
+import rostest
+
+PKG = 'complex_recovery'
+NAME = 'parallel_complex_recovery_test'
+
+
+class TestParallelComplexRecovery(unittest.TestCase):
+
+ def __init__(self, *args):
+ super(self.__class__, self).__init__(*args)
+ rospy.init_node(NAME)
+
+ self.lock_for_msg = threading.Lock()
+ self.speech_text_list = []
+
+ self.action_server = \
+ actionlib.SimpleActionServer(
+ '/sound_play', SoundRequestAction, execute_cb=self.handler, auto_start=False)
+ self.action_server.start()
+
+ def handler(self, goal):
+
+ with self.lock_for_msg:
+ self.speech_text_list.append(goal.sound_request.arg)
+ rospy.logwarn('Receive a message')
+ self.action_server.set_succeeded(
+ SoundRequestResult(playing=True, stamp=rospy.Time.now()))
+
+ def test_parallel_complex_recovery(self):
+
+ rate = rospy.Rate(1)
+ while not rospy.is_shutdown():
+ with self.lock_for_msg:
+ if 'test0' in self.speech_text_list and 'test1' in self.speech_text_list:
+ break
+ else:
+ rospy.logwarn('waiting')
+ rate.sleep()
+
+ with self.lock_for_msg:
+ rospy.logwarn('speech_text_list: {}'.format(self.speech_text_list))
+ self.assertTrue('test0' in self.speech_text_list and 'test1' in self.speech_text_list)
+
+
+if __name__ == '__main__':
+ rostest.rosrun(PKG, NAME, TestParallelComplexRecovery, sys.argv)
diff --git a/jsk_robot_common/complex_recovery/tests/test_sequential_complex_recovery.py b/jsk_robot_common/complex_recovery/tests/test_sequential_complex_recovery.py
new file mode 100755
index 0000000000..4dce3752b1
--- /dev/null
+++ b/jsk_robot_common/complex_recovery/tests/test_sequential_complex_recovery.py
@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+
+from sound_play.msg import SoundRequestResult
+from sound_play.msg import SoundRequestAction
+import rospy
+import actionlib
+import threading
+import sys
+import unittest
+import rostest
+
+PKG = 'complex_recovery'
+NAME = 'sequential_complex_recovery_test'
+
+
+class TestSequentialComplexRecovery(unittest.TestCase):
+
+ def __init__(self, *args):
+ super(self.__class__, self).__init__(*args)
+ rospy.init_node(NAME)
+
+ self.lock_for_msg = threading.Lock()
+ self.speech_text_list = []
+
+ self.action_server = \
+ actionlib.SimpleActionServer(
+ '/sound_play', SoundRequestAction, execute_cb=self.handler, auto_start=False)
+ self.action_server.start()
+
+ def handler(self, goal):
+
+ with self.lock_for_msg:
+ self.speech_text_list.append(goal.sound_request.arg)
+ rospy.logwarn('Receive a message')
+ self.action_server.set_succeeded(
+ SoundRequestResult(playing=True, stamp=rospy.Time.now()))
+
+ def test_sequential_complex_recovery(self):
+
+ rate = rospy.Rate(1)
+ while not rospy.is_shutdown():
+ with self.lock_for_msg:
+ if 'test0' in self.speech_text_list and 'test1' in self.speech_text_list:
+ break
+ else:
+ rospy.logwarn('waiting')
+ rate.sleep()
+
+ with self.lock_for_msg:
+ rospy.logwarn('speech_text_list: {}'.format(self.speech_text_list))
+ self.assertTrue('test0' in self.speech_text_list and 'test1' in self.speech_text_list)
+
+
+if __name__ == '__main__':
+ rostest.rosrun(PKG, NAME, TestSequentialComplexRecovery, sys.argv)