Add complex_recovery package

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()

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_diagrams](https://user-images.githubusercontent.com/9410362/189815175-a3265d23-01d4-4ae9-831c-b01aacad2872.png)
+
+## 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.

jsk_robot_common/complex_recovery/complex_recovery_plugins.xml

@@ -0,0 +1,18 @@
+<library path="lib/libcomplex_recovery">
+    <class
+        name="complex_recovery/SequentialComplexRecovery"
+        type="complex_recovery::SequentialComplexRecovery"
+        base_class_type="nav_core::RecoveryBehavior">
+        <description>
+            A recovery behavior that runs other recovery behaviors sequentially.
+        </description>
+    </class>
+    <class
+        name="complex_recovery/ParallelComplexRecovery"
+        type="complex_recovery::ParallelComplexRecovery"
+        base_class_type="nav_core::RecoveryBehavior">
+        <description>
+            A recovery behavior that runs other recovery behaviors in parallel.
+        </description>
+    </class>
+</library>

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 <nav_core/recovery_behavior.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <tf2_ros/buffer.h>
+#include <actionlib/client/simple_action_client.h>
+
+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<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;
+    std::vector<std::string> recovery_behavior_names_;
+    pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
+};
+};
+
+#endif

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 <nav_core/recovery_behavior.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <tf2_ros/buffer.h>
+#include <pluginlib/class_loader.hpp>
+
+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<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;
+    std::vector<std::string> recovery_behavior_names_;
+    pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
+};
+};
+
+#endif

jsk_robot_common/complex_recovery/include/complex_recovery/utils.h

@@ -0,0 +1,21 @@
+#include <pluginlib/class_list_macros.h>
+#include <nav_core/recovery_behavior.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <tf2_ros/buffer.h>
+#include <pluginlib/class_loader.hpp>
+
+namespace complex_recovery
+{
+
+
+bool loadRecoveryBehaviors(
+        std::string parent_name,
+        ros::NodeHandle& node,
+        pluginlib::ClassLoader<nav_core::RecoveryBehavior>& recovery_loader,
+        std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> >& recovery_behaviors,
+        std::vector<std::string>& recovery_behavior_names,
+        tf2_ros::Buffer* ptr_tf_buffer,
+        costmap_2d::Costmap2DROS* ptr_global_costmap,
+        costmap_2d::Costmap2DROS* ptr_local_costmap
+        );
+};

jsk_robot_common/complex_recovery/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>complex_recovery</name>
+  <version>1.1.0</version>
+  <description>The complex_recovery package</description>
+
+  <author email="shinjo@jsk.imi.i.u-tokyo.ac.jp">Koki Shinjo</author>
+  <maintainer email="k-okada@jsk.t.u-tokyo.ac.jp">Kei Okada</maintainer>
+  <maintainer email="shinjo@jsk.imi.i.u-tokyo.ac.jp">Koki Shinjo</maintainer>
+
+  <license>BSD</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>costmap_2d</depend>
+  <depend>nav_core</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>rostest</test_depend>
+  <test_depend>speak_and_wait_recovery</test_depend>
+  <test_depend>sound_play</test_depend>
+
+  <export>
+      <nav_core plugin="${prefix}/complex_recovery_plugins.xml" />
+  </export>
+</package>

jsk_robot_common/complex_recovery/src/parallel_complex_recovery.cpp

@@ -0,0 +1,84 @@
+#include <complex_recovery/parallel_complex_recovery.h>
+#include <complex_recovery/utils.h>
+#include <pluginlib/class_list_macros.h>
+#include <thread>
+
+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<std::shared_ptr<std::thread>> 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<std::thread>(
+                  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.");
+}
+
+};