From fa74679d0a2e89930e733ebf90fed7063825293d Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Tue, 16 May 2023 17:50:19 +0200
Subject: [PATCH] Updates to new base_bt_client version

---
 cfg/TiagoArmBtClient.cfg             |  2 --
 src/tiago_arm_bt_client_alg_node.cpp | 12 --------
 src/xml/bt_definitions.xml           | 40 ++++++++++++++++++++-------
 src/xml/bt_test.xml                  | 41 ++++++++++++++++++++--------
 4 files changed, 60 insertions(+), 35 deletions(-)

diff --git a/cfg/TiagoArmBtClient.cfg b/cfg/TiagoArmBtClient.cfg
index 98c3e87..67c5e8a 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 4621447..175bb52 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 da650f9..3eeae67 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 335e591..d2c12c5 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"/>
-- 
GitLab