diff --git a/include/tiago_nav_bt_client_alg_node.h b/include/tiago_nav_bt_client_alg_node.h
index 3def9ba987501f9d4c025e3fc2b225800154c096..60f4fd228f1a59fe6779da4ea8cddcfdc7717c09 100644
--- a/include/tiago_nav_bt_client_alg_node.h
+++ b/include/tiago_nav_bt_client_alg_node.h
@@ -63,6 +63,30 @@ class TiagoNavModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<
// [action client attributes]
bool set_go_to_pose;
+
+ /**
+ * \brief Async function to set the tiago_nav_move_to_posestamped input ports.
+ *
+ * It takes the values from Dynamic Reconfigure.
+ *
+ * It has the following output ports:
+ *
+ * pose (geometry_msg::PoseStamepd): desired pose with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * heading_tol (double): heading tolerance for the goal.
+ *
+ * x_y_pos_tol (double): position tolerance for the goal.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus set_tiago_nav_go_to_posestamped_inputs(BT::TreeNode& self);
+
/**
* \brief Async function to set the tiago_nav_move_to_joints input ports.
*
diff --git a/src/tiago_nav_bt_client_alg_node.cpp b/src/tiago_nav_bt_client_alg_node.cpp
index 309cc4c2cbf2d0975f8a5025dd360640263f2b1b..410b122496a36dba21877ffdfa0c5dd06e66404e 100644
--- a/src/tiago_nav_bt_client_alg_node.cpp
+++ b/src/tiago_nav_bt_client_alg_node.cpp
@@ -1,4 +1,6 @@
#include "tiago_nav_bt_client_alg_node.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/utils.h>
TiagoNavModuleBtClientAlgNode::TiagoNavModuleBtClientAlgNode(void) :
behaviortree_base::IriBaseBTClient<tiago_nav_module::TiagoNavBtClientConfig>(),
@@ -21,8 +23,10 @@ TiagoNavModuleBtClientAlgNode::TiagoNavModuleBtClientAlgNode(void) :
// Init local functions
BT::PortsList async_go_to_pose_inputs_ports = {BT::OutputPort<double>("x"), BT::OutputPort<double>("y"), BT::OutputPort<double>("yaw"), BT::OutputPort<double>("heading_tol"), BT::OutputPort<double>("x_y_pos_tol"), BT::OutputPort<bool>("new_goal")};
+ BT::PortsList sync_go_to_posestamped_inputs_ports = {BT::OutputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("heading_tol"), BT::OutputPort<double>("x_y_pos_tol")};
this->factory.registerIriAsyncAction("set_tiago_nav_go_to_pose_inputs", std::bind(&TiagoNavModuleBtClientAlgNode::set_tiago_nav_go_to_pose_inputs, this, std::placeholders::_1), async_go_to_pose_inputs_ports);
+ this->factory.registerIriAsyncAction("set_tiago_nav_go_to_posestamped_inputs", std::bind(&TiagoNavModuleBtClientAlgNode::set_tiago_nav_go_to_posestamped_inputs, this, std::placeholders::_1), sync_go_to_posestamped_inputs_ports);
}
@@ -54,6 +58,28 @@ void TiagoNavModuleBtClientAlgNode::node_config_update(tiago_nav_module::TiagoNa
this->unlock();
}
+BT::NodeStatus TiagoNavModuleBtClientAlgNode::set_tiago_nav_go_to_posestamped_inputs(BT::TreeNode& self)
+{
+ if (this->set_go_to_pose)
+ {
+ this->set_go_to_pose = false;
+ geometry_msgs::PoseStamped pose;
+ tf2::Quaternion q;
+ pose.pose.position.x = this->config_.mb_x_pos;
+ pose.pose.position.y = this->config_.mb_y_pos;
+ pose.pose.position.z = 0.0;
+ pose.pose.orientation = tf2::toMsg(q);
+ pose.header.frame_id = this->config_.mb_frame_id;
+ pose.header.stamp = ros::Time::now();
+ q.setRPY(0.0, 0.0, this->config_.mb_yaw);
+ self.setOutput("pose", pose);
+ self.setOutput("heading_tol", this->config_.mb_xy_tol);
+ self.setOutput("x_y_pos_tol", this->config_.mb_yaw_tol);
+ return BT::NodeStatus::SUCCESS;
+ }
+ return BT::NodeStatus::RUNNING;
+}
+
BT::NodeStatus TiagoNavModuleBtClientAlgNode::set_tiago_nav_go_to_pose_inputs(BT::TreeNode& self)
{
if (this->set_go_to_pose)
diff --git a/src/tiago_nav_bt_module.cpp b/src/tiago_nav_bt_module.cpp
index a1a6dfa53bd4393c60b82ad6aaba17144dc0cb28..97c3816d10d5b7daad8a17b2c939d4f80ed4faf2 100644
--- a/src/tiago_nav_bt_module.cpp
+++ b/src/tiago_nav_bt_module.cpp
@@ -231,13 +231,17 @@ BT::NodeStatus CTiagoNavModuleBT::sync_tiago_nav_go_to_posestamped(BT::TreeNode&
if (!heading_tol)
{
if (!this->tiago_nav_module.go_to_pose(pose_goal))
+ {
return BT::NodeStatus::FAILURE;
+ }
}
else
{
heading_tol_goal = heading_tol.value();
if (!this->tiago_nav_module.go_to_pose(pose_goal, heading_tol_goal))
+ {
return BT::NodeStatus::FAILURE;
+ }
}
}
else
@@ -245,7 +249,9 @@ BT::NodeStatus CTiagoNavModuleBT::sync_tiago_nav_go_to_posestamped(BT::TreeNode&
heading_tol_goal = heading_tol.value();
x_y_pos_tol_goal = x_y_pos_tol.value();
if (!this->tiago_nav_module.go_to_pose(pose_goal, heading_tol_goal, x_y_pos_tol_goal))
+ {
return BT::NodeStatus::FAILURE;
+ }
}
return BT::NodeStatus::SUCCESS;
}
diff --git a/src/tiago_nav_module.cpp b/src/tiago_nav_module.cpp
index f6e962e092f6b47a99272423218f82a1eb6d18ef..aec421c64e9105a568dc34748efb0379cc175528 100644
--- a/src/tiago_nav_module.cpp
+++ b/src/tiago_nav_module.cpp
@@ -533,7 +533,6 @@ bool CTiagoNavModule::go_to_pose(double x,double y,double yaw,double heading_tol
bool CTiagoNavModule::go_to_pose(geometry_msgs::PoseStamped& pose,double heading_tol,double x_y_pos_tol)
{
double xy_tol,yaw_tol;
-
this->lock();
if(heading_tol!=DEFAULT_HEADING_TOL || x_y_pos_tol!=DEFAULT_X_Y_POS_TOL)
{
@@ -556,7 +555,9 @@ bool CTiagoNavModule::go_to_pose(geometry_msgs::PoseStamped& pose,double heading
}
}
/* store the new information goal */
+ this->unlock();
set_goal_frame_id(pose.header.frame_id);
+ this->lock();
this->goal_x=pose.pose.position.x;
this->goal_y=pose.pose.position.y;
this->goal_yaw=tf2::getYaw(pose.pose.orientation);
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
index 95fb78589459a88ff898a5c26c4db9c10fd60185..4642ab3ff5200002d5a4d91ca278e313dac62b62 100644
--- a/src/xml/bt_test.xml
+++ b/src/xml/bt_test.xml
@@ -3,12 +3,20 @@
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<SequenceStar>
+ <SubTree ID="move_to_posestamped"/>
<SubTree ID="move_to_pose"/>
<SubTree ID="move_to_position"/>
<SubTree ID="move_to_orientation"/>
</SequenceStar>
</BehaviorTree>
<!-- ////////// -->
+ <BehaviorTree ID="move_to_posestamped">
+ <SequenceStar>
+ <Action ID="set_tiago_nav_go_to_posestamped_inputs" pose="{pose}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}"/>
+ <Action ID="sync_tiago_nav_go_to_posestamped" pose="{pose}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}"/>
+ </SequenceStar>
+ </BehaviorTree>
+ <!-- ////////// -->
<BehaviorTree ID="move_to_pose">
<SequenceStar>
<Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>