Added an action to compute the distance only on the y direction.

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.

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,8 +129,36 @@ 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 << "pose1: x: " << pose1_value.pose.position.x << ", y: " << pose1_value.pose.position.y << ", z: " << pose1_value.pose.position.z << std::endl;
-  std::cout << "pose1: x: " << pose2_value.pose.position.x << ", y: " << pose2_value.pose.position.y << ", z: " << pose2_value.pose.position.z << std::endl;
+  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;