Skip to content
Snippets Groups Projects
Commit 7d285881 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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

parent ec073a8b
No related branches found
No related tags found
1 merge request!2Added an action to compute the distance only on the y direction.
......@@ -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.
......
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment