Added follow local planner module and BT module

CMakeLists.txt

@@ -131,6 +131,7 @@ add_library(${module_name}
   include/${PROJECT_NAME}/osm_gp_module.h
   include/${PROJECT_NAME}/gp_module.h
   include/${PROJECT_NAME}/teb_lp_module.h
+  include/${PROJECT_NAME}/follow_lp_module.h
 )
 set_target_properties(${module_name} PROPERTIES LINKER_LANGUAGE CXX)
 
@@ -143,6 +144,7 @@ add_library(${module_bt_name}
   include/${PROJECT_NAME}/nav_costmap_module_bt.h
   include/${PROJECT_NAME}/ackermann_lp_module_bt.h
   include/${PROJECT_NAME}/teb_lp_module_bt.h
+  include/${PROJECT_NAME}/follow_lp_module_bt.h
 )
 set_target_properties(${module_bt_name} PROPERTIES LINKER_LANGUAGE CXX)
 

include/iri_nav_module/follow_lp_module.h

+#ifndef _FOLLOW_LP_MODULE_H
+#define _FOLLOW_LP_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 CFOLLOWLPModule : public CNavPlannerModule<ModuleCfg>
+{
+  public:
+    /**
+      * \brief Constructor
+      *
+      */
+    CFOLLOWLPModule(const std::string &name,const std::string &name_space=std::string(""));
+
+    /**
+     * \brief Function to set the parameter update_dist
+     *
+     * \param value The value to set.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t set_update_dist(double &value);
+
+    /**
+     * \brief Function to get the parameter update_dist
+     *
+     * \param value The parameter value.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t get_update_dist(double &value);
+
+    /**
+     * \brief Function to set the parameter follow_dist
+     *
+     * \param value The value to set.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t set_follow_dist(double &value);
+
+    /**
+     * \brief Function to get the parameter follow_dist
+     *
+     * \param value The parameter value.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t get_follow_dist(double &value);
+
+    /**
+     * \brief Function to get follow_restart_dist.
+     *
+     * \param value The parameter value.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t get_follow_restart_dist(double &value);
+
+    /**
+     * \brief Function to set follow_restart_dist.
+     *
+     * \param value The parameter value.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t set_follow_restart_dist(double &value);
+
+    /**
+     * \brief Function to set the parameter dist_samples
+     *
+     * \param value The value to set.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t set_spline_dist_samples(int &value);
+
+    /**
+     * \brief Function to get the parameter dist_samples
+     *
+     * \param value The parameter value.
+     * 
+     * \return The status of the operation.
+     */
+    dyn_reconf_status_t get_spline_dist_samples(int &value);
+
+    /**
+      * \brief
+      *
+      */
+    void dynamic_reconfigure(ModuleCfg &config, const std::string &name);
+    /**
+      * \brief Destructor
+      *
+      */
+    ~CFOLLOWLPModule();
+  };
+
+  template<class ModuleCfg>
+  CFOLLOWLPModule<ModuleCfg>::CFOLLOWLPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
+  {
+
+  }
+
+  /* parameter functions */
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::set_update_dist(double &value)
+  {
+    this->planner_reconf.set_parameter("update_dist",value);
+    return this->planner_reconf.get_status();
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::get_update_dist(double &value)
+  {
+    if(this->planner_reconf.get_parameter("update_dist",value))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::set_follow_dist(double &value)
+  {
+    this->planner_reconf.set_parameter("follow_dist",value);
+    return this->planner_reconf.get_status();
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::get_follow_dist(double &value)
+  {
+    if(this->planner_reconf.get_parameter("follow_dist",value))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::get_follow_restart_dist(double &value)
+  {
+    this->planner_reconf.set_parameter("follow_restart_dist",value);
+    return this->planner_reconf.get_status();
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::set_follow_restart_dist(double &value)
+  {
+    if(this->planner_reconf.get_parameter("follow_restart_dist",value))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::set_spline_dist_samples(int &value)
+  {
+    this->planner_reconf.set_parameter("dist_samples",value);
+    return this->planner_reconf.get_status();
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CFOLLOWLPModule<ModuleCfg>::get_spline_dist_samples(int &value)
+  {
+    if(this->planner_reconf.get_parameter("dist_samples",value))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
+  template<class ModuleCfg>
+  void CFOLLOWLPModule<ModuleCfg>::dynamic_reconfigure(ModuleCfg &config, const std::string &name)
+  {
+  }
+
+  template<class ModuleCfg>
+  CFOLLOWLPModule<ModuleCfg>::~CFOLLOWLPModule()
+  {
+
+  }
+
+  #endif

include/iri_nav_module/follow_lp_module_bt.h

+#ifndef _FOLLOW_LP_MODULE_BT_H
+#define _FOLLOW_LP_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <iri_nav_module/follow_lp_module.h>
+
+template <class ModuleCfg>
+class CFOLLOWLPModuleBT
+{
+  private:
+    // object of nav_module
+    CFOLLOWLPModule<ModuleCfg> &module;
+    std::string name;
+
+  public:
+    /**
+      * \brief Constructor
+      *
+      */
+    CFOLLOWLPModuleBT(CFOLLOWLPModule<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.
+    */
+    ~CFOLLOWLPModuleBT();
+  protected:
+    // conditions
+
+    // actions
+    BT::NodeStatus set_update_dist(BT::TreeNode& self);
+    BT::NodeStatus get_update_dist(BT::TreeNode& self);
+    BT::NodeStatus set_follow_dist(BT::TreeNode& self);
+    BT::NodeStatus get_follow_dist(BT::TreeNode& self);
+    BT::NodeStatus get_follow_restart_dist(BT::TreeNode& self);
+    BT::NodeStatus set_follow_restart_dist(BT::TreeNode& self);
+    BT::NodeStatus set_spline_dist_samples(BT::TreeNode& self);
+    BT::NodeStatus get_spline_dist_samples(BT::TreeNode& self);
+  };
+
+  template <class ModuleCfg>
+  CFOLLOWLPModuleBT<ModuleCfg>::CFOLLOWLPModuleBT(CFOLLOWLPModule<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 CFOLLOWLPModuleBT<ModuleCfg>::init(IriBehaviorTreeFactory &factory)
+  {
+    BT::PortsList set_update_dist_ports = {BT::InputPort<double>("update_dist")};
+    BT::PortsList get_update_dist_ports = {BT::OutputPort<double>("update_dist")};
+    BT::PortsList set_follow_dist_ports = {BT::InputPort<double>("follow_dist")};
+    BT::PortsList get_follow_dist_ports = {BT::OutputPort<double>("follow_dist")};
+    BT::PortsList get_follow_restart_dist_ports = {BT::InputPort<double>("follow_restart_dist")};
+    BT::PortsList set_follow_restart_dist_ports = {BT::OutputPort<double>("follow_restart_dist")};
+    BT::PortsList set_spline_dist_samples_ports = {BT::InputPort<int>("spline_dist_samples")};
+    BT::PortsList get_spline_dist_samples_ports = {BT::OutputPort<int>("spline_dist_samples")};
+
+
+    // Registration of conditions   
+
+    //Registration of actions
+    factory.registerSimpleAction(this->name+"_follow_set_update_dist", std::bind(&CFOLLOWLPModuleBT::set_update_dist, this, std::placeholders::_1), set_update_dist_ports);
+    factory.registerSimpleAction(this->name+"_follow_get_update_dist", std::bind(&CFOLLOWLPModuleBT::get_update_dist, this, std::placeholders::_1), get_update_dist_ports);
+    factory.registerSimpleAction(this->name+"_follow_set_follow_dist", std::bind(&CFOLLOWLPModuleBT::set_follow_dist, this, std::placeholders::_1), set_follow_dist_ports);
+    factory.registerSimpleAction(this->name+"_follow_get_follow_dist", std::bind(&CFOLLOWLPModuleBT::get_follow_dist, this, std::placeholders::_1), get_follow_dist_ports);
+    factory.registerSimpleAction(this->name+"_follow_get_follow_restart_dist", std::bind(&CFOLLOWLPModuleBT::get_follow_restart_dist, this, std::placeholders::_1), get_follow_restart_dist_ports);
+    factory.registerSimpleAction(this->name+"_follow_set_follow_restart_dist", std::bind(&CFOLLOWLPModuleBT::set_follow_restart_dist, this, std::placeholders::_1), set_follow_restart_dist_ports);
+    factory.registerSimpleAction(this->name+"_follow_set_spline_dist_samples", std::bind(&CFOLLOWLPModuleBT::set_spline_dist_samples, this, std::placeholders::_1), set_spline_dist_samples_ports);
+    factory.registerSimpleAction(this->name+"_follow_get_spline_dist_samples", std::bind(&CFOLLOWLPModuleBT::get_spline_dist_samples, this, std::placeholders::_1), get_spline_dist_samples_ports);
+  }
+
+  template <class ModuleCfg>
+  CFOLLOWLPModuleBT<ModuleCfg>::~CFOLLOWLPModuleBT(void)
+  {
+    // [free dynamic memory]
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::set_update_dist(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::set_update_dist-> set_update_dist");
+
+    BT::Optional<double> bt_update_dist = self.getInput<double>("update_dist");
+    if (!bt_update_dist)
+    {
+      ROS_ERROR("CFOLLOWLPModuleBT::set_update_dist-> Incorrect or missing input. It needs the following input ports: update_dist(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    double xy=bt_update_dist.value();
+    if(this->module.set_update_dist(update_dist)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::get_update_dist(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::get_update_dist-> get_update_dist");
+
+    double update_dist;
+    if(this->module.get_update_dist(update_dist)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("update_dist", update_dist);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+    { //Just beacuse in case of error to put something on the output port
+      self.setOutput("update_dist", update_dist);
+      return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::set_follow_dist(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::set_follow_dist-> set_follow_dist");
+
+    BT::Optional<double> bt_follow_dist = self.getInput<double>("follow_dist");
+    if (!bt_follow_dist)
+    {
+      ROS_ERROR("CFOLLOWLPModuleBT::set_follow_dist-> Incorrect or missing input. It needs the following input ports: follow_dist(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    double follow_dist=bt_follow_dist.value();
+    if(this->module.set_follow_dist(follow_dist)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::get_follow_dist(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::get_follow_dist-> get_follow_dist");
+
+    double follow_dist;
+    if(this->module.get_follow_dist(follow_dist)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("follow_dist", yaw);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+    { //Just beacuse in case of error to put something on the output port
+      self.setOutput("follow_dist", yaw);
+      return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::get_follow_restart_dist(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::get_follow_restart_dist-> get_follow_restart_dist");
+
+    BT::Optional<double> bt_follow_restart_dist = self.getInput<double>("follow_restart_dist");
+    if (!bt_follow_restart_dist)
+    {
+      ROS_ERROR("CFOLLOWLPModuleBT::get_follow_restart_dist-> Incorrect or missing input. It needs the following input ports: follow_restart_dist(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    double follow_restart_dist=bt_follow_restart_dist.value();
+    if(this->module.get_follow_restart_dist(follow_restart_dist)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::set_follow_restart_dist(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::set_follow_restart_dist-> set_follow_restart_dist");
+
+    double follow_restart_dist;
+    if(this->module.set_follow_restart_dist(follow_restart_dist)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("follow_restart_dist", follow_restart_dist);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+    { //Just beacuse in case of error to put something on the output port
+      self.setOutput("follow_restart_dist", follow_restart_dist);
+      return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::set_spline_dist_samples(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::set_spline_dist_samples-> set_spline_dist_samples");
+
+    BT::Optional<int> bt_dist_samples = self.getInput<int>("dist_samples");
+    if (!bt_dist_samples)
+    {
+      ROS_ERROR("CFOLLOWLPModuleBT::set_spline_dist_samples-> Incorrect or missing input. It needs the following input ports: dist_samples(int)");
+      return BT::NodeStatus::FAILURE;
+    }
+    int dist_samples=bt_dist_samples.value();
+    if(this->module.set_spline_dist_samples(dist_samples)==DYN_RECONF_SUCCESSFULL)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+
+  template <class ModuleCfg>
+  BT::NodeStatus CFOLLOWLPModuleBT<ModuleCfg>::get_spline_dist_samples(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CFOLLOWLPModuleBT::get_spline_dist_samples-> get_spline_dist_samples");
+
+    int dist_samples;
+    if(this->module.get_spline_dist_samples(dist_samples)==DYN_RECONF_SUCCESSFULL)
+    {
+      self.setOutput("dist_samples", dist_samples);
+      return BT::NodeStatus::SUCCESS;
+    }
+    else
+    { //Just beacuse in case of error to put something on the output port
+      self.setOutput("dist_samples", dist_samples);
+      return BT::NodeStatus::FAILURE;
+    }
+  }
+
+#endif

src/xml/bt_definitions.xml

@@ -159,6 +159,31 @@
         <Action ID="nav_module_teb_get_max_vel_theta">
             <output_port name="max_vel_theta"> Local planner max vel theta</output_port>
         </Action>
+        <!-- Follow local planner -->
+        <Action ID="nav_module_teb_sfollow_set_update_dist">
+            <input_port name="update_dist"> Local planner update_dist parameter</input_port>
+        </Action>
+        <Action ID="nav_module_teb_gfollow_get_update_dist">
+            <output_port name="update_dist"> Local planner update_dist parameter</output_port>
+        </Action>
+        <Action ID="nav_modfollow_set_follow_dist">
+            <input_port name="follow_dist"> Local planner follow_dist parameter</input_port>
+        </Action>
+        <Action ID="nav_modfollow_get_follow_dist">
+            <output_port name="follow_dist"> Local planner follow_dist parameter</output_port>
+        </Action>
+        <Action ID="nav_module_teb_sefollow_get_follow_restart_dist">
+            <input_port name="follow_restart_dist"> Local planner follow_restart_dist parameter</input_port>
+        </Action>
+        <Action ID="nav_module_teb_gefollow_set_follow_restart_dist">
+            <output_port name="follow_restart_dist"> Local planner follow_restart_dist parameter</output_port>
+        </Action>
+        <Action ID="nav_module_follow_set_spline_dist_samples">
+            <input_port name="spline_dist_samples"> Local planner spline_dist_samples parameter</input_port>
+        </Action>
+        <Action ID="nav_module_follow_get_spline_dist_samples">
+            <output_port name="spline_dist_samples"> Local planner spline_dist_samples parameter</output_port>
+        </Action>
         <!-- iri_nav_module -->
         <Action ID="nav_module_set_goal_frame">
             <input_port name="frame_id"> The name of the new reference frame for the future goals</input_port>