Added a SBPL global planner module.

Added the behavior tree layers for the generic navigation and costmap modules and the ackermann local planner module.

include/iri_nav_module/nav_costmap_module_bt.h

+#ifndef _NAV_COSTMAP_MODULE_BT_H
+#define _NAV_COSTMAP_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <iri_nav_module/nav_costmap_module.h>
+
+template <class ModuleCfg>
+class CNavCostmapModuleBT
+{
+  private:
+    // object of nav_module
+    CNavCostmapModule<ModuleCfg> &module;
+    std::string name;
+
+  public:
+    /**
+      * \brief Constructor
+      *
+      */
+    CNavCostmapModuleBT(CNavCostmapModule<ModuleCfg> &module,const std::string &name);
+
+    /**
+      * \brief Register all conditions and actions needed for using the module
+      *
+      * This function registers all conditions and actions needed for using the module
+      * to the factory provided including all input and output ports required
+      * by actions.
+      *
+      * \param factory (IriBehaviorTreeFactory)
+      *
+      */
+    void init(IriBehaviorTreeFactory &factory);
+
+    /**
+    * \brief Destructor
+    *
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~CNavCostmapModuleBT();
+  protected:
+    // actions
+    BT::NodeStatus get_width(BT::TreeNode& self);
+    BT::NodeStatus set_width(BT::TreeNode& self);
+    BT::NodeStatus get_height(BT::TreeNode& self);
+    BT::NodeStatus set_height(BT::TreeNode& self);
+    BT::NodeStatus get_footprint(BT::TreeNode& self);
+    BT::NodeStatus set_footprint(BT::TreeNode& self);
+    BT::NodeStatus get_transform_tolerance(BT::TreeNode& self);
+    BT::NodeStatus set_transform_tolerance(BT::TreeNode& self);
+    BT::NodeStatus get_update_frequency(BT::TreeNode& self);
+    BT::NodeStatus set_update_frequency(BT::TreeNode& self);
+    BT::NodeStatus get_publish_frequency(BT::TreeNode& self);
+    BT::NodeStatus set_publish_frequency(BT::TreeNode& self);
+    BT::NodeStatus get_resolution(BT::TreeNode& self);
+    BT::NodeStatus set_resolution(BT::TreeNode& self);
+    BT::NodeStatus get_origin(BT::TreeNode& self);
+    BT::NodeStatus set_origin(BT::TreeNode& self);
+    BT::NodeStatus get_footprint_padding(BT::TreeNode& self);
+    BT::NodeStatus set_footprint_padding(BT::TreeNode& self);
+  };
+
+  template <class ModuleCfg>
+  CNavCostmapModuleBT<ModuleCfg>::CNavCostmapModuleBT(CNavCostmapModule<ModuleCfg> &module,const std::string &name) :
+    module(module)
+  {
+    std::size_t found = name.find_last_of("/\\");
+    this->name=name.substr(found+1);    
+  }
+
+  template <class ModuleCfg>
+  void CNavCostmapModuleBT<ModuleCfg>::init(IriBehaviorTreeFactory &factory)
+  {
+    //Definition of ports
+    BT::PortsList set_width_ports = {BT::InputPort<unsigned int>("width")};
+    BT::PortsList get_width_ports = {BT::OutputPort<unsigned int>("width")};
+    BT::PortsList set_height_ports = {BT::InputPort<unsigned int>("height")};
+    BT::PortsList get_height_ports = {BT::OutputPort<unsigned int>("height")};
+    BT::PortsList set_footprint_ports = {BT::InputPort<std::vector<geometry_msgs::Point>>("footprint")};
+    BT::PortsList get_footprint_ports = {BT::OutputPort<std::vector<geometry_msgs::Point>>("footprint")};
+    BT::PortsList set_transform_tolerance_ports = {BT::InputPort<double>("tolerance")};
+    BT::PortsList get_transform_tolerance_ports = {BT::OutputPort<double>("tolerance")};
+    BT::PortsList set_update_freq_ports = {BT::InputPort<double>("frequency")};
+    BT::PortsList get_update_freq_ports = {BT::OutputPort<double>("frequency")};
+    BT::PortsList set_publish_freq_ports = {BT::InputPort<double>("frequency")};
+    BT::PortsList get_publish_freq_ports = {BT::OutputPort<double>("frequency")};
+    BT::PortsList set_resolution_ports = {BT::InputPort<double>("resolution")};
+    BT::PortsList get_resolution_ports = {BT::OutputPort<double>("resolution")};
+    BT::PortsList set_origin_ports = {BT::InputPort<double>("x"),BT::InputPort<double>("y")};
+    BT::PortsList get_origin_ports = {BT::OutputPort<double>("x"),BT::InputPort<double>("y")};
+    BT::PortsList set_padding_ports = {BT::InputPort<double>("padding")};
+    BT::PortsList get_padding_ports = {BT::OutputPort<double>("padding")};
+
+    //Registration of actions
+    factory.registerSimpleAction(this->name+"_costmap_set_width", std::bind(&CNavCostmapModuleBT::set_width, this, std::placeholders::_1), set_width_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_width", std::bind(&CNavCostmapModuleBT::get_width, this, std::placeholders::_1), get_width_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_height", std::bind(&CNavCostmapModuleBT::set_height, this, std::placeholders::_1), set_height_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_height", std::bind(&CNavCostmapModuleBT::get_height, this, std::placeholders::_1), get_height_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_footprint", std::bind(&CNavCostmapModuleBT::set_footprint, this, std::placeholders::_1), set_footprint_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_footprint", std::bind(&CNavCostmapModuleBT::get_footprint, this, std::placeholders::_1), get_footprint_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_transform_tolerance", std::bind(&CNavCostmapModuleBT::set_transform_tolerance, this, std::placeholders::_1), set_transform_tolerance_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_transform_tolerance", std::bind(&CNavCostmapModuleBT::get_transform_tolerance, this, std::placeholders::_1), get_transform_tolerance_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_update_freq", std::bind(&CNavCostmapModuleBT::set_update_frequency, this, std::placeholders::_1), set_update_freq_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_update_freq", std::bind(&CNavCostmapModuleBT::get_update_frequency, this, std::placeholders::_1), get_update_freq_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_publish_freq", std::bind(&CNavCostmapModuleBT::set_publish_frequency, this, std::placeholders::_1), set_publish_freq_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_publish_freq", std::bind(&CNavCostmapModuleBT::get_publish_frequency, this, std::placeholders::_1), get_publish_freq_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_resolution", std::bind(&CNavCostmapModuleBT::set_resolution, this, std::placeholders::_1), set_resolution_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_resolution", std::bind(&CNavCostmapModuleBT::get_resolution, this, std::placeholders::_1), get_resolution_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_origin", std::bind(&CNavCostmapModuleBT::set_origin, this, std::placeholders::_1), set_origin_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_origin", std::bind(&CNavCostmapModuleBT::get_origin, this, std::placeholders::_1), get_origin_ports);
+    factory.registerSimpleAction(this->name+"_costmap_set_footprint_padding", std::bind(&CNavCostmapModuleBT::set_footprint_padding, this, std::placeholders::_1), set_padding_ports);
+    factory.registerSimpleAction(this->name+"_costmap_get_footprint_padding", std::bind(&CNavCostmapModuleBT::get_footprint, this, std::placeholders::_1), get_padding_ports);
+  }
+
+  template <class ModuleCfg>
+  CNavCostmapModuleBT<ModuleCfg>::~CNavCostmapModuleBT(void)
+  {
+    // [free dynamic memory]
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_width(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_width-> get_width");
+
+    unsigned int width;
+    if(this->module.get_width(width)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("width", width);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_width(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_width-> set_width");
+
+    BT::Optional<unsigned int> bt_width = self.getInput<unsigned int>("width");
+    if (!bt_width)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_width-> Incorrect or missing input. It needs the following input ports: width(int)");
+      return BT::NodeStatus::FAILURE;
+    }
+    unsigned int width=bt_width.value();
+    if(this->module.set_width(width)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_height(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_heigth-> get_height");
+
+    unsigned int height;
+    if(this->module.get_height(height)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("height", height);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_height(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_height-> set_height");
+    
+    BT::Optional<unsigned int> bt_height = self.getInput<unsigned int>("height");
+    if (!bt_height)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_height-> Incorrect or missing input. It needs the following input ports: height(int)");
+      return BT::NodeStatus::FAILURE;
+    }
+    unsigned int height=bt_height.value();
+    if(this->module.set_height(height)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_footprint(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_footprint-> get_footprint");
+
+    std::vector<geometry_msgs::Point> footprint;
+    if(this->module.get_footprint(footprint)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("footprint", footprint);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_footprint(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_footprint-> set_footprint");
+
+    BT::Optional<std::vector<geometry_msgs::Point>> bt_footprint = self.getInput<std::vector<geometry_msgs::Point>>("footprint");
+    if (!bt_footprint)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_footprint-> Incorrect or missing input. It needs the following input ports: footprint(std::vector<geometry_msgs::Point>)");
+      return BT::NodeStatus::FAILURE;
+    }
+    std::vector<geometry_msgs::Point> footprint=bt_footprint.value();
+    if(this->module.set_footprint(footprint)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_transform_tolerance(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_transform_tolerance-> get_transform_tolerance");
+
+    double tolerance;
+    if(this->module.get_transform_tolerance(tolerance)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("tolerance", tolerance);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_transform_tolerance(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_transform_tolerance-> set_transform_tolerance");
+
+    BT::Optional<double> bt_tol = self.getInput<double>("tolerance");
+    if (!bt_tol)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_transform_tolerance-> Incorrect or missing input. It needs the following input ports: tolerance(double)");
+      return BT::NodeStatus::FAILURE;
+    }    
+    double tol=bt_tol.value();
+    if(this->module.set_transform_tolerance(tol)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_update_frequency(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_update_frequency-> get_update_frequency");
+
+    double freq;
+    if(this->module.get_update_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("frequency", freq);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_update_frequency(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_update_frequency-> set_update_frequency");
+
+    BT::Optional<double> bt_freq = self.getInput<double>("frequency");
+    if (!bt_freq)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_update_freq-> Incorrect or missing input. It needs the following input ports: frequency(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    double freq=bt_freq.value();
+    if(this->module.set_update_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_publish_frequency(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_publish_frequency-> get_publish_frequency");
+
+    double freq;
+    if(this->module.get_publish_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("frequency", freq);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_publish_frequency(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_publish_frequency-> set_publish_frequency");
+
+    BT::Optional<double> bt_freq = self.getInput<double>("frequency");
+    if (!bt_freq)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_publish_frequency-> Incorrect or missing input. It needs the following input ports: frequency(double)");
+      return BT::NodeStatus::FAILURE;
+    }   
+    double freq=bt_freq.value();
+    if(this->module.set_publish_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_resolution(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_resolution-> get_resolution");
+
+    double res;
+    if(this->module.get_resolution(res)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("resolution", res);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_resolution(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_resolution-> set_resolution");
+
+    BT::Optional<double> bt_res = self.getInput<double>("resolution");
+    if (!bt_res)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_resolution-> Incorrect or missing input. It needs the following input ports: resolution(double)");
+      return BT::NodeStatus::FAILURE;
+    }   
+    double res=bt_res.value();
+    if(this->module.set_resolution(res)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_origin(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_origin-> get_origin");
+
+    double x;
+    double y;
+    if(this->module.get_origin(x,y)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("x", x);
+      self.setOutput("y", y);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_origin(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_origin-> set_origin");
+
+    BT::Optional<double> bt_x = self.getInput<double>("x");
+    BT::Optional<double> bt_y = self.getInput<double>("y");
+    if (!bt_x || !bt_y)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_origin-> Incorrect or missing input. It needs the following input ports: x(double) y(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    double x=bt_x.value();
+    double y=bt_y.value();
+    if(this->module.set_origin(x,y)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::get_footprint_padding(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::get_footprint_padding-> get_footprint_padding");
+
+    double pad;
+    if(this->module.get_footprint_padding(pad)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("padding", pad);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CNavCostmapModuleBT<ModuleCfg>::set_footprint_padding(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavCostmapModuleBT::set_footprint_padding-> set_footprint_padding");
+
+    BT::Optional<double> bt_pad = self.getInput<double>("padding");
+    if (!bt_pad)
+    {
+      ROS_ERROR("CNavCostmapModuleBT::nav_set_-footprint_padding> Incorrect or missing input. It needs the following input ports: padding(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    double pad=bt_pad.value();
+    if(this->module.set_footprint_padding(pad)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+#endif

include/iri_nav_module/sbpl_gp_module.h

+#ifndef _SBPL_GP_MODULE_H
+#define _SBPL_GP_MODULE_H
+
+// IRI ROS headers
+#include <iri_ros_tools/module_service.h>
+#include <iri_nav_module/nav_planner_module.h>
+
+/**
+  * \brief 
+  *
+  */
+template <class ModuleCfg>
+class CSBPLGPModule : public CNavPlannerModule<ModuleCfg>
+{
+  public:
+    /**
+      * \brief Constructor
+      *
+      */
+    CSBPLGPModule(const std::string &name,const std::string &name_space=std::string(""));
+    /**
+      * \brief
+      *
+      */
+    void dynamic_reconfigure(ModuleCfg &config, const std::string &name);
+    /**
+      * \brief Destructor
+      *
+      */
+    ~CSBPLGPModule();
+  };
+
+  template<class ModuleCfg>
+  CSBPLGPModule<ModuleCfg>::CSBPLGPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
+  {
+
+  }
+
+  template<class ModuleCfg>
+  void CSBPLGPModule<ModuleCfg>::dynamic_reconfigure(ModuleCfg &config, const std::string &name)
+  {
+  }
+
+  template<class ModuleCfg>
+  CSBPLGPModule<ModuleCfg>::~CSBPLGPModule()
+  {
+
+  }
+
+  #endif