From 7d28588142015c33fac928cc2e8a4fd37cf1c594 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 28 Nov 2023 16:07:56 +0000
Subject: [PATCH] Added an action to compute the distance only on the y
direction.
---
include/iri_bt_basic_nodes.h | 1 +
src/iri_bt_basic_nodes.cpp | 33 +++++++++++++++++++++++++++++++--
2 files changed, 32 insertions(+), 2 deletions(-)
diff --git a/include/iri_bt_basic_nodes.h b/include/iri_bt_basic_nodes.h
index 5701dc1..820043f 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 0e0859e..5955797 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,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;
--
GitLab