diff --git a/include/iri_bt_basic_nodes.h b/include/iri_bt_basic_nodes.h
index 5701dc1212062638aac190ebc311ebb4feb81b49..820043fc826af265625083b6fd2f199070a5c2ae 100644
--- a/include/iri_bt_basic_nodes.h
+++ b/include/iri_bt_basic_nodes.h
@@ -102,6 +102,7 @@ class IriBTBasicNodes
       * returns BT:NodeStatus::FAILURE.
       */
     BT::NodeStatus compute_distance(BT::TreeNode& self);
+    BT::NodeStatus compute_y_distance(BT::TreeNode& self);
 
     /**
       * \brief Function to compare if a double is bigger than other.
diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp
index bef9c7662192b55c6da60ab8ab4d66075e9c5317..59557972a8f92e990076a2b188ad1003e496bb78 100644
--- a/src/iri_bt_basic_nodes.cpp
+++ b/src/iri_bt_basic_nodes.cpp
@@ -21,6 +21,7 @@ void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
   factory.registerIriAsyncAction("RUNNING",  std::bind(&IriBTBasicNodes::RUNNING, this));
   factory.registerSimpleAction("transform_pose", std::bind(&IriBTBasicNodes::transform_pose, this, std::placeholders::_1),transform_pose_ports);
   factory.registerSimpleAction("compute_distance", std::bind(&IriBTBasicNodes::compute_distance, this, std::placeholders::_1),compute_distance_ports);
+  factory.registerSimpleAction("compute_y_distance", std::bind(&IriBTBasicNodes::compute_y_distance, this, std::placeholders::_1),compute_distance_ports);
   factory.registerSimpleAction("compare_bigger", std::bind(&IriBTBasicNodes::compare_bigger, this, std::placeholders::_1),compare_ports);
   factory.registerSimpleAction("compare_smaller", std::bind(&IriBTBasicNodes::compare_smaller, this, std::placeholders::_1),compare_ports);
   factory.registerSimpleAction("print_msg",  std::bind(&IriBTBasicNodes::print_msg, this, std::placeholders::_1), print_msg_ports);
@@ -128,6 +129,37 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
   }
 
   distance=sqrt(pow(pose1_value.pose.position.x-pose2_value.pose.position.x,2.0)+pow(pose1_value.pose.position.y-pose2_value.pose.position.y,2.0)+pow(pose1_value.pose.position.z-pose2_value.pose.position.z,2.0));
+  std::cout << "distance: " << distance << std::endl;
+  self.setOutput("distance",distance);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus IriBTBasicNodes::compute_y_distance(BT::TreeNode& self)
+{
+  ROS_DEBUG("IriBTBasicNodes::compute_y_distance");
+  BT::Optional<geometry_msgs::PoseStamped> pose1 = self.getInput<geometry_msgs::PoseStamped>("pose1");
+  BT::Optional<geometry_msgs::PoseStamped> pose2 = self.getInput<geometry_msgs::PoseStamped>("pose2");
+
+  double distance;
+  self.setOutput("distance",distance);
+
+  if((!pose1) || (!pose2))
+  {
+    ROS_ERROR("IriBTBasicNodes::compute_y_distance-> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  geometry_msgs::PoseStamped pose1_value=pose1.value();
+  geometry_msgs::PoseStamped pose2_value=pose2.value();
+
+  if (pose1_value.header.frame_id != pose1_value.header.frame_id)
+  {
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_y_distance-> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
+    return BT::NodeStatus::FAILURE;
+  }
+
+  distance=fabs(pose1_value.pose.position.y-pose2_value.pose.position.y);
+  std::cout << "distance: " << distance << std::endl;
   self.setOutput("distance",distance);
   return BT::NodeStatus::SUCCESS;
 }