diff --git a/include/iri_nav_module/ackermann_lp_module_bt.h b/include/iri_nav_module/ackermann_lp_module_bt.h
new file mode 100644
index 0000000000000000000000000000000000000000..c27d32b0ae3e680eb63fe2a2a43c36d82b2ca3aa
--- /dev/null
+++ b/include/iri_nav_module/ackermann_lp_module_bt.h
@@ -0,0 +1,1226 @@
+#ifndef _ACKERMANN_LP_MODULE_BT_H
+#define _ACKERMANN_LP_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <iri_nav_module/ackermann_lp_module.h>
+
+template <class ModuleCfg>
+class CAckermannLPModuleBT
+{
+ private:
+ // object of nav_module
+ CAckermannLPModule<ModuleCfg> &module;
+ std::string name;
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CAckermannLPModuleBT(CAckermannLPModule<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.
+ */
+ ~CAckermannLPModuleBT();
+ protected:
+ // conditions
+ BT::NodeStatus using_steer_angle(void);
+ BT::NodeStatus pruning_plan(void);
+ BT::NodeStatus using_dead_zone(void);
+ BT::NodeStatus is_inverted_ackermann(void);
+ BT::NodeStatus is_steering_while_stopped(void);
+ BT::NodeStatus is_stuck_check_enabled(void);
+
+ // actions
+ BT::NodeStatus use_steer_angle(BT::TreeNode& self);
+ BT::NodeStatus prune_plan(BT::TreeNode& self);
+ BT::NodeStatus use_dead_zone(BT::TreeNode& self);
+ BT::NodeStatus get_trans_vel_samples(BT::TreeNode& self);
+ BT::NodeStatus set_trans_vel_samples(BT::TreeNode& self);
+ BT::NodeStatus get_steer_angle_samples(BT::TreeNode& self);
+ BT::NodeStatus set_steer_angle_samples(BT::TreeNode& self);
+ BT::NodeStatus get_angular_vel_samples(BT::TreeNode& self);
+ BT::NodeStatus set_angular_vel_samples(BT::TreeNode& self);
+ BT::NodeStatus get_cmd_vel_average_samples(BT::TreeNode& self);
+ BT::NodeStatus set_cmd_vel_average_samples(BT::TreeNode& self);
+ BT::NodeStatus get_odom_average_samples(BT::TreeNode& self);
+ BT::NodeStatus set_odom_average_samples(BT::TreeNode& self);
+ BT::NodeStatus get_heading_cost_points(BT::TreeNode& self);
+ BT::NodeStatus set_heading_cost_points(BT::TreeNode& self);
+ BT::NodeStatus get_simulation_time(BT::TreeNode& self);
+ BT::NodeStatus set_simulation_time(BT::TreeNode& self);
+ BT::NodeStatus get_simulation_res(BT::TreeNode& self);
+ BT::NodeStatus set_simulation_res(BT::TreeNode& self);
+ BT::NodeStatus get_path_distance_cost(BT::TreeNode& self);
+ BT::NodeStatus set_path_distance_cost(BT::TreeNode& self);
+ BT::NodeStatus get_goal_distance_cost(BT::TreeNode& self);
+ BT::NodeStatus set_goal_distance_cost(BT::TreeNode& self);
+ BT::NodeStatus get_obstacle_cost(BT::TreeNode& self);
+ BT::NodeStatus set_obstacle_cost(BT::TreeNode& self);
+ BT::NodeStatus get_heading_cost(BT::TreeNode& self);
+ BT::NodeStatus set_heading_cost(BT::TreeNode& self);
+ BT::NodeStatus get_goal_tolerances(BT::TreeNode& self);
+ BT::NodeStatus set_goal_tolerances(BT::TreeNode& self);
+ BT::NodeStatus get_linear_velocity_range(BT::TreeNode& self);
+ BT::NodeStatus set_linear_velocity_range(BT::TreeNode& self);
+ BT::NodeStatus get_angular_velocity_range(BT::TreeNode& self);
+ BT::NodeStatus set_angular_velocity_range(BT::TreeNode& self);
+ BT::NodeStatus get_linear_max_accel(BT::TreeNode& self);
+ BT::NodeStatus set_linear_max_accel(BT::TreeNode& self);
+ BT::NodeStatus get_steer_max_accel(BT::TreeNode& self);
+ BT::NodeStatus set_steer_max_accel(BT::TreeNode& self);
+ BT::NodeStatus get_steer_angle_range(BT::TreeNode& self);
+ BT::NodeStatus set_steer_angle_range(BT::TreeNode& self);
+ BT::NodeStatus get_steer_velocity_range(BT::TreeNode& self);
+ BT::NodeStatus set_steer_velocity_range(BT::TreeNode& self);
+ BT::NodeStatus get_min_segment_length(BT::TreeNode& self);
+ BT::NodeStatus set_min_segment_length(BT::TreeNode& self);
+ BT::NodeStatus get_axis_distance(BT::TreeNode& self);
+ BT::NodeStatus set_axis_distance(BT::TreeNode& self);
+ BT::NodeStatus get_wheel_distance(BT::TreeNode& self);
+ BT::NodeStatus set_wheel_distance(BT::TreeNode& self);
+ BT::NodeStatus get_wheel_radius(BT::TreeNode& self);
+ BT::NodeStatus set_wheel_radius(BT::TreeNode& self);
+ BT::NodeStatus use_inverted_ackermann(BT::TreeNode& self);
+ BT::NodeStatus use_steering_while_stopped(BT::TreeNode& self);
+ BT::NodeStatus use_stuck_check(BT::TreeNode& self);
+ };
+
+ template <class ModuleCfg>
+ CAckermannLPModuleBT<ModuleCfg>::CAckermannLPModuleBT(CAckermannLPModule<ModuleCfg> &module,const std::string &name) :
+ module(module)
+ {
+ std::size_t found = name.find_last_of("/\\");
+ this->name=name.substr(0,found);
+ }
+
+ template <class ModuleCfg>
+ void CAckermannLPModuleBT<ModuleCfg>::init(IriBehaviorTreeFactory &factory)
+ {
+ BT::PortsList use_steer_angle_ports = {BT::InputPort<bool>("steer_angle")};
+ BT::PortsList prune_plan_ports = {BT::InputPort<bool>("prune")};
+ BT::PortsList use_dead_zone_ports = {BT::InputPort<bool>("enabled"),BT::InputPort<double>("dead_zone")};
+ BT::PortsList get_trans_vel_samples_ports = {BT::OutputPort<int>("samples")};
+ BT::PortsList set_trans_vel_samples_ports = {BT::InputPort<int>("samples")};
+ BT::PortsList get_steer_angle_samples_ports = {BT::OutputPort<int>("samples")};
+ BT::PortsList set_steer_angle_samples_ports = {BT::InputPort<int>("samples")};
+ BT::PortsList get_angular_vel_samples_ports = {BT::OutputPort<int>("samples")};
+ BT::PortsList set_angular_vel_samples_ports = {BT::InputPort<int>("samples")};
+ BT::PortsList get_cmd_vel_average_samples_ports = {BT::OutputPort<int>("samples")};
+ BT::PortsList set_cmd_vel_average_samples_ports = {BT::InputPort<int>("samples")};
+ BT::PortsList get_odom_average_samples_ports = {BT::OutputPort<int>("samples")};
+ BT::PortsList set_odom_average_samples_ports = {BT::InputPort<int>("samples")};
+ BT::PortsList get_heading_cost_points_ports = {BT::OutputPort<int>("num")};
+ BT::PortsList set_heading_cost_points_ports = {BT::InputPort<int>("num")};
+ BT::PortsList get_simulation_time_ports = {BT::OutputPort<double>("min"),BT::OutputPort<double>("max")};
+ BT::PortsList set_simulation_time_ports = {BT::InputPort<double>("min"),BT::InputPort<double>("max")};
+ BT::PortsList get_simulation_res_ports = {BT::OutputPort<double>("linear"),BT::OutputPort<double>("angular")};
+ BT::PortsList set_simulation_res_ports = {BT::InputPort<double>("linear"),BT::InputPort<double>("angular")};
+ BT::PortsList get_path_distance_cost_ports = {BT::OutputPort<double>("cost")};
+ BT::PortsList set_path_distance_cost_ports = {BT::InputPort<double>("cost")};
+ BT::PortsList get_goal_distance_cost_ports = {BT::OutputPort<double>("cost")};
+ BT::PortsList set_goal_distance_cost_ports = {BT::InputPort<double>("cost")};
+ BT::PortsList get_obstacle_cost_ports = {BT::OutputPort<double>("cost")};
+ BT::PortsList set_obstacle_cost_ports = {BT::InputPort<double>("cost")};
+ BT::PortsList get_heading_cost_ports = {BT::OutputPort<double>("cost")};
+ BT::PortsList set_heading_cost_ports = {BT::InputPort<double>("cost")};
+ BT::PortsList get_goal_tolerances_ports = {BT::OutputPort<double>("xy_tolerance"),BT::OutputPort<double>("yaw_tolerance")};
+ BT::PortsList set_goal_tolerances_ports = {BT::InputPort<double>("xy_tolerance"),BT::InputPort<double>("yaw_tolerance")};
+ BT::PortsList get_linear_velocity_range_ports = {BT::OutputPort<double>("max"),BT::OutputPort<double>("min")};
+ BT::PortsList set_linear_velocity_range_ports = {BT::InputPort<double>("max"),BT::InputPort<double>("min")};
+ BT::PortsList get_angular_velocity_range_ports = {BT::OutputPort<double>("max"),BT::OutputPort<double>("min")};
+ BT::PortsList set_angular_velocity_range_ports = {BT::InputPort<double>("max"),BT::InputPort<double>("min")};
+ BT::PortsList get_linear_max_accel_ports = {BT::OutputPort<double>("max")};
+ BT::PortsList set_linear_max_accel_ports = {BT::InputPort<double>("max")};
+ BT::PortsList get_steer_max_accel_ports = {BT::OutputPort<double>("max")};
+ BT::PortsList set_steer_max_accel_ports = {BT::InputPort<double>("max")};
+ BT::PortsList get_steer_angle_range_ports = {BT::OutputPort<double>("max"),BT::OutputPort<double>("min")};
+ BT::PortsList set_steer_angle_range_ports = {BT::InputPort<double>("max"),BT::InputPort<double>("min")};
+ BT::PortsList get_steer_velocity_range_ports = {BT::OutputPort<double>("max"),BT::OutputPort<double>("max")};
+ BT::PortsList set_steer_velocity_range_ports = {BT::InputPort<double>("max"),BT::InputPort<double>("max")};
+ BT::PortsList get_min_segment_length_ports = {BT::OutputPort<double>("length")};
+ BT::PortsList set_min_segment_length_ports = {BT::InputPort<double>("length")};
+ BT::PortsList get_axis_distance_ports = {BT::OutputPort<double>("distance")};
+ BT::PortsList set_axis_distance_ports = {BT::InputPort<double>("distance")};
+ BT::PortsList get_wheel_distance_ports = {BT::OutputPort<double>("distance")};
+ BT::PortsList set_wheel_distance_ports = {BT::InputPort<double>("distance")};
+ BT::PortsList get_wheel_radius_ports = {BT::OutputPort<double>("radius")};
+ BT::PortsList set_wheel_radius_ports = {BT::InputPort<double>("radius")};
+ BT::PortsList use_inverted_ackermann_ports = {BT::InputPort<bool>("inverted")};
+ BT::PortsList use_steering_while_stopped_ports = {BT::InputPort<bool>("enabled"),BT::InputPort<double>("threshold")};
+ BT::PortsList use_stuck_check_ports = {BT::InputPort<bool>("enabled"),BT::InputPort<double>("threshold"),BT::InputPort<int>("max_num")};
+
+ // Registration of conditions
+ factory.registerSimpleCondition(this->name+"_ackermann_lp_using_steer_angle", std::bind(&CAckermannLPModuleBT::using_steer_angle, this));
+
+ //Registration of actions
+ factory.registerSimpleAction(this->name+"_ackermann_lp_use_steer_angle", std::bind(&CAckermannLPModuleBT::use_steer_angle, this, std::placeholders::_1), use_steer_angle_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_prune_plan", std::bind(&CAckermannLPModuleBT::prune_plan, this, std::placeholders::_1), prune_plan_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_use_dead_zone", std::bind(&CAckermannLPModuleBT::use_dead_zone, this, std::placeholders::_1), use_dead_zone_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_trans_vel_samples", std::bind(&CAckermannLPModuleBT::get_trans_vel_samples, this, std::placeholders::_1), get_trans_vel_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_trans_vel_samples", std::bind(&CAckermannLPModuleBT::set_trans_vel_samples, this, std::placeholders::_1), set_trans_vel_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_steer_angle_samples", std::bind(&CAckermannLPModuleBT::get_steer_angle_samples, this, std::placeholders::_1), get_steer_angle_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_steer_angle_samples", std::bind(&CAckermannLPModuleBT::set_steer_angle_samples, this, std::placeholders::_1), set_steer_angle_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_angular_vel_samples", std::bind(&CAckermannLPModuleBT::get_angular_vel_samples, this, std::placeholders::_1), get_angular_vel_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_angular_vel_samples", std::bind(&CAckermannLPModuleBT::set_angular_vel_samples, this, std::placeholders::_1), set_angular_vel_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_cmd_vel_average_samples", std::bind(&CAckermannLPModuleBT::get_cmd_vel_average_samples, this, std::placeholders::_1), get_cmd_vel_average_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_cmd_vel_average_samples", std::bind(&CAckermannLPModuleBT::set_cmd_vel_average_samples, this, std::placeholders::_1), set_cmd_vel_average_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_odom_average_samples", std::bind(&CAckermannLPModuleBT::get_odom_average_samples, this, std::placeholders::_1), get_odom_average_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_odom_average_samples", std::bind(&CAckermannLPModuleBT::set_odom_average_samples, this, std::placeholders::_1), set_odom_average_samples_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_heading_cost_points", std::bind(&CAckermannLPModuleBT::get_heading_cost_points, this, std::placeholders::_1), get_heading_cost_points_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_heading_cost_points", std::bind(&CAckermannLPModuleBT::set_heading_cost_points, this, std::placeholders::_1), set_heading_cost_points_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_simulation_time", std::bind(&CAckermannLPModuleBT::get_simulation_time, this, std::placeholders::_1), get_simulation_time_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_simulation_time", std::bind(&CAckermannLPModuleBT::set_simulation_time, this, std::placeholders::_1), set_simulation_time_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_simulation_res", std::bind(&CAckermannLPModuleBT::get_simulation_res, this, std::placeholders::_1), get_simulation_res_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_simulation_res", std::bind(&CAckermannLPModuleBT::set_simulation_res, this, std::placeholders::_1), set_simulation_res_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_path_distance_cost", std::bind(&CAckermannLPModuleBT::get_path_distance_cost, this, std::placeholders::_1), get_path_distance_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_path_distance_cost", std::bind(&CAckermannLPModuleBT::set_path_distance_cost, this, std::placeholders::_1), set_path_distance_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_goal_distance_cost", std::bind(&CAckermannLPModuleBT::get_goal_distance_cost, this, std::placeholders::_1), get_goal_distance_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_goal_distance_cost", std::bind(&CAckermannLPModuleBT::set_goal_distance_cost, this, std::placeholders::_1), set_goal_distance_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_obstacle_cost", std::bind(&CAckermannLPModuleBT::get_obstacle_cost, this, std::placeholders::_1), get_obstacle_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_obstacle_cost", std::bind(&CAckermannLPModuleBT::set_obstacle_cost, this, std::placeholders::_1), set_obstacle_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_heading_cost", std::bind(&CAckermannLPModuleBT::get_heading_cost, this, std::placeholders::_1), get_heading_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_heading_cost", std::bind(&CAckermannLPModuleBT::set_heading_cost, this, std::placeholders::_1), set_heading_cost_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_goal_tolerances", std::bind(&CAckermannLPModuleBT::get_goal_tolerances, this, std::placeholders::_1), get_goal_tolerances_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_goal_tolerances", std::bind(&CAckermannLPModuleBT::set_goal_tolerances, this, std::placeholders::_1), set_goal_tolerances_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_linear_velocity_range", std::bind(&CAckermannLPModuleBT::get_linear_velocity_range, this, std::placeholders::_1), get_linear_velocity_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_linear_velocity_range", std::bind(&CAckermannLPModuleBT::set_linear_velocity_range, this, std::placeholders::_1), set_linear_velocity_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_angular_velocity_range", std::bind(&CAckermannLPModuleBT::get_angular_velocity_range, this, std::placeholders::_1), get_angular_velocity_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_angular_velocity_range", std::bind(&CAckermannLPModuleBT::set_angular_velocity_range, this, std::placeholders::_1), set_angular_velocity_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_linear_max_accel", std::bind(&CAckermannLPModuleBT::get_linear_max_accel, this, std::placeholders::_1), get_linear_max_accel_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_linear_max_accel", std::bind(&CAckermannLPModuleBT::set_linear_max_accel, this, std::placeholders::_1), set_linear_max_accel_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_steer_max_accel", std::bind(&CAckermannLPModuleBT::get_steer_max_accel, this, std::placeholders::_1), get_steer_max_accel_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_steer_max_accel", std::bind(&CAckermannLPModuleBT::set_steer_max_accel, this, std::placeholders::_1), set_steer_max_accel_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_steer_angle_range", std::bind(&CAckermannLPModuleBT::get_steer_angle_range, this, std::placeholders::_1), get_steer_angle_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_steer_angle_range", std::bind(&CAckermannLPModuleBT::set_steer_angle_range, this, std::placeholders::_1), set_steer_angle_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_steer_velocity_range", std::bind(&CAckermannLPModuleBT::get_steer_velocity_range, this, std::placeholders::_1), get_steer_velocity_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_steer_velocity_range", std::bind(&CAckermannLPModuleBT::set_steer_velocity_range, this, std::placeholders::_1), set_steer_velocity_range_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_min_segment_length", std::bind(&CAckermannLPModuleBT::get_min_segment_length, this, std::placeholders::_1), get_min_segment_length_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_min_segment_length", std::bind(&CAckermannLPModuleBT::set_min_segment_length, this, std::placeholders::_1), set_min_segment_length_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_axis_distance", std::bind(&CAckermannLPModuleBT::get_axis_distance, this, std::placeholders::_1), get_axis_distance_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_axis_distance", std::bind(&CAckermannLPModuleBT::set_axis_distance, this, std::placeholders::_1), set_axis_distance_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_wheel_distance", std::bind(&CAckermannLPModuleBT::get_wheel_distance, this, std::placeholders::_1), get_wheel_distance_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_wheel_distance", std::bind(&CAckermannLPModuleBT::set_wheel_distance, this, std::placeholders::_1), set_wheel_distance_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_get_wheel_radius", std::bind(&CAckermannLPModuleBT::get_wheel_radius, this, std::placeholders::_1), get_wheel_radius_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_set_wheel_radius", std::bind(&CAckermannLPModuleBT::set_wheel_radius, this, std::placeholders::_1), set_wheel_radius_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_use_inverted_ackermann", std::bind(&CAckermannLPModuleBT::use_inverted_ackermann, this, std::placeholders::_1), use_inverted_ackermann_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_use_steering_while_stopped", std::bind(&CAckermannLPModuleBT::use_steering_while_stopped, this, std::placeholders::_1), use_steering_while_stopped_ports);
+ factory.registerSimpleAction(this->name+"_ackermann_lp_use_stuck_check", std::bind(&CAckermannLPModuleBT::use_stuck_check, this, std::placeholders::_1), use_stuck_check_ports);
+ }
+
+ template <class ModuleCfg>
+ CAckermannLPModuleBT<ModuleCfg>::~CAckermannLPModuleBT(void)
+ {
+ // [free dynamic memory]
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::using_steer_angle(void)
+ {
+ bool steer;
+ dyn_reconf_status_t status = this->module.using_steer_angle(steer);
+
+ if (status == DYN_RECONF_SUCCESSFULL)
+ {
+ if(steer)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::pruning_plan(void)
+ {
+ bool prune;
+ dyn_reconf_status_t status = this->module.pruning_plan(prune);
+
+ if (status == DYN_RECONF_SUCCESSFULL)
+ {
+ if(prune)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::using_dead_zone(void)
+ {
+ bool dead_zone;
+ dyn_reconf_status_t status = this->module.using_dead_zone(dead_zone);
+
+ if (status == DYN_RECONF_SUCCESSFULL)
+ {
+ if(dead_zone)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::is_inverted_ackermann(void)
+ {
+ bool inverted;
+ dyn_reconf_status_t status = this->module.is_inverted_ackermann(inverted);
+
+ if (status == DYN_RECONF_SUCCESSFULL)
+ {
+ if(inverted)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::is_steering_while_stopped(void)
+ {
+ bool enabled;
+ dyn_reconf_status_t status = this->module.is_steering_while_stopped(enabled);
+
+ if (status == DYN_RECONF_SUCCESSFULL)
+ {
+ if(enabled)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::is_stuck_check_enabled(void)
+ {
+ bool enabled;
+ dyn_reconf_status_t status = this->module.is_stuck_check_enabled(enabled);
+
+ if (status == DYN_RECONF_SUCCESSFULL)
+ {
+ if(enabled)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::FAILURE;
+ }
+
+
+ // actions
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::use_steer_angle(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT:: use_steer_angle -> use_steer_angle");
+
+ BT::Optional<bool> bt_steer_angle = self.getInput<bool>("steer_angle");
+ if (!bt_steer_angle)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::use_steer_angle-> Incorrect or missing input. It needs the following input ports: steer_angle(bool)");
+ return BT::NodeStatus::FAILURE;
+ }
+ bool steer_angle=bt_steer_angle.value();
+ if(this->module.use_steer_angle(steer_angle)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::prune_plan(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT:: prune_plan -> prune_plan");
+
+ BT::Optional<bool> bt_prune = self.getInput<bool>("prune");
+ if (!bt_prune)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::prune_plan-> Incorrect or missing input. It needs the following input ports: prune(bool)");
+ return BT::NodeStatus::FAILURE;
+ }
+ bool prune=bt_prune.value();
+ if(this->module.prune_plan(prune)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::use_dead_zone(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::use_dead_zone-> use_dead_zone");
+
+ BT::Optional<bool> bt_enabled = self.getInput<bool>("enabled");
+ BT::Optional<double> bt_dead_zone = self.getInput<double>("dead_zone");
+ if (!bt_enabled || !bt_dead_zone)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::use_dead_zone-> Incorrect or missing input. It needs the following input ports: enabled(bool) dead_zone(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ bool enabled=bt_enabled.value();
+ double dead_zone=bt_dead_zone.value();
+ if(this->module.use_dead_zone(enabled,dead_zone)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_trans_vel_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_trans_vel_samples-> get_trans_vel_samples");
+
+ unsigned int samples;
+ if(this->module.get_trans_vel_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("samples", samples);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_trans_vel_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_trans_vel_samples-> set_trans_vel_samples");
+
+ BT::Optional<int> bt_samples = self.getInput<int>("samples");
+ if (!bt_samples)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_trans_vel_samples-> Incorrect or missing input. It needs the following input ports: samples(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ int samples=bt_samples.value();
+ if(this->module.set_trans_vel_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_steer_angle_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_steer_angle_samples-> get_steer_angle_samples");
+
+ unsigned int samples;
+ if(this->module.get_steer_angle_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("samples", samples);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_steer_angle_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_steer_angle_samples-> set_steer_angle_samples");
+
+ BT::Optional<int> bt_samples = self.getInput<int>("samples");
+ if (!bt_samples)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_steer_angle_samples-> Incorrect or missing input. It needs the following input ports: samples(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ int samples=bt_samples.value();
+ if(this->module.set_steer_angle_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_angular_vel_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_angular_vel_samples-> get_angular_vel_samples");
+
+ unsigned int samples;
+ if(this->module.get_angular_vel_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("samples", samples);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_angular_vel_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_angular_vel_samples-> set_angular_vel_samples");
+
+ BT::Optional<int> bt_samples = self.getInput<int>("samples");
+ if (!bt_samples)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_angular_vel_samples-> Incorrect or missing input. It needs the following input ports: samples(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ int samples=bt_samples.value();
+ if(this->module.set_angular_vel_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_cmd_vel_average_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_cmd_vel_average_samples-> get_cmd_vel_average_samples");
+
+ unsigned int samples;
+ if(this->module.get_cmd_vel_average_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("samples", samples);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_cmd_vel_average_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_cmd_vel_average_samples-> set_cmd_vel_average_samples");
+
+ BT::Optional<int> bt_samples = self.getInput<int>("samples");
+ if (!bt_samples)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_cmd_vel_average_samples-> Incorrect or missing input. It needs the following input ports: samples(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ int samples=bt_samples.value();
+ if(this->module.set_cmd_vel_average_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_odom_average_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_odom_average_samples-> get_odom_average_samples");
+
+ unsigned int samples;
+ if(this->module.get_odom_average_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("samples", samples);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_odom_average_samples(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_odom_average_samples-> set_odom_average_samples");
+
+ BT::Optional<int> bt_samples = self.getInput<int>("samples");
+ if (!bt_samples)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_odom_average_samples-> Incorrect or missing input. It needs the following input ports: samples(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ int samples=bt_samples.value();
+ if(this->module.set_odom_average_samples(samples)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_heading_cost_points(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_heading_cost_points-> get_heading_cost_points");
+
+ unsigned int num;
+ if(this->module.get_heading_cost_points(num)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("num", num);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_heading_cost_points(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_heading_cost_points-> set_heading_cost_points");
+
+ BT::Optional<int> bt_num = self.getInput<int>("num");
+ if (!bt_num)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_heading_cost_points-> Incorrect or missing input. It needs the following input ports: num(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ int num=bt_num.value();
+ if(this->module.set_heading_cost_points(num)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_simulation_time(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_simulation_time-> get_trans_vel_samples");
+
+ double min,max;
+ if(this->module.get_simulation_time(min,max)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("min", min);
+ self.setOutput("max", max);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_simulation_time(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_simulation_time-> set_simulation_time");
+
+ BT::Optional<double> bt_min = self.getInput<double>("min");
+ BT::Optional<double> bt_max = self.getInput<double>("max");
+ if (!bt_min || !bt_max)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_simulation_time-> Incorrect or missing input. It needs the following input ports: min(double) max(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double min=bt_min.value();
+ double max=bt_max.value();
+ if(this->module.set_simulation_time(min,max)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_simulation_res(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_simulation_res-> get_simulation_res");
+
+ double linear,angular;
+ if(this->module.get_simulation_res(linear,angular)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("linear", linear);
+ self.setOutput("angular", angular);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_simulation_res(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_simulation_res-> set_simulation_res");
+
+ BT::Optional<double> bt_linear = self.getInput<double>("linear");
+ BT::Optional<double> bt_angular = self.getInput<double>("angular");
+ if (!bt_linear || !bt_angular)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_simulation_res-> Incorrect or missing input. It needs the following input ports: linear(double) angular(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double linear=bt_linear.value();
+ double angular=bt_angular.value();
+ if(this->module.set_simulation_res(linear,angular)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_path_distance_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_path_distance_cost-> get_path_distance_cost");
+
+ double cost;
+ if(this->module.get_path_distance_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("cost", cost);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_path_distance_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_path_distance_cost-> set_path_distance_cost");
+
+ BT::Optional<double> bt_cost = self.getInput<double>("cost");
+ if (!bt_cost)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_path_distance_cost-> Incorrect or missing input. It needs the following input ports: cost(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double cost=bt_cost.value();
+ if(this->module.set_path_distance_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_goal_distance_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_goal_distance_cost-> get_goal_distance_cost");
+
+ double cost;
+ if(this->module.get_goal_distance_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("cost", cost);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_goal_distance_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_goal_distance_cost-> set_goal_distance_cost");
+
+ BT::Optional<double> bt_cost = self.getInput<double>("cost");
+ if (!bt_cost)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_goal_distance_cost-> Incorrect or missing input. It needs the following input ports: cost(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double cost=bt_cost.value();
+ if(this->module.set_goal_distance_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_obstacle_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_obstacle_cost-> get_obstacle_cost");
+
+ double cost;
+ if(this->module.get_obstacle_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("cost", cost);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_obstacle_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_obstacle_cost-> set_obstacle_cost");
+
+ BT::Optional<double> bt_cost = self.getInput<double>("cost");
+ if (!bt_cost)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_obstacle_cost-> Incorrect or missing input. It needs the following input ports: cost(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double cost=bt_cost.value();
+ if(this->module.set_obstacle_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_heading_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_heading_cost-> get_heading_cost");
+
+ double cost;
+ if(this->module.get_heading_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("cost", cost);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_heading_cost(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_heading_cost-> set_heading_cost");
+
+ BT::Optional<double> bt_cost = self.getInput<double>("cost");
+ if (!bt_cost)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_heading_cost-> Incorrect or missing input. It needs the following input ports: cost(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double cost=bt_cost.value();
+ if(this->module.set_heading_cost(cost)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_goal_tolerances(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_goal_tolerances-> get_goal_tolerances");
+
+ double xy,yaw;
+ if(this->module.get_goal_tolerances(xy,yaw)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("xy_tolerance", xy);
+ self.setOutput("yaw_tolerance", yaw);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_goal_tolerances(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_goal_tolerances-> set_goal_tolerances");
+
+ 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("CAckermannLPModuleBT::set_goal_tolerances-> 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_tolerances(xy,yaw)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_linear_velocity_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_linear_velocity_range-> get_linear_velocity_range");
+
+ double max,min;
+ if(this->module.get_linear_velocity_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max", max);
+ self.setOutput("min", min);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_linear_velocity_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_linear_velocity_range-> set_linear_velocity_range");
+
+ BT::Optional<double> bt_min = self.getInput<double>("min");
+ BT::Optional<double> bt_max = self.getInput<double>("max");
+ if (!bt_min || !bt_max)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_linear_velocity_range-> Incorrect or missing input. It needs the following input ports: max(double) min(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double min=bt_min.value();
+ double max=bt_max.value();
+ if(this->module.set_linear_velocity_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_angular_velocity_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_angular_velocity_range-> get_angular_velocity_range");
+
+ double max,min;
+ if(this->module.get_angular_velocity_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max", max);
+ self.setOutput("min", min);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_angular_velocity_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_angular_velocity_range-> set_angular_velocity_range");
+
+ BT::Optional<double> bt_min = self.getInput<double>("min");
+ BT::Optional<double> bt_max = self.getInput<double>("max");
+ if (!bt_min || !bt_max)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_angular_velocity_range-> Incorrect or missing input. It needs the following input ports: max(double) min(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double min=bt_min.value();
+ double max=bt_max.value();
+ if(this->module.set_angular_velocity_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_linear_max_accel(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_linear_max_accel-> get_linear_max_accel");
+
+ double max;
+ if(this->module.get_linear_max_accel(max)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max", max);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_linear_max_accel(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_linear_max_accel-> set_linear_max_accel");
+
+ BT::Optional<double> bt_max = self.getInput<double>("max");
+ if (!bt_max)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_linear_max_accel-> Incorrect or missing input. It needs the following input ports: max(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double max=bt_max.value();
+ if(this->module.set_linear_max_accel(max)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_steer_max_accel(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_steer_max_accel-> get_steer_max_accel");
+
+ double max;
+ if(this->module.get_steer_max_accel(max)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max", max);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_steer_max_accel(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_steer_max_accel-> set_steer_max_accel");
+
+ BT::Optional<double> bt_max = self.getInput<double>("max");
+ if (!bt_max)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_steer_max_accel-> Incorrect or missing input. It needs the following input ports: max(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double max=bt_max.value();
+ if(this->module.set_steer_max_accel(max)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_steer_angle_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_steer_angle_range-> get_steer_angle_range");
+
+ double max,min;
+ if(this->module.get_steer_angle_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max", max);
+ self.setOutput("min", min);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_steer_angle_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_steer_angle_range-> set_steer_angle_range");
+
+ BT::Optional<double> bt_min = self.getInput<double>("min");
+ BT::Optional<double> bt_max = self.getInput<double>("max");
+ if (!bt_min || !bt_max)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_steer_angle_range-> Incorrect or missing input. It needs the following input ports: max(double) min(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double min=bt_min.value();
+ double max=bt_max.value();
+ if(this->module.set_steer_angle_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_steer_velocity_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::-get_steer_velocity_range-> get_steer_velocity_range");
+
+ double max,min;
+ if(this->module.get_steer_velocity_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("max", max);
+ self.setOutput("min", min);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_steer_velocity_range(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_steer_velocity_range-> set_steer_velocity_range");
+
+ BT::Optional<double> bt_min = self.getInput<double>("min");
+ BT::Optional<double> bt_max = self.getInput<double>("max");
+ if (!bt_min || !bt_max)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_steer_velocity_range-> Incorrect or missing input. It needs the following input ports: max(double) min(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double min=bt_min.value();
+ double max=bt_max.value();
+ if(this->module.set_steer_velocity_range(max,min)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_min_segment_length(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_min_segment_length-> get_min_segment_length");
+
+ double length;
+ if(this->module.get_min_segment_length(length)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("length", length);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_min_segment_length(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_min_segment_length-> set_min_segment_length");
+
+ BT::Optional<double> bt_length = self.getInput<double>("length");
+ if (!bt_length)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_min_segment_length-> Incorrect or missing input. It needs the following input ports: length(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double length=bt_length.value();
+ if(this->module.set_min_segment_length(length)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_axis_distance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_axis_distance-> get_axis_distance");
+
+ double distance;
+ if(this->module.get_axis_distance(distance)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("distance", distance);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_axis_distance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_axis_distance-> set_axis_distance");
+
+ BT::Optional<double> bt_distance = self.getInput<double>("distance");
+ if (!bt_distance)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_axis_distance-> Incorrect or missing input. It needs the following input ports: distance(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double distance=bt_distance.value();
+ if(this->module.set_axis_distance(distance)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_wheel_distance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_wheel_distance-> get_wheel_distance");
+
+ double distance;
+ if(this->module.get_wheel_distance(distance)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("distance", distance);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_wheel_distance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_wheel_distance-> set_wheel_distance");
+
+ BT::Optional<double> bt_distance = self.getInput<double>("distance");
+ if (!bt_distance)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_wheel_distance-> Incorrect or missing input. It needs the following input ports: distance(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double distance=bt_distance.value();
+ if(this->module.set_wheel_distance(distance)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::get_wheel_radius(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::get_wheel_radius-> get_wheel_radius");
+
+ double radius;
+ if(this->module.get_wheel_radius(radius)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("radius", radius);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::set_wheel_radius(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::set_wheel_radius-> set_wheel_radius");
+
+ BT::Optional<double> bt_radius = self.getInput<double>("radius");
+ if (!bt_radius)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::set_wheel_radius-> Incorrect or missing input. It needs the following input ports: radius(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double radius=bt_radius.value();
+ if(this->module.set_wheel_radius(radius)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::use_inverted_ackermann(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::use_inverted_ackermann-> use_inverted_ackermann");
+
+ bool inverted;
+ if(this->module.use_inverted_ackermann(inverted)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("inverted", inverted);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::use_steering_while_stopped(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::use_steering_while_stopped-> use_steering_while_stopped");
+
+ BT::Optional<bool> bt_enabled = self.getInput<bool>("enabled");
+ BT::Optional<double> bt_threshold = self.getInput<double>("threshold");
+ if (!bt_enabled || !bt_threshold)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::use_steering_while_stopped-> Incorrect or missing input. It needs the following input ports: enabled(bool) threshold(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ bool enabled=bt_enabled.value();
+ double threshold=bt_threshold.value();
+ if(this->module.use_steering_while_stopped(enabled,threshold)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CAckermannLPModuleBT<ModuleCfg>::use_stuck_check(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CAckermannLPModuleBT::use_stuck_check-> use_stuck_check");
+
+ BT::Optional<bool> bt_enabled = self.getInput<bool>("enabled");
+ BT::Optional<double> bt_threshold = self.getInput<double>("threshold");
+ BT::Optional<int> bt_max_num = self.getInput<int>("max_num");
+ if (!bt_enabled || !bt_threshold || !bt_max_num)
+ {
+ ROS_ERROR("CAckermannLPModuleBT::use_stuck_check-> Incorrect or missing input. It needs the following input ports: enabled(bool) threshold(double) max_num(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ bool enabled=bt_enabled.value();
+ double threshold=bt_threshold.value();
+ int max_num=bt_max_num.value();
+ if(this->module.use_stuck_check(enabled,threshold,max_num)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+#endif
diff --git a/include/iri_nav_module/nav_costmap_module_bt.h b/include/iri_nav_module/nav_costmap_module_bt.h
new file mode 100644
index 0000000000000000000000000000000000000000..9676f61e36fbe17999a9d5e7ee3982e79b43f92b
--- /dev/null
+++ b/include/iri_nav_module/nav_costmap_module_bt.h
@@ -0,0 +1,424 @@
+#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
diff --git a/include/iri_nav_module/nav_module_bt.h b/include/iri_nav_module/nav_module_bt.h
new file mode 100644
index 0000000000000000000000000000000000000000..878fd3146c72486f9f8465a7178ba68ac421dae4
--- /dev/null
+++ b/include/iri_nav_module/nav_module_bt.h
@@ -0,0 +1,609 @@
+#ifndef _NAV_MODULE_BT_H
+#define _NAV_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <iri_nav_module/nav_module.h>
+
+template <class ModuleCfg>
+class CNavModuleBT
+{
+ private:
+ // object of nav_module
+ CNavModule<ModuleCfg> &nav_module;
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CNavModuleBT(CNavModule<ModuleCfg> &module);
+
+ /**
+ * \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.
+ */
+ ~CNavModuleBT();
+
+ protected:
+ // Actions
+ /**
+ * \brief Synchronized set_adc_nav_goal_frame adc nav function.
+ *
+ * This function calls set_adc_nav_goal_frame of adc_nav_module.
+ *
+ * It has the following input ports:
+ *
+ * frame_id (std::string): name of the new reference frame for the future goals.
+ * This frame can be any that exist inside the TF tree
+ * of the robot.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus set_goal_frame(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized costmaps_clear add_sbpl nav function.
+ *
+ * This function calls costmaps_clear of nav_module.
+ *
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus costmaps_clear(void);
+
+ /**
+ * \brief Synchronized costmaps_enable_auto_clear add_sbpl nav function.
+ *
+ * This function calls costmaps_enable_auto_clear of nav_module.
+ *
+ * It has the following input ports:
+ *
+ * rate_hz (double): the desired clearing rate in Hz. This value should be less
+ * than 1 Hz, normally in the range of 0.1 to 0.01 Hz.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus costmaps_enable_auto_clear(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized costmaps_disable_auto_clear add_sbpl nav function.
+ *
+ * This function calls costmaps_disable_auto_clear of nav_module.
+ *
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus costmaps_disable_auto_clear(void);
+
+ /**
+ * \brief Synchronized getCurrentPose add_sbpl nav function.
+ *
+ * This function calls getCurrentPose of nav_module.
+ *
+ * It has the following output ports:
+ *
+ * pose (geometry_msgs::Pose): current pose.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus get_current_pose(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized getGoalDistance add_sbpl nav function.
+ *
+ * This function calls getGoalDistance of nav_module.
+ *
+ * It has the following output ports:
+ *
+ * distance (double): current distance to goal.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus get_goal_distance(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized getGoalDistance add_sbpl nav function.
+ *
+ * This function calls getGoalDistance of nav_module.
+ *
+ * It has the following output ports:
+ *
+ * distance (double): current distance to goal.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus get_path_length(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus costmaps_shutdown(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus get_max_num_retries(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus set_max_num_retries(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus get_planner_frequency(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus set_planner_frequency(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus get_planner_patience(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus set_planner_patience(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus get_controller_frequency(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus set_controller_frequency(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus get_controller_patience(BT::TreeNode& self);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus set_controller_patience(BT::TreeNode& self);
+
+ // conditions
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus are_costmaps_shutdown(void);
+
+ /**
+ * \brief
+ *
+ */
+ BT::NodeStatus is_costmap_autoclear_enabled(void);
+ };
+
+ template <class ModuleCfg>
+ CNavModuleBT<ModuleCfg>::CNavModuleBT(CNavModule<ModuleCfg> &module) :
+ nav_module(module)
+ {
+ }
+
+ template <class ModuleCfg>
+ void CNavModuleBT<ModuleCfg>::init(IriBehaviorTreeFactory &factory)
+ {
+ std::string name;
+
+ //Definition of ports
+ BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")};
+ BT::PortsList auto_clear_ports = {BT::InputPort<double>("rate_hz")};
+ BT::PortsList current_pose_ports = {BT::OutputPort<geometry_msgs::Pose>("pose")};
+ BT::PortsList goal_distance_ports = {BT::OutputPort<double>("distance")};
+ BT::PortsList path_length_ports = {BT::OutputPort<double>("length")};
+ BT::PortsList costmaps_shutdown_ports = {BT::InputPort<bool>("shutdown")};
+ BT::PortsList set_max_retries_ports = {BT::InputPort<int>("retries")};
+ BT::PortsList get_max_retries_ports = {BT::OutputPort<int>("retries")};
+ BT::PortsList set_planner_freq_ports = {BT::InputPort<double>("freq")};
+ BT::PortsList get_planner_freq_ports = {BT::OutputPort<int>("freq")};
+ BT::PortsList set_planner_patience_ports = {BT::InputPort<double>("time")};
+ BT::PortsList get_planner_patience_ports = {BT::OutputPort<int>("time")};
+ BT::PortsList set_controller_freq_ports = {BT::InputPort<double>("freq")};
+ BT::PortsList get_controller_freq_ports = {BT::OutputPort<int>("freq")};
+ BT::PortsList set_controller_patience_ports = {BT::InputPort<double>("time")};
+ BT::PortsList get_controller_patience_ports = {BT::OutputPort<int>("time")};
+
+ std::size_t found = this->nav_module.get_name().find_last_of("/\\");
+ name=this->nav_module.get_name().substr(found+1);
+
+ // Registration of conditions
+ factory.registerSimpleCondition(name+"_are_costmaps_shutdown", std::bind(&CNavModuleBT::are_costmaps_shutdown, this));
+ factory.registerSimpleCondition(name+"_is_costmap_autoclear_enabled", std::bind(&CNavModuleBT::is_costmap_autoclear_enabled, this));
+
+ //Registration of actions
+ factory.registerSimpleAction(name+"_set_goal_frame", std::bind(&CNavModuleBT::set_goal_frame, this, std::placeholders::_1), set_goal_frame_ports);
+ factory.registerSimpleAction(name+"_costmaps_clear", std::bind(&CNavModuleBT::costmaps_clear, this));
+ factory.registerSimpleAction(name+"_costmaps_enable_auto_clear", std::bind(&CNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), auto_clear_ports);
+ factory.registerSimpleAction(name+"_costmaps_disable_auto_clear", std::bind(&CNavModuleBT::costmaps_disable_auto_clear, this));
+ factory.registerSimpleAction(name+"_get_current_pose", std::bind(&CNavModuleBT::get_current_pose, this, std::placeholders::_1), current_pose_ports);
+ factory.registerSimpleAction(name+"_get_goal_distance", std::bind(&CNavModuleBT::get_goal_distance, this, std::placeholders::_1), goal_distance_ports);
+ factory.registerSimpleAction(name+"_get_path_length", std::bind(&CNavModuleBT::get_path_length, this, std::placeholders::_1), path_length_ports);
+ factory.registerSimpleAction(name+"_costmaps_shutdown", std::bind(&CNavModuleBT::costmaps_shutdown, this,std::placeholders::_1),costmaps_shutdown_ports);
+ factory.registerSimpleAction(name+"_set_max_retries", std::bind(&CNavModuleBT::set_max_num_retries, this,std::placeholders::_1),set_max_retries_ports);
+ factory.registerSimpleAction(name+"_get_max_retries", std::bind(&CNavModuleBT::get_max_num_retries, this,std::placeholders::_1),get_max_retries_ports);
+ factory.registerSimpleAction(name+"_set_planner_frequency", std::bind(&CNavModuleBT::set_planner_frequency, this,std::placeholders::_1),set_planner_freq_ports);
+ factory.registerSimpleAction(name+"_get_planner_frequency", std::bind(&CNavModuleBT::get_planner_frequency, this,std::placeholders::_1),get_planner_freq_ports);
+ factory.registerSimpleAction(name+"_set_planner_patience", std::bind(&CNavModuleBT::set_planner_patience, this,std::placeholders::_1),set_planner_patience_ports);
+ factory.registerSimpleAction(name+"_get_planner_patience", std::bind(&CNavModuleBT::get_planner_patience, this,std::placeholders::_1),get_planner_patience_ports);
+ factory.registerSimpleAction(name+"_set_controller_frequency", std::bind(&CNavModuleBT::set_controller_frequency, this,std::placeholders::_1),set_controller_freq_ports);
+ factory.registerSimpleAction(name+"_get_controller_frequency", std::bind(&CNavModuleBT::get_controller_frequency, this,std::placeholders::_1),get_controller_freq_ports);
+ factory.registerSimpleAction(name+"_set_controller_patience", std::bind(&CNavModuleBT::set_controller_patience, this,std::placeholders::_1),set_controller_patience_ports);
+ factory.registerSimpleAction(name+"_get_controller_patience", std::bind(&CNavModuleBT::get_controller_patience, this,std::placeholders::_1),get_controller_patience_ports);
+ }
+
+ template <class ModuleCfg>
+ CNavModuleBT<ModuleCfg>::~CNavModuleBT(void)
+ {
+ // [free dynamic memory]
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::set_goal_frame(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::set_goal_frame-> set_goal_frame");
+ BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
+
+ std::string frame_id_goal;
+ if (!frame_id)
+ {
+ ROS_ERROR("CNavModuleBT::set_goal_frame-> Incorrect or missing input. It needs the following input ports: frame_id(std::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ frame_id_goal = frame_id.value();
+
+ this->nav_module.set_goal_frame_id(frame_id_goal);
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::costmaps_clear(void)
+ {
+ ROS_DEBUG("CNavModuleBT::costmaps_clear-> costmaps_clear");
+ if (this->nav_module.costmaps_clear())
+ return BT::NodeStatus::SUCCESS;
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::costmaps_enable_auto_clear(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::costmaps_enable_auto_clear-> costmaps_enable_auto_clear");
+ BT::Optional<double> rate_hz = self.getInput<double>("rate_hz");
+
+ double rate_hz_goal;
+ if (!rate_hz)
+ {
+ ROS_ERROR("CNavModuleBT::costmaps_enable_auto_clear-> Incorrect or missing input. It needs the following input ports: rate_hz(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ rate_hz_goal = rate_hz.value();
+
+ this->nav_module.costmaps_enable_auto_clear(rate_hz_goal);
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::costmaps_disable_auto_clear(void)
+ {
+ ROS_DEBUG("CNavModuleBT::costmaps_disable_auto_clear-> costmaps_disable_auto_clear");
+ this->nav_module.costmaps_disable_auto_clear();
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_pose(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_current_pose-> get_current_pose");
+
+ geometry_msgs::Pose pose_out = this->nav_module.get_current_pose();
+
+ self.setOutput("pose", pose_out);
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_goal_distance(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_goal_distance-> get_goal_distance");
+
+ double dist = this->nav_module.get_goal_distance();
+ if(dist!=-1.0)
+ {
+ self.setOutput("distance", dist);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_path_length(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_goal_distance-> get_goal_distance");
+
+ double length = this->nav_module.get_path_length();
+ if(length!=-1.0)
+ {
+ self.setOutput("length", length);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::costmaps_shutdown(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT:: costmaps_shutdown-> costmaps_shutdown");
+
+ BT::Optional<bool> bt_shutdown = self.getInput<bool>("shutdown");
+ if (!bt_shutdown)
+ {
+ ROS_ERROR("CNavModuleBT::costmaps_shutdown-> Incorrect or missing input. It needs the following input ports: shutdown(bool)");
+ return BT::NodeStatus::FAILURE;
+ }
+ bool shutdown=bt_shutdown.value();
+ if(this->nav_module.shutdown_costmaps(shutdown)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_max_num_retries(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_max_num_retries-> get_max_num_retries");
+
+ unsigned int num;
+ if(this->nav_module.get_max_num_retries(num)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("retries", num);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::set_max_num_retries(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::set_max_num_retries -> set_max_num_retries");
+
+ BT::Optional<int> bt_num = self.getInput<int>("retries");
+ if (!bt_num)
+ {
+ ROS_ERROR("CNavModuleBT::set_max_num_retries-> Incorrect or missing input. It needs the following input ports: num(int)");
+ return BT::NodeStatus::FAILURE;
+ }
+ unsigned int num=bt_num.value();
+ if(this->nav_module.set_max_num_retries(num)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_planner_frequency(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_planner_frequency-> get_planner_frequency");
+
+ double freq;
+ if(this->nav_module.get_planner_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("freq", freq);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::set_planner_frequency(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT:: set_planner_frequency-> set_planner_frequency");
+
+ BT::Optional<double> bt_freq = self.getInput<double>("freq");
+ if (!bt_freq)
+ {
+ ROS_ERROR("CNavModuleBT::set_planner_frequency-> Incorrect or missing input. It needs the following input ports: freq(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double freq=bt_freq.value();
+ if(this->nav_module.set_planner_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_planner_patience(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_planner_patience-> get_planner_patience");
+
+ double pat;
+ if(this->nav_module.get_planner_patience(pat)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("time", pat);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::set_planner_patience(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT:: set_planner_patience-> set_planner_patience");
+
+ BT::Optional<double> bt_pat = self.getInput<double>("time");
+ if (!bt_pat)
+ {
+ ROS_ERROR("CNavModuleBT::set_planner_patience-> Incorrect or missing input. It needs the following input ports: time(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double pat=bt_pat.value();
+ if(this->nav_module.set_planner_patience(pat)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_controller_frequency(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_controller_frequency-> get_controller_frequency");
+
+ double freq;
+ if(this->nav_module.get_controller_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("freq", freq);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::set_controller_frequency(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT:: set_controller_frequency-> set_controller_frequency");
+
+ BT::Optional<double> bt_freq = self.getInput<double>("freq");
+ if (!bt_freq)
+ {
+ ROS_ERROR("CNavModuleBT::set_controller_frequency-> Incorrect or missing input. It needs the following input ports: freq(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double freq=bt_freq.value();
+ if(this->nav_module.set_controller_frequency(freq)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_controller_patience(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_controller_patience-> get_controller_patience");
+
+ double pat;
+ if(this->nav_module.get_controller_patience(pat)==DYN_RECONF_SUCCESSFULL)
+ {
+ self.setOutput("time", pat);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::set_controller_patience(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT:: set_controller_patience-> set_controller_patience");
+
+ BT::Optional<double> bt_pat = self.getInput<double>("time");
+ if (!bt_pat)
+ {
+ ROS_ERROR("CNavModuleBT::set_controller_patience-> Incorrect or missing input. It needs the following input ports: time(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ double pat=bt_pat.value();
+ if(this->nav_module.set_controller_patience(pat)==DYN_RECONF_SUCCESSFULL)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::are_costmaps_shutdown(void)
+ {
+ bool shutdown;
+
+ if(this->nav_module.are_costmaps_shutdown(shutdown)==DYN_RECONF_SUCCESSFULL)
+ {
+ if(shutdown)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::RUNNING;
+ }
+ return BT::NodeStatus::RUNNING;
+ }
+
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::is_costmap_autoclear_enabled(void)
+ {
+ if(this->nav_module.costamp_is_auto_clear_enabled())
+ return BT::NodeStatus::SUCCESS;
+ return BT::NodeStatus::RUNNING;
+ }
+
+#endif
diff --git a/include/iri_nav_module/sbpl_gp_module.h b/include/iri_nav_module/sbpl_gp_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..71b5bfa7d9e215798a02580c56fc467224d9e3fd
--- /dev/null
+++ b/include/iri_nav_module/sbpl_gp_module.h
@@ -0,0 +1,50 @@
+#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