From ef896d2dbe99ef43f172037a735b451dc5a00315 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Enrique=20Dom=C3=ADnguez?= <jdominguez@iri.upc.edu> Date: Tue, 14 Jan 2020 09:23:25 +0100 Subject: [PATCH] get_distance_to_goal added --- .../iri_ana_nav_module_bt.h | 2 + src/iri_ana_nav_module_bt.cpp | 48 +++++++++++++++++++ 2 files changed, 50 insertions(+) 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 60075e7..3658e12 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 @@ -494,6 +494,8 @@ class CIriAnaNavModuleBT * */ BT::NodeStatus costmaps_clear(); + + BT::NodeStatus get_distance_to_goal(BT::TreeNode& self); /** * \brief Enable the autoclear of the costmaps * diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp index 07884c6..91abef3 100644 --- a/src/iri_ana_nav_module_bt.cpp +++ b/src/iri_ana_nav_module_bt.cpp @@ -27,6 +27,7 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory) BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") }; BT::PortsList recovery_port = { BT::InputPort<bool>("recovery_active") }; BT::PortsList success_port = { BT::InputPort<bool>("goal_reached") }; + BT::PortsList distance_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("dist_threshold") }; // registry of conditions factory.registerSimpleCondition("is_goal_succeded", std::bind(&CIriAnaNavModuleBT::is_goal_succeded, this, std::placeholders::_1), success_port); @@ -57,6 +58,7 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory) factory.registerSimpleAction("costmaps_enable_auto_clear", std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port); factory.registerSimpleAction("costmaps_disable_auto_clear", std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this)); factory.registerSimpleAction("reset_goal_variables", std::bind(&CIriAnaNavModuleBT::reset_goal_variables, this)); + factory.registerSimpleAction("get_distance_to_goal", std::bind(&CIriAnaNavModuleBT::get_distance_to_goal, this, std::placeholders::_1), distance_port); // registry of asynchronous actions factory.registerIriAsyncAction("async_go_to_orientation", std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), async_orientation_port); factory.registerIriAsyncAction("async_go_to_position", std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), async_position_port); @@ -721,6 +723,52 @@ BT::NodeStatus CIriAnaNavModuleBT::costmaps_clear() return BT::NodeStatus::SUCCESS; } +BT::NodeStatus CIriAnaNavModuleBT::get_distance_to_goal(BT::TreeNode& self) +{ + ROS_INFO("get_distance_to_goal"); + + geometry_msgs::Pose current_pose = nav.getCurrentPose(); + + BT::Optional<double> x_input = self.getInput<double>("x"); + BT::Optional<double> y_input = self.getInput<double>("y"); + BT::Optional<double> dist_threshold_input = self.getInput<double>("dist_threshold"); + + if (!x_input) + { + ROS_ERROR("Incorrect or missing required input [x]"); + return BT::NodeStatus::FAILURE; + } + if (!y_input) + { + ROS_ERROR("Incorrect or missing required input [y]"); + return BT::NodeStatus::FAILURE; + } + if (!dist_threshold_input) + { + ROS_ERROR("Incorrect or missing required input [dist_threshold]"); + return BT::NodeStatus::FAILURE; + } + + double x_goal = x_input.value(); + double y_goal = y_input.value(); + double dist_threshold = dist_threshold_input.value(); + + double x_current = current_pose.position.x; + double y_current = current_pose.position.y; + + ROS_INFO("The current pose is: x-> [%f], y-> [%f], yaw-> [%f]", current_pose.position.x, current_pose.position.y, tf::getYaw(current_pose.orientation)); + + float distance_to_goal = sqrt((x_goal-x_current)*(x_goal-x_current) + (y_goal-y_current)*(y_goal-y_current)); + + ROS_INFO("The distance is: %f", distance_to_goal); + + if (distance_to_goal < dist_threshold) { + return BT::NodeStatus::FAILURE; + } + + return BT::NodeStatus::SUCCESS; +} + BT::NodeStatus CIriAnaNavModuleBT::costmaps_enable_auto_clear(BT::TreeNode& self) { ROS_INFO("costmaps_enable_auto_clear"); -- GitLab