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 b4971cf33c79266f8bdf39dd77c4978797370249..aa2b965b04ef7c336141e4a8ce4a2f100ccdd7e4 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 @@ -471,7 +471,7 @@ class CIriAnaNavModuleBT BT::NodeStatus succeded_or_recovery(BT::TreeNode& self); - BT::NodeStatus is_recovery_demanded(BT::TreeNode& self); + BT::NodeStatus is_recovery_in_progress(BT::TreeNode& self); BT::NodeStatus is_goal_succeded(BT::TreeNode& self); diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp index 91e35769d7244ff0ea250a1aa6f77d9a4b4d77ea..52f789a2b4b86caffa639def9072577bbb2a8504 100644 --- a/src/iri_ana_nav_module_bt.cpp +++ b/src/iri_ana_nav_module_bt.cpp @@ -24,12 +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 recovery_port = { BT::InputPort<bool>("recovery_active") }; 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_recovery_in_progress", std::bind(&CIriAnaNavModuleBT::is_recovery_in_progress, 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)); @@ -60,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); + factory.registerIriAsyncAction("succeded_or_recovery", std::bind(&CIriAnaNavModuleBT::succeded_or_recovery, this, std::placeholders::_1), success_port); } CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void) @@ -592,37 +592,37 @@ BT::NodeStatus CIriAnaNavModuleBT::NOP() 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"); + BT::Optional<bool> goal_reached_input = self.getInput<bool>("goal_reached"); - if (!recovery_activated_input) + if (!goal_reached_input) { - ROS_ERROR("Incorrect or missing required input [recovery_activated]"); + ROS_ERROR("Incorrect or missing required input [goal_reached]"); return BT::NodeStatus::FAILURE; } - bool recovery_activated = recovery_activated_input.value(); + bool goal_reached = goal_reached_input.value(); - if (recovery_activated) + if (goal_reached) { return BT::NodeStatus::SUCCESS; } return BT::NodeStatus::RUNNING; } -BT::NodeStatus CIriAnaNavModuleBT::is_recovery_demanded(BT::TreeNode& self) +BT::NodeStatus CIriAnaNavModuleBT::is_recovery_in_progress(BT::TreeNode& self) { - ROS_INFO("recovery demanded?"); - BT::Optional<bool> recovery_activated_input = self.getInput<bool>("recovery_activated"); + ROS_INFO("recovery active?"); + BT::Optional<bool> recovery_active_input = self.getInput<bool>("recovery_active"); - if (!recovery_activated_input) + if (!recovery_active_input) { - ROS_ERROR("Incorrect or missing required input [recovery_activated]"); + ROS_ERROR("Incorrect or missing required input [recovery_active]"); return BT::NodeStatus::FAILURE; } - bool recovery_activated = recovery_activated_input.value(); + bool recovery_active = recovery_active_input.value(); - if (recovery_activated) + if (recovery_active) { return BT::NodeStatus::SUCCESS; }