Fixed overrido of module default values

src/tiago_gripper_module_bt.cpp

@@ -78,13 +78,14 @@ BT::NodeStatus CTiagoGripperModuleBT::sync_close_tiago_gripper(BT::TreeNode& sel
   ROS_DEBUG("CTiagoGripperModuleBT::sync_close_tiago_gripper-> sync_close_tiago_gripper");
   BT::Optional<double> duration = self.getInput<double>("duration");
 
-  double duration_goal;
   if (!duration)
-    duration_goal = -1.0;
+    this->tiago_gripper_module.close();
   else
+  {
+    double duration_goal;
     duration_goal = duration.value();
-
-  this->tiago_gripper_module.close(duration_goal);
+    this->tiago_gripper_module.close(duration_goal);
+  }
   return BT::NodeStatus::SUCCESS;
 }
 
@@ -102,13 +103,14 @@ BT::NodeStatus CTiagoGripperModuleBT::async_close_tiago_gripper(BT::TreeNode& se
 
   if (new_goal)
   {
-    double duration_goal;
     if (!duration)
-      duration_goal = -1.0;
+      this->tiago_gripper_module.close();
     else
+    {
+      double duration_goal;
       duration_goal = duration.value();
-
-    this->tiago_gripper_module.close(duration_goal);
+      this->tiago_gripper_module.close(duration_goal);
+    }
   }
   if (this->tiago_gripper_module.is_finished())
   {
@@ -128,13 +130,14 @@ BT::NodeStatus CTiagoGripperModuleBT::sync_open_tiago_gripper(BT::TreeNode& self
   ROS_DEBUG("CTiagoGripperModuleBT::sync_open_tiago_gripper-> sync_open_tiago_gripper");
   BT::Optional<double> duration = self.getInput<double>("duration");
 
-  double duration_goal;
   if (!duration)
-    duration_goal = -1.0;
+    this->tiago_gripper_module.open();
   else
+  {
+    double duration_goal;
     duration_goal = duration.value();
-
-  this->tiago_gripper_module.open(duration_goal);
+    this->tiago_gripper_module.open(duration_goal);
+  }
   return BT::NodeStatus::SUCCESS;
 }
 
@@ -152,13 +155,14 @@ BT::NodeStatus CTiagoGripperModuleBT::async_open_tiago_gripper(BT::TreeNode& sel
 
   if (new_goal)
   {
-    double duration_goal;
     if (!duration)
-      duration_goal = -1.0;
+      this->tiago_gripper_module.open();
     else
+    {
+      double duration_goal;
       duration_goal = duration.value();
-
-    this->tiago_gripper_module.open(duration_goal);
+      this->tiago_gripper_module.open(duration_goal);
+    }
   }
   if (this->tiago_gripper_module.is_finished())
   {
@@ -190,13 +194,14 @@ BT::NodeStatus CTiagoGripperModuleBT::sync_move_fingers_tiago_gripper(BT::TreeNo
   left_finger_goal = left_finger.value();
   right_finger_goal = right_finger.value();
 
-  double duration_goal;
   if (!duration)
-    duration_goal = -1.0;
+    this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal);
   else
+  {
+    double duration_goal;
     duration_goal = duration.value();
-
-  this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+    this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+  }
   return BT::NodeStatus::SUCCESS;
 }
 
@@ -226,13 +231,14 @@ BT::NodeStatus CTiagoGripperModuleBT::async_move_fingers_tiago_gripper(BT::TreeN
     left_finger_goal = left_finger.value();
     right_finger_goal = right_finger.value();
 
-    double duration_goal;
     if (!duration)
-      duration_goal = -1.0;
+      this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal);
     else
+    {
+      double duration_goal;
       duration_goal = duration.value();
-
-    this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+      this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+    }
   }
   if (this->tiago_gripper_module.is_finished())
   {
@@ -326,13 +332,14 @@ BT::NodeStatus CTiagoGripperModuleBT::sync_fingers_distance_tiago_gripper(BT::Tr
   double distance_goal;
   distance_goal = distance.value();
 
-  double duration_goal;
   if (!duration)
-    duration_goal = -1.0;
+    this->tiago_gripper_module.fingers_distance(distance_goal);
   else
+  {
+    double duration_goal;
     duration_goal = duration.value();
-
-  this->tiago_gripper_module.fingers_distance(distance_goal, duration_goal);
+    this->tiago_gripper_module.fingers_distance(distance_goal, duration_goal);
+  }
   return BT::NodeStatus::SUCCESS;
 }
 
@@ -360,13 +367,14 @@ BT::NodeStatus CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper(BT::T
     double distance_goal;
     distance_goal = distance.value();
 
-    double duration_goal;
     if (!duration)
-      duration_goal = -1.0;
+      this->tiago_gripper_module.fingers_distance(distance_goal);
     else
+    {
+      double duration_goal;
       duration_goal = duration.value();
-
-    this->tiago_gripper_module.fingers_distance(distance_goal, duration_goal);
+      this->tiago_gripper_module.fingers_distance(distance_goal, duration_goal);
+    }
   }
   if (this->tiago_gripper_module.is_finished())
   {

src/xml/bt_test.xml

@@ -39,49 +39,49 @@
         <Action ID="async_cancel_tiago_gripper_action"/>
         <Action ID="async_is_tiago_gripper_finished"/>
         <Action ID="sync_close_tiago_gripper">
-            <input_port default="-1.0" name="duration"> Time from start to close the gripper</input_port>
+            <input_port name="duration"> Time from start to close the gripper</input_port>
         </Action>
         <Action ID="async_close_tiago_gripper">
-            <input_port default="-1.0" name="duration"> Time from start to close the gripper</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="duration"> Time from start to close the gripper</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_open_tiago_gripper">
-            <input_port default="-1.0" name="duration"> Time from start to open the gripper</input_port>
+            <input_port name="duration"> Time from start to open the gripper</input_port>
         </Action>
         <Action ID="async_open_tiago_gripper">
-            <input_port default="-1.0" name="duration"> Time from start to open the gripper</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="duration"> Time from start to open the gripper</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_move_fingers_tiago_gripper">
             <input_port name="left_finger"> Left finger position</input_port>
             <input_port name="right_finger"> Right finger position</input_port>
-            <input_port default="-1.0" name="duration"> Time from start to move the fingers</input_port>
+            <input_port name="duration"> Time from start to move the fingers</input_port>
         </Action>
         <Action ID="async_move_fingers_tiago_gripper">
             <input_port name="left_finger"> Left finger position</input_port>
             <input_port name="right_finger"> Right finger position</input_port>
-            <input_port default="-1.0" name="duration"> Time from start to move the fingers</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="duration"> Time from start to move the fingers</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_move_fingers_multi_tiago_gripper">
             <input_port name="left_finger"> Left finger positions</input_port>
             <input_port name="right_finger"> Right finger positions</input_port>
-            <input_port default="-1.0" name="duration"> Time from start to move the fingers</input_port>
+            <input_port name="duration"> Time from start to move the fingers</input_port>
         </Action>
         <Action ID="async_move_fingers_multi_tiago_gripper">
             <input_port name="left_finger"> Left finger positions</input_port>
             <input_port name="right_finger"> Right finger positions</input_port>
-            <input_port default="-1.0" name="duration"> Time from start to move the fingers</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="duration"> Time from start to move the fingers</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_fingers_distance_tiago_gripper">
             <input_port name="distance"> Distance between fingers</input_port>
-            <input_port default="-1.0" name="duration"> Time from start to move the fingers</input_port>
+            <input_port name="duration"> Time from start to move the fingers</input_port>
         </Action>
         <Action ID="async_fingers_distance_tiago_gripper">
             <input_port name="distance"> Distance between fingers</input_port>
-            <input_port default="-1.0" name="duration"> Time from start to move the fingers</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="duration"> Time from start to move the fingers</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_close_grasp_tiago_gripper"/>
         <Action ID="async_close_grasp_tiago_gripper">
@@ -97,6 +97,7 @@
         <Condition ID="is_tiago_gripper_module_preempted"/>
         <Condition ID="is_tiago_gripper_module_rejected"/>
         <Condition ID="is_tiago_gripper_module_failed_grasp"/>
+        <!-- Tree -->
         <SubTree ID="set_fingers_pos_blackboard"/>
     </TreeNodesModel>
     <!-- ////////// -->