gpp_plugin.hpp

Go to the documentation of this file.

/*
 * MIT License
 *
 * Copyright (c) 2020 Dima Dorezyuk
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
 
#pragma once
 
#include <gpp_interface/post_planning_interface.hpp>
#include <gpp_interface/pre_planning_interface.hpp>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mbf_costmap_core/costmap_planner.h>
#include <nav_core/base_global_planner.h>
#include <pluginlib/class_loader.h>
#include <ros/ros.h>
 
#include <memory>
#include <string>
#include <utility>
#include <vector>
 
namespace gpp_plugin {
 
template <typename _Plugin>
struct PluginDefinition {
  using type = _Plugin;
  static const std::string package;
  static const std::string base_class;
};
 
// define shortcuts to the resource types
using gpp_interface::PostPlanningInterface;
using gpp_interface::PrePlanningInterface;
using mbf_costmap_core::CostmapPlanner;
using nav_core::BaseGlobalPlanner;
 
// clang-format off
 
// below the definition of the different plugins.
// we put it into the header, since this is part of the interface.
// extend here, if you want to load other types.
 
// Preplanning specialization
template <>
const std::string PluginDefinition<PrePlanningInterface>::package = "gpp_interface";
 
template <>
const std::string PluginDefinition<PrePlanningInterface>::base_class = "gpp_interface::PrePlanningInterface";
 
// Preplanning specialization
template <>
const std::string PluginDefinition<PostPlanningInterface>::package = "gpp_interface";
 
template <>
const std::string PluginDefinition<PostPlanningInterface>::base_class = "gpp_interface::PostPlanningInterface";
 
// nav-core specialization
template <>
const std::string PluginDefinition<BaseGlobalPlanner>::package = "nav_core";
 
template <>
const std::string PluginDefinition<BaseGlobalPlanner>::base_class = "nav_core::BaseGlobalPlanner";
 
// mbf-costmap-core specialization
template <>
const std::string PluginDefinition<CostmapPlanner>::package = "mbf_costmap_core";
 
template <>
const std::string PluginDefinition<CostmapPlanner>::base_class = "mbf_costmap_core::CostmapPlanner";
 
// clang-format on
 
// below the plugin loading machinery. the implementation is moved into cpp,
// since we provide the (only valid) template arguments at compile time.
 
template <typename _Plugin>
struct PluginManager : public pluginlib::ClassLoader<_Plugin> {
  // the defintion with compile-time package and base_class tags
  using Definition = PluginDefinition<_Plugin>;
  using Base = pluginlib::ClassLoader<_Plugin>;
 
  // this will get you a linker error, if you try to load unknown plugins
  PluginManager() : Base(Definition::package, Definition::base_class) {}
 
  virtual pluginlib::UniquePtr<_Plugin>
  createCustomInstance(const std::string& _type) {
    return Base::createUniqueInstance(_type);
  }
};
 
struct PluginParameter {
  std::string name;
  bool on_success_break = false;
  bool on_failure_break = true;
};
 
template <typename _Plugin>
struct PluginGroup {
  // this class owns the plugin
  using PluginPtr = typename pluginlib::UniquePtr<_Plugin>;
 
  using NamedPlugin = std::pair<PluginParameter, PluginPtr>;
  using PluginMap = std::vector<NamedPlugin>;
 
  inline const PluginMap&
  getPlugins() const noexcept {
    return plugins_;
  }
 
  inline const std::string&
  getName() const noexcept {
    return name_;
  }
 
  inline const bool&
  getDefaultValue() const noexcept {
    return default_value_;
  }
 
protected:
  bool default_value_;
  std::string name_ = "undefined";
  PluginMap plugins_;
};
 
template <typename _Plugin, typename _Functor>
bool
_runPlugins(const PluginGroup<_Plugin>& _grp, const _Functor& _func,
            const std::atomic_bool& _cancel) {
  const auto& plugins = _grp.getPlugins();
  const std::string name = "[" + _grp.getName() + "]: ";
  for (const auto& plugin : plugins) {
    // allow the user to cancel the job
    if (_cancel) {
      ROS_INFO_STREAM(name << "cancelled");
      return false;
    }
 
    // tell my name
    ROS_INFO_STREAM(name << "runs " << plugin.first.name);
 
    // run the impl, but don't die
    if (!plugin.second || !_func(*plugin.second)) {
      // we have failed - we can either abort or ignore
      ROS_WARN_STREAM(name << "failed at " << plugin.first.name);
      if (plugin.first.on_failure_break)
        return false;
    }
    else if (plugin.first.on_success_break)
      return true;
  }
  return _grp.getDefaultValue();
}
 
template <typename _Plugin, typename _Functor>
bool
runPlugins(const PluginGroup<_Plugin>& _grp, const _Functor& _func,
           const std::atomic_bool& _cancel) {
  const auto result = _runPlugins(_grp, _func, _cancel);
  // print a conditional warning
  ROS_WARN_STREAM_COND(!result, "[gpp]: failed at group " << _grp.getName());
  return result;
}
template <typename _Plugin>
struct ArrayPluginManager : public PluginManager<_Plugin>,
                            public PluginGroup<_Plugin> {
  void
  load(const std::string& _resource, ros::NodeHandle& _nh);
};
 
// compile time specification of the ArrayPluginManager
using PrePlanningManager = ArrayPluginManager<PrePlanningInterface>;
using PostPlanningManager = ArrayPluginManager<PostPlanningInterface>;
using GlobalPlannerManager = ArrayPluginManager<BaseGlobalPlanner>;
 
struct BaseGlobalPlannerWrapper : public BaseGlobalPlanner {
  // define the interface types
  using Pose = geometry_msgs::PoseStamped;
  using Path = std::vector<Pose>;
  using Map = costmap_2d::Costmap2DROS;
  using ImplPlanner = pluginlib::UniquePtr<CostmapPlanner>;
 
  explicit BaseGlobalPlannerWrapper(ImplPlanner&& _impl);
 
  bool
  makePlan(const Pose& start, const Pose& goal, Path& plan) override;
 
  bool
  makePlan(const Pose& start, const Pose& goal, Path& plan,
           double& cost) override;
 
  void
  initialize(std::string name, Map* costmap_ros) override;
 
private:
  ImplPlanner impl_;
};
 
struct CostmapPlannerManager : public GlobalPlannerManager {
  ~CostmapPlannerManager();
 
  // dont call this yourself
  pluginlib::UniquePtr<BaseGlobalPlanner>
  createCustomInstance(const std::string& _type) override;
 
private:
  PluginManager<CostmapPlanner> manager_;
};
 
struct GppPlugin : public BaseGlobalPlanner, public CostmapPlanner {
  // define our interface types
  using Pose = geometry_msgs::PoseStamped;
  using Path = std::vector<Pose>;
  using Map = costmap_2d::Costmap2DROS;
 
  bool
  makePlan(const Pose& _start, const Pose& _goal, Path& _plan) override;
 
  bool
  makePlan(const Pose& _start, const Pose& _goal, Path& _plan,
           double& _cost) override;
 
  uint32_t
  makePlan(const Pose& start, const Pose& goal, double tolerance, Path& plan,
           double& cost, std::string& message) override;
  void
  initialize(std::string name, Map* costmap_ros) override;
 
  bool
  cancel() override;
 
private:
  bool
  prePlanning(Pose& _start, Pose& _goal);
 
  bool
  postPlanning(const Pose& _start, const Pose& _goal, Path& _path,
               double& _cost);
 
  bool
  globalPlanning(const Pose& _start, const Pose& _goal, Path& _plan,
                 double& _cost);
 
  std::atomic_bool cancel_;
 
  // nav_core conforming members
  std::string name_;
  Map* costmap_ = nullptr;
 
  PrePlanningManager pre_planning_;
  PostPlanningManager post_planning_;
  CostmapPlannerManager global_planning_;
};
 
}  // namespace gpp_plugin