diff --git a/cfg/TiagoArmBtClient.cfg b/cfg/TiagoArmBtClient.cfg
index 98c3e8728e1a6f89238f9cfbcecb419dcda70f6b..67c5e8ae58b08b7a7601543df259e3a697ff5bc7 100755
--- a/cfg/TiagoArmBtClient.cfg
+++ b/cfg/TiagoArmBtClient.cfg
@@ -45,8 +45,6 @@ move_pose = gen.add_group("Move arm in cartesian space")
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
add_bt_client_params(gen)
-gen.add("START", bool_t, 0, "synchronous start", False)
-gen.add("RESTART", bool_t, 0, "asynchronous restart of BT", False)
move_joints.add("set_joints", bool_t, 0, "Set selected angles", False)
move_joints.add("torso", double_t, 0, "Target angle for torso", 0.15, 0.0, 0.35)
diff --git a/src/tiago_arm_bt_client_alg_node.cpp b/src/tiago_arm_bt_client_alg_node.cpp
index 46214478d9feeda1a757764f6b14784e58bbc5da..175bb52e552ad0f6a5dd9dabdad3fe9008318d29 100644
--- a/src/tiago_arm_bt_client_alg_node.cpp
+++ b/src/tiago_arm_bt_client_alg_node.cpp
@@ -50,18 +50,6 @@ void TiagoArmModuleBtClientAlgNode::node_config_update(tiago_arm_module::TiagoAr
std::string move_group;
this->lock();
- if(config.START)
- {
- this->core.start_tree();
- config.START=false;
- }
-
- if(config.RESTART)
- {
- this->core.stop_tree();
- config.RESTART=false;
- }
-
if (config.set_joints)
{
ROS_WARN("TiagoArmModuleBtClientAlgNode: To change the move_group modify it on module dynamic Reconfigure");
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
index da650f96ac6a8bb6b8850729612d28b4d19103f9..3eeae67cf963887e86e8a4c791d906c44fe2b0e0 100644
--- a/src/xml/bt_definitions.xml
+++ b/src/xml/bt_definitions.xml
@@ -2,20 +2,40 @@
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
- <Action ID="NOP"/>
+ <Action ID="RUNNING"/>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<!-- basic_nodes -->
- <Action ID="NOP"/>
- <Action ID="is_not_start_tree"/>
- <Condition ID="async_is_start_tree"/>
- <Action ID="is_not_stop_tree"/>
- <Condition ID="async_is_stop_tree"/>
- <Action ID="set_start_tree"/>
- <Action ID="set_stop_tree"/>
- <Action ID="reset_start_tree"/>
- <Action ID="reset_stop_tree"/>
+ <Action ID="RUNNING"/>
+ <Action ID="is_variable_true">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="is_variable_false">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="transform_pose">
+ <input_port name="pose_in"> (geometry_msgs::PoseStamped): Pose to be transformed.</input_port>
+ <input_port name="target_frame"> (std::string): Frame to transform the pose.</input_port>
+ <output_port name="pose_out"> (geometry_msgs::PoseStamped): Transformed pose.</output_port>
+ </Action>
+ <Action ID="compute_distance">
+ <input_port name="pose1"> (geometry_msgs::PoseStamped): Pose 1.</input_port>
+ <input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
+ <output_port name="distance"> (double): Distance.</output_port>
+ </Action>
+ <Action ID="compare_bigger">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="compare_smaller">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="print_msg">
+ <input_port name="msg"> (std::string): Msg to print.</input_port>
+ <input_port name="type"> (std::string): It must be info, warn, error or debug.</input_port>
+ </Action>
<!-- tiago_arm_module -->
<Action ID="sync_cancel_tiago_arm_action"/>
<Action ID="async_cancel_tiago_arm_action"/>
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
index 335e591d6391e5be7f1a7f5fa53cd8fcac1bbcfb..d2c12c5e34f8f2016aaf0164956970c512fef625 100644
--- a/src/xml/bt_test.xml
+++ b/src/xml/bt_test.xml
@@ -3,7 +3,6 @@
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<SequenceStar>
- <Action ID="async_is_start_tree"/>
<Action ID="set_tiago_arm_move_to_joints_input" target_joints="{target_joints}" joint_tol="{joint_tol}" new_goal="{new_goal}"/>
<Action ID="async_tiago_arm_move_to_joints" target_joints="{target_joints}" joint_tol="{joint_tol}" new_goal="{new_goal}"/>
<Action ID="set_tiago_arm_move_to_pose_input" pose="{pose}" position_tol="{position_tol}" orientation_tol="{orientation_tol}" new_goal="{new_goal}"/>
@@ -12,16 +11,36 @@
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
- <!-- basic_nodes -->
- <Action ID="NOP"/>
- <Action ID="is_not_start_tree"/>
- <Condition ID="async_is_start_tree"/>
- <Action ID="is_not_stop_tree"/>
- <Condition ID="async_is_stop_tree"/>
- <Action ID="set_start_tree"/>
- <Action ID="set_stop_tree"/>
- <Action ID="reset_start_tree"/>
- <Action ID="reset_stop_tree"/>
+ <!-- basic_nodes -->
+ <Action ID="RUNNING"/>
+ <Action ID="is_variable_true">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="is_variable_false">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="transform_pose">
+ <input_port name="pose_in"> (geometry_msgs::PoseStamped): Pose to be transformed.</input_port>
+ <input_port name="target_frame"> (std::string): Frame to transform the pose.</input_port>
+ <output_port name="pose_out"> (geometry_msgs::PoseStamped): Transformed pose.</output_port>
+ </Action>
+ <Action ID="compute_distance">
+ <input_port name="pose1"> (geometry_msgs::PoseStamped): Pose 1.</input_port>
+ <input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
+ <output_port name="distance"> (double): Distance.</output_port>
+ </Action>
+ <Action ID="compare_bigger">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="compare_smaller">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="print_msg">
+ <input_port name="msg"> (std::string): Msg to print.</input_port>
+ <input_port name="type"> (std::string): It must be info, warn, error or debug.</input_port>
+ </Action>
<!-- tiago_arm_module -->
<Action ID="sync_cancel_tiago_arm_action"/>
<Action ID="async_cancel_tiago_arm_action"/>