diff --git a/cfg/IriAnaNavModule.cfg b/cfg/IriAnaNavModule.cfg index 5e6165d514110a415af145e85046ba273f06ae1d..46bfb645490501893143846508b906cece95da45 100755 --- a/cfg/IriAnaNavModule.cfg +++ b/cfg/IriAnaNavModule.cfg @@ -49,9 +49,9 @@ op_mode = gen.add_group("Set operation mode") gen.add("rate_hz", double_t, 0, "Navigation module rate in Hz", 1.0, 1.0, 100.0) move_base_action.add("mb_max_retries",int_t, 0, "Maximum number of retries for the action start", 1, 1, 10) -move_base_action.add("mb_feedback_watchdog_time_s",double_t,0, "Maximum time between feedback messages", 10, 0.01, 600) +move_base_action.add("mb_feedback_watchdog_time_s",double_t,0, "Maximum time between feedback messages", 600, 0.01, 600) move_base_action.add("mb_enable_watchdog",bool_t, 0, "Enable action watchdog", True) -move_base_action.add("mb_timeout_s",double_t, 0, "Maximum time allowed to complete the action", 300, 0.1, 600) +move_base_action.add("mb_timeout_s",double_t, 0, "Maximum time allowed to complete the action", 6000, 0.1, 6000) move_base_action.add("mb_enable_timeout",bool_t, 0, "Enable action timeout", True) move_base_action.add("mb_frame_id",str_t, 0, "Reference frame of the position goals","/map") move_base_action.add("mb_enabled", bool_t, 0, "Enable action execution", True) @@ -73,4 +73,3 @@ op_mode.add("op_mode_max_retries", int_t, 0, op_mode.add("op_mode_enabled", bool_t, 0, "Enable service call", True) exit(gen.generate(PACKAGE, "IriAnaNavModuleAlgorithm", "IriAnaNavModule")) - diff --git a/config/nav_module_default.yaml b/config/nav_module_default.yaml index 6d7a8326d400346a09199c8b9b218a7339975519..5a4732aa7588f2c0a922a29f1f2ed39f015593cb 100644 --- a/config/nav_module_default.yaml +++ b/config/nav_module_default.yaml @@ -1,19 +1,19 @@ rate_hz: 1.0 mb_max_retries: 1 -mb_feedback_watchdog_time_s: 10.0 +mb_feedback_watchdog_time_s: 60.0 mb_enable_watchdog: True -mb_timeout_s: 300.0 +mb_timeout_s: 6000.0 mb_enable_timeout: True mb_frame_id: "/ana/base_link" mb_enabled: True mb_cancel_prev: True -odom_watchdog_time_s: 1.0 +odom_watchdog_time_s: 1.0 -tf_timeout_time_s: 5.0 +tf_timeout_time_s: 5.0 -cm_max_retries: 1.0 +cm_max_retries: 1.0 cm_enable_auto_clear: False cm_auto_clear_rate_hz: 0.1 cm_enabled: True diff --git a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h index bdebb21413e219f138a1ecb002e2fa4803617cc2..b4971cf33c79266f8bdf39dd77c4978797370249 100644 --- a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h +++ b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h @@ -468,6 +468,13 @@ class CIriAnaNavModuleBT * */ BT::NodeStatus NOP(); + + BT::NodeStatus succeded_or_recovery(BT::TreeNode& self); + + BT::NodeStatus is_recovery_demanded(BT::TreeNode& self); + + BT::NodeStatus is_goal_succeded(BT::TreeNode& self); + // Costmaps functions /** * \brief Clears the costmaps diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp index 45e27380c2f8e83f495a744c941ee71114c066b3..91e35769d7244ff0ea250a1aa6f77d9a4b4d77ea 100644 --- a/src/iri_ana_nav_module_bt.cpp +++ b/src/iri_ana_nav_module_bt.cpp @@ -24,8 +24,12 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory) BT::PortsList frame_port = { BT::InputPort<std::string>("frame_id") }; BT::PortsList current_pose_port = { BT::OutputPort<double>("current_x"), BT::OutputPort<double>("current_y"), BT::OutputPort<double>("current_yaw") }; BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") }; + BT::PortsList recovery_port = { BT::InputPort<bool>("recovery_activated") }; + BT::PortsList success_port = { BT::InputPort<bool>("goal_reached") }; // registry of conditions + factory.registerSimpleCondition("is_goal_succeded", std::bind(&CIriAnaNavModuleBT::is_goal_succeded, this, std::placeholders::_1), success_port); + factory.registerSimpleCondition("is_recovery_demanded", std::bind(&CIriAnaNavModuleBT::is_recovery_demanded, this, std::placeholders::_1), recovery_port); factory.registerSimpleCondition("is_finished", std::bind(&CIriAnaNavModuleBT::is_finished, this)); factory.registerSimpleCondition("is_succeded", std::bind(&CIriAnaNavModuleBT::is_succeded, this)); factory.registerSimpleCondition("is_running", std::bind(&CIriAnaNavModuleBT::is_running, this)); @@ -56,7 +60,7 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory) factory.registerIriAsyncAction("async_go_to_position", std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), async_position_port); factory.registerIriAsyncAction("async_go_to_pose", std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), async_pose_port); factory.registerIriAsyncAction("NOP", std::bind(&CIriAnaNavModuleBT::NOP, this)); - + factory.registerIriAsyncAction("succeded_or_recovery", std::bind(&CIriAnaNavModuleBT::succeded_or_recovery, this, std::placeholders::_1), recovery_port); } CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void) @@ -582,9 +586,69 @@ BT::NodeStatus CIriAnaNavModuleBT::get_current_pose(BT::TreeNode& self) BT::NodeStatus CIriAnaNavModuleBT::NOP() { + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus CIriAnaNavModuleBT::succeded_or_recovery(BT::TreeNode& self) +{ + ROS_INFO("succeded_or_recovery?"); + BT::Optional<bool> recovery_activated_input = self.getInput<bool>("recovery_activated"); + + if (!recovery_activated_input) + { + ROS_ERROR("Incorrect or missing required input [recovery_activated]"); + return BT::NodeStatus::FAILURE; + } + + bool recovery_activated = recovery_activated_input.value(); + + if (recovery_activated) + { + return BT::NodeStatus::SUCCESS; + } return BT::NodeStatus::RUNNING; } +BT::NodeStatus CIriAnaNavModuleBT::is_recovery_demanded(BT::TreeNode& self) +{ + ROS_INFO("recovery demanded?"); + BT::Optional<bool> recovery_activated_input = self.getInput<bool>("recovery_activated"); + + if (!recovery_activated_input) + { + ROS_ERROR("Incorrect or missing required input [recovery_activated]"); + return BT::NodeStatus::FAILURE; + } + + bool recovery_activated = recovery_activated_input.value(); + + if (recovery_activated) + { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CIriAnaNavModuleBT::is_goal_succeded(BT::TreeNode& self) +{ + ROS_INFO("goal succeded?"); + BT::Optional<bool> goal_reached_input = self.getInput<bool>("goal_reached"); + + if (!goal_reached_input) + { + ROS_ERROR("Incorrect or missing required input [goal_reached]"); + return BT::NodeStatus::FAILURE; + } + + bool goal_reached = goal_reached_input.value(); + + if (goal_reached) + { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + BT::NodeStatus CIriAnaNavModuleBT::costmaps_clear() { ROS_INFO("costmaps_clear");