diff --git a/CMakeLists.txt b/CMakeLists.txt
index 793a549cc3dde6688d08d7d5326ee71dd8529cd4..56c6eb067f0f896df8cdabc7c459ed4a0bfd7619 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/include/iri_nav_module/follow_lp_module.h b/include/iri_nav_module/follow_lp_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ce6f1f4c19718e84b94dde106a39c2a0ead9cf3
--- /dev/null
+++ b/include/iri_nav_module/follow_lp_module.h
@@ -0,0 +1,188 @@
+#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
diff --git a/include/iri_nav_module/follow_lp_module_bt.h b/include/iri_nav_module/follow_lp_module_bt.h
new file mode 100644
index 0000000000000000000000000000000000000000..442689b605d3504e122cdc228dc8f0203c3bc954
--- /dev/null
+++ b/include/iri_nav_module/follow_lp_module_bt.h
@@ -0,0 +1,241 @@
+#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
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
index 0eaaf91bd9d47dc30b250902dc7523c01cf9023b..186ab9f89548b0d26054dfea4aa8a6d8622e52d2 100644
--- a/src/xml/bt_definitions.xml
+++ b/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>