get_distance_to_goal added

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
       *

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");