From 8f76512b1f6732fb5e5afa53aa57079b19591725 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Thu, 1 Dec 2022 17:43:32 +0100
Subject: [PATCH] Added ros global planner and teb local planner module
templates
---
CMakeLists.txt | 6 +-
include/iri_nav_module/gp_module.h | 278 ++++++++++++++++++
include/iri_nav_module/teb_lp_module.h | 320 +++++++++++++++++++++
include/iri_nav_module/teb_lp_module_bt.h | 332 ++++++++++++++++++++++
4 files changed, 935 insertions(+), 1 deletion(-)
create mode 100644 include/iri_nav_module/gp_module.h
create mode 100644 include/iri_nav_module/teb_lp_module.h
create mode 100644 include/iri_nav_module/teb_lp_module_bt.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a83245f..793a549 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -129,6 +129,8 @@ add_library(${module_name}
include/${PROJECT_NAME}/opendrive_gp_module.h
include/${PROJECT_NAME}/sbpl_gp_module.h
include/${PROJECT_NAME}/osm_gp_module.h
+ include/${PROJECT_NAME}/gp_module.h
+ include/${PROJECT_NAME}/teb_lp_module.h
)
set_target_properties(${module_name} PROPERTIES LINKER_LANGUAGE CXX)
@@ -139,7 +141,9 @@ set(module_bt_name ${module_name}_bt)
add_library(${module_bt_name}
include/${PROJECT_NAME}/nav_module_bt.h
include/${PROJECT_NAME}/nav_costmap_module_bt.h
- include/${PROJECT_NAME}/ackermann_lp_module_bt.h)
+ include/${PROJECT_NAME}/ackermann_lp_module_bt.h
+ include/${PROJECT_NAME}/teb_lp_module_bt.h
+)
set_target_properties(${module_bt_name} PROPERTIES LINKER_LANGUAGE CXX)
target_link_libraries(${module_bt_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
diff --git a/include/iri_nav_module/gp_module.h b/include/iri_nav_module/gp_module.h
new file mode 100644
index 0000000..51370bb
--- /dev/null
+++ b/include/iri_nav_module/gp_module.h
@@ -0,0 +1,278 @@
+#ifndef _GP_MODULE_H
+#define _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 CGPModule : public CNavPlannerModule<ModuleCfg>
+{
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CGPModule(const std::string &name,const std::string &name_space=std::string(""));
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ // dyn_reconf_status_t set_allow_unknown(bool &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ // dyn_reconf_status_t get_allow_unknown(bool &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_lethal_cost(int &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_lethal_cost(int &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_neutral_cost(int &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_neutral_cost(int &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_cost_factor(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_cost_factor(double &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_publish_potential(bool &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_publish_potential(bool &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_orientation_mode(int &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_orientation_mode(int &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_orientation_window_size(int &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_orientation_window_size(int &value);
+
+
+
+ /**
+ * \brief
+ *
+ */
+ void dynamic_reconfigure(ModuleCfg &config, const std::string &name);
+ /**
+ * \brief Destructor
+ *
+ */
+ ~CGPModule();
+ };
+
+ template<class ModuleCfg>
+ CGPModule<ModuleCfg>::CGPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
+ {
+
+ }
+
+ /* parameter functions */
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::set_lethal_cost(int &value)
+ {
+ this->planner_reconf.set_parameter("lethal_cost",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::get_lethal_cost(int &value)
+ {
+ if(this->planner_reconf.get_parameter("lethal_cost",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::set_neutral_cost(int &value)
+ {
+ this->planner_reconf.set_parameter("neutral_cost",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::get_neutral_cost(int &value)
+ {
+ if(this->planner_reconf.get_parameter("neutral_cost",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::set_cost_factor(double &value)
+ {
+ this->planner_reconf.set_parameter("cost_factor",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::get_cost_factor(double &value)
+ {
+ if(this->planner_reconf.get_parameter("cost_factor",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::set_publish_potential(bool &value)
+ {
+ this->planner_reconf.set_parameter("publish_potential",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::get_publish_potential(bool &value)
+ {
+ if(this->planner_reconf.get_parameter("publish_potential",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::set_orientation_mode(int &value)
+ {
+ this->planner_reconf.set_parameter("orientation_mode",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::get_orientation_mode(int &value)
+ {
+ if(this->planner_reconf.get_parameter("orientation_mode",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::set_orientation_window_size(int &value)
+ {
+ this->planner_reconf.set_parameter("orientation_window_size",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CGPModule<ModuleCfg>::get_orientation_window_size(int &value)
+ {
+ if(this->planner_reconf.get_parameter("orientation_window_size",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+
+
+ template<class ModuleCfg>
+ void CGPModule<ModuleCfg>::dynamic_reconfigure(ModuleCfg &config, const std::string &name)
+ {
+ }
+
+ template<class ModuleCfg>
+ CGPModule<ModuleCfg>::~CGPModule()
+ {
+
+ }
+
+ #endif
diff --git a/include/iri_nav_module/teb_lp_module.h b/include/iri_nav_module/teb_lp_module.h
new file mode 100644
index 0000000..1d2b08c
--- /dev/null
+++ b/include/iri_nav_module/teb_lp_module.h
@@ -0,0 +1,320 @@
+#ifndef _TEB_LP_MODULE_H
+#define _TEB_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 CTEBLPModule : public CNavPlannerModule<ModuleCfg>
+{
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CTEBLPModule(const std::string &name,const std::string &name_space=std::string(""));
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_xy_goal_tolerance(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_xy_goal_tolerance(double &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_yaw_goal_tolerance(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_yaw_goal_tolerance(double &value);
+
+ /**
+ * \brief Function to get the both goal tolerances parameters.
+ *
+ * \param xy_tol The xy_tol value.
+ *
+ * \param yaw_tol The yaw_tol value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_goal_tolerance(double &xy_tol, double &yaw_tol);
+
+ /**
+ * \brief Function to set the both goal tolerances parameters.
+ *
+ * \param xy_tol The xy_tol to set.
+ *
+ * \param yaw_tol The yaw_tol to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_goal_tolerance(double &xy_tol, double &yaw_tol);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_max_vel_x(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_max_vel_x(double &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_mav_vel_x_backwards(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_mav_vel_x_backwards(double &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_max_vel_theta(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_max_vel_theta(double &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_acc_lim_x(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_acc_lim_x(double &value);
+
+ /**
+ * \brief Function to set the parameter
+ *
+ * \param value The value to set.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t set_acc_lim_theta(double &value);
+
+ /**
+ * \brief Function to get the parameter
+ *
+ * \param value The parameter value.
+ *
+ * \return The status of the operation.
+ */
+ dyn_reconf_status_t get_acc_lim_theta(double &value);
+
+
+
+ /**
+ * \brief
+ *
+ */
+ void dynamic_reconfigure(ModuleCfg &config, const std::string &name);
+ /**
+ * \brief Destructor
+ *
+ */
+ ~CTEBLPModule();
+ };
+
+ template<class ModuleCfg>
+ CTEBLPModule<ModuleCfg>::CTEBLPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
+ {
+
+ }
+
+ /* parameter functions */
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_xy_goal_tolerance(double &value)
+ {
+ this->planner_reconf.set_parameter("xy_goal_tolerance",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_xy_goal_tolerance(double &value)
+ {
+ if(this->planner_reconf.get_parameter("xy_goal_tolerance",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_goal_tolerance(double &xy_tol, double &yaw_tol)
+ {
+ this->planner_reconf.set_parameter("xy_goal_tolerance",xy_tol);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ this->planner_reconf.set_parameter("yaw_goal_tolerance",yaw_tol);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_goal_tolerance(double &xy_tol, double &yaw_tol)
+ {
+ if(this->planner_reconf.get_parameter("xy_goal_tolerance",xy_tol))
+ {
+ if(this->planner_reconf.get_parameter("yaw_goal_tolerance",yaw_tol))
+ return DYN_RECONF_SUCCESSFULL;
+ }
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_max_vel_x(double &value)
+ {
+ this->planner_reconf.set_parameter("max_vel_x",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_max_vel_x(double &value)
+ {
+ if(this->planner_reconf.get_parameter("max_vel_x",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_mav_vel_x_backwards(double &value)
+ {
+ this->planner_reconf.set_parameter("mav_vel_x_backwards",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_mav_vel_x_backwards(double &value)
+ {
+ if(this->planner_reconf.get_parameter("mav_vel_x_backwards",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_max_vel_theta(double &value)
+ {
+ this->planner_reconf.set_parameter("max_vel_theta",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_max_vel_theta(double &value)
+ {
+ if(this->planner_reconf.get_parameter("max_vel_theta",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_acc_lim_x(double &value)
+ {
+ this->planner_reconf.set_parameter("acc_lim_x",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_acc_lim_x(double &value)
+ {
+ if(this->planner_reconf.get_parameter("acc_lim_x",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_acc_lim_theta(double &value)
+ {
+ this->planner_reconf.set_parameter("acc_lim_theta",value);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_acc_lim_theta(double &value)
+ {
+ if(this->planner_reconf.get_parameter("acc_lim_theta",value))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+
+
+ template<class ModuleCfg>
+ void CTEBLPModule<ModuleCfg>::dynamic_reconfigure(ModuleCfg &config, const std::string &name)
+ {
+ }
+
+ template<class ModuleCfg>
+ CTEBLPModule<ModuleCfg>::~CTEBLPModule()
+ {
+
+ }
+
+ #endif
diff --git a/include/iri_nav_module/teb_lp_module_bt.h b/include/iri_nav_module/teb_lp_module_bt.h
new file mode 100644
index 0000000..a639b2d
--- /dev/null
+++ b/include/iri_nav_module/teb_lp_module_bt.h
@@ -0,0 +1,332 @@
+#ifndef _TEB_LP_MODULE_BT_H
+#define _TEB_LP_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <iri_nav_module/teb_lp_module.h>
+
+template <class ModuleCfg>
+class CTEBLPModuleBT
+{
+ private:
+ // object of nav_module
+ CTEBLPModule<ModuleCfg> &module;
+ std::string name;
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CTEBLPModuleBT(CTEBLPModule<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.
+ */
+ ~CTEBLPModuleBT();
+ protected:
+ // conditions
+
+ // actions
+ BT::NodeStatus set_goal_tolerance(BT::TreeNode& self);
+ BT::NodeStatus get_goal_tolerance(BT::TreeNode& self);
+ BT::NodeStatus set_xy_goal_tolerance(BT::TreeNode& self);
+ BT::NodeStatus get_xy_goal_tolerance(BT::TreeNode& self);
+ BT::NodeStatus set_yaw_goal_tolerance(BT::TreeNode& self);
+ BT::NodeStatus get_yaw_goal_tolerance(BT::TreeNode& self);
+ BT::NodeStatus set_max_vel_x(BT::TreeNode& self);
+ BT::NodeStatus get_max_vel_x(BT::TreeNode& self);
+ BT::NodeStatus set_mav_vel_x_backwards(BT::TreeNode& self);
+ BT::NodeStatus get_mav_vel_x_backwards(BT::TreeNode& self);
+ BT::NodeStatus set_max_vel_theta(BT::TreeNode& self);
+ BT::NodeStatus get_max_vel_theta(BT::TreeNode& self);
+ };
+
+ template <class ModuleCfg>
+ CTEBLPModuleBT<ModuleCfg>::CTEBLPModuleBT(CTEBLPModule<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 CTEBLPModuleBT<ModuleCfg>::init(IriBehaviorTreeFactory &factory)
+ {
+ BT::PortsList set_goal_tol_ports = {BT::InputPort<double>("xy_tolerance"),BT::InputPort<double>("yaw_tolerance")};
+ BT::PortsList get_goal_tol_ports = {BT::OutputPort<double>("xy_tolerance"),BT::OutputPort<double>("yaw_tolerance")};
+ BT::PortsList set_xy_goal_tol_ports = {BT::InputPort<double>("xy_tolerance")};
+ BT::PortsList get_xy_goal_tol_ports = {BT::OutputPort<double>("xy_tolerance")};
+ BT::PortsList set_yaw_goal_tol_ports = {BT::InputPort<double>("yaw_tolerance")};
+ BT::PortsList get_yaw_goal_tol_ports = {BT::OutputPort<double>("yaw_tolerance")};
+ BT::PortsList set_max_vel_x_ports = {BT::InputPort<double>("max_vel_x")};
+ BT::PortsList get_max_vel_x_ports = {BT::OutputPort<double>("max_vel_x")};
+ BT::PortsList set_max_vel_x_back_ports = {BT::InputPort<double>("max_vel_x_back")};
+ BT::PortsList get_max_vel_x_back_ports = {BT::OutputPort<double>("max_vel_x_back")};
+ BT::PortsList set_max_vel_theta_ports = {BT::InputPort<double>("max_vel_theta")};
+ BT::PortsList get_max_vel_theta_ports = {BT::OutputPort<double>("max_vel_theta")};
+
+
+ // Registration of conditions
+
+ //Registration of actions
+ factory.registerSimpleAction(this->name+"teb_set_goal_tolerance", std::bind(&CTEBLPModuleBT::set_goal_tolerance, this, std::placeholders::_1), set_goal_tol_ports);
+ factory.registerSimpleAction(this->name+"teb_get_goal_tolerance", std::bind(&CTEBLPModuleBT::get_goal_tolerance, this, std::placeholders::_1), get_goal_tol_ports);
+ factory.registerSimpleAction(this->name+"teb_set_xy_goal_tolerance", std::bind(&CTEBLPModuleBT::set_xy_goal_tolerance, this, std::placeholders::_1), set_xy_goal_tol_ports);
+ factory.registerSimpleAction(this->name+"teb_get_xy_goal_tolerance", std::bind(&CTEBLPModuleBT::get_xy_goal_tolerance, this, std::placeholders::_1), get_xy_goal_tol_ports);
+ factory.registerSimpleAction(this->name+"teb_set_yaw_goal_tolerance", std::bind(&CTEBLPModuleBT::set_yaw_goal_tolerance, this, std::placeholders::_1), set_yaw_goal_tol_ports);
+ factory.registerSimpleAction(this->name+"teb_get_yaw_goal_tolerance", std::bind(&CTEBLPModuleBT::get_yaw_goal_tolerance, this, std::placeholders::_1), get_yaw_goal_tol_ports);
+ factory.registerSimpleAction(this->name+"teb_set_max_vel_x", std::bind(&CTEBLPModuleBT::set_max_vel_x, this, std::placeholders::_1), set_max_vel_x_ports);
+ factory.registerSimpleAction(this->name+"teb_get_max_vel_x", std::bind(&CTEBLPModuleBT::get_max_vel_x, this, std::placeholders::_1), get_max_vel_x_ports);
+ factory.registerSimpleAction(this->name+"teb_set_mav_vel_x_backwards", std::bind(&CTEBLPModuleBT::set_mav_vel_x_backwards, this, std::placeholders::_1), set_max_vel_x_back_ports);
+ factory.registerSimpleAction(this->name+"teb_get_mav_vel_x_backwards", std::bind(&CTEBLPModuleBT::get_mav_vel_x_backwards, this, std::placeholders::_1), get_max_vel_x_back_ports);
+ factory.registerSimpleAction(this->name+"teb_set_max_vel_theta", std::bind(&CTEBLPModuleBT::set_max_vel_theta, this, std::placeholders::_1), set_max_vel_theta_ports);
+ factory.registerSimpleAction(this->name+"teb_get_max_vel_theta", std::bind(&CTEBLPModuleBT::get_max_vel_theta, this, std::placeholders::_1), get_max_vel_theta_ports);
+
+ }
+
+ template <class ModuleCfg>
+ CTEBLPModuleBT<ModuleCfg>::~CTEBLPModuleBT(void)
+ {
+ // [free dynamic memory]
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_goal_tolerance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::set_goal_tolerance-> set_goal_tolerance");
+
+ BT::Optional<double> bt_xy = self.getInput<double>("xy_tolerance");
+ BT::Optional<double> bt_yaw = self.getInput<double>("yaw_tolerance");
+ if (!bt_xy || !bt_yaw)
+ {
+ ROS_ERROR("CTEBLPModuleBT::set_goal_tolerance-> Incorrect or missing input. It needs the following input ports: xy_tolerance(double) yaw_tolerance(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double xy=bt_xy.value();
+ double yaw=bt_yaw.value();
+ if(this->module.set_goal_tolerance(xy,yaw)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_goal_tolerance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::get_goal_tolerance-> get_goal_tolerance");
+
+ double xy,yaw;
+ if(this->module.get_goal_tolerance(xy,yaw)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("xy_tolerance", xy);
+ self.setOutput("yaw_tolerance", yaw);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ { //Just beacuse in case of error to put something on the output port
+ self.setOutput("xy_tolerance", xy);
+ self.setOutput("yaw_tolerance", yaw);
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_xy_goal_tolerance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::set_xy_goal_tolerance-> set_xy_goal_tolerance");
+
+ BT::Optional<double> bt_xy = self.getInput<double>("xy_tolerance");
+ if (!bt_xy)
+ {
+ ROS_ERROR("CTEBLPModuleBT::set_xy_goal_tolerance-> Incorrect or missing input. It needs the following input ports: xy_tolerance(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double xy=bt_xy.value();
+ if(this->module.set_xy_goal_tolerance(xy)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_xy_goal_tolerance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::get_xy_goal_tolerance-> get_xy_goal_tolerance");
+
+ double xy;
+ if(this->module.get_xy_goal_tolerance(xy)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("xy_tolerance", xy);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ { //Just beacuse in case of error to put something on the output port
+ self.setOutput("xy_tolerance", xy);
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_yaw_goal_tolerance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::set_yaw_goal_tolerance-> set_yaw_goal_tolerance");
+
+ BT::Optional<double> bt_yaw = self.getInput<double>("yaw_tolerance");
+ if (!bt_yaw)
+ {
+ ROS_ERROR("CTEBLPModuleBT::set_yaw_goal_tolerance-> Incorrect or missing input. It needs the following input ports: yaw_tolerance(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double yaw=bt_yaw.value();
+ if(this->module.set_yaw_goal_tolerance(yaw)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_yaw_goal_tolerance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::get_yaw_goal_tolerance-> get_yaw_goal_tolerance");
+
+ double yaw;
+ if(this->module.get_yaw_goal_tolerance(yaw)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("yaw_tolerance", yaw);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ { //Just beacuse in case of error to put something on the output port
+ self.setOutput("yaw_tolerance", yaw);
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_max_vel_x(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::set_max_vel_x-> set_max_vel_x");
+
+ BT::Optional<double> bt_vel = self.getInput<double>("max_vel_x");
+ if (!bt_vel)
+ {
+ ROS_ERROR("CTEBLPModuleBT::set_max_vel_x-> Incorrect or missing input. It needs the following input ports: max_vel_x(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double vel=bt_vel.value();
+ if(this->module.set_max_vel_x(vel)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_max_vel_x(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::get_max_vel_x-> get_max_vel_x");
+
+ double vel;
+ if(this->module.get_max_vel_x(vel)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max_vel_x", vel);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ { //Just beacuse in case of error to put something on the output port
+ self.setOutput("max_vel_x", vel);
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_mav_vel_x_backwards(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::set_mav_vel_x_backwards-> set_mav_vel_x_backwards");
+
+ BT::Optional<double> bt_vel = self.getInput<double>("max_vel_x_back");
+ if (!bt_vel)
+ {
+ ROS_ERROR("CTEBLPModuleBT::set_mav_vel_x_backwards-> Incorrect or missing input. It needs the following input ports: max_vel_x_back(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double vel=bt_vel.value();
+ if(this->module.set_mav_vel_x_backwards(vel)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_mav_vel_x_backwards(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::get_mav_vel_x_backwards-> get_mav_vel_x_backwards");
+
+ double vel;
+ if(this->module.get_mav_vel_x_backwards(vel)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max_vel_x_back", vel);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ { //Just beacuse in case of error to put something on the output port
+ self.setOutput("max_vel_x_back", vel);
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_max_vel_theta(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::set_max_vel_theta-> set_max_vel_theta");
+
+ BT::Optional<double> bt_vel = self.getInput<double>("max_vel_theta");
+ if (!bt_vel)
+ {
+ ROS_ERROR("CTEBLPModuleBT::set_max_vel_theta-> Incorrect or missing input. It needs the following input ports: max_vel_theta(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double vel=bt_vel.value();
+ if(this->module.set_max_vel_theta(vel)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_max_vel_theta(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CTEBLPModuleBT::get_max_vel_theta-> get_max_vel_theta");
+
+ double vel;
+ if(this->module.get_max_vel_theta(vel)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max_vel_theta", vel);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ { //Just beacuse in case of error to put something on the output port
+ self.setOutput("max_vel_theta", vel);
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+
+
+
+#endif
--
GitLab