new_goal port of async actions is automatically changed to false only if the action returns running

include/tiago_gripper_module/tiago_gripper_module_bt.h

@@ -399,13 +399,13 @@ class CTiagoGripperModuleBT
     BT::NodeStatus is_tiago_gripper_module_fb_watchdog(void);
 
     /**
-      * \brief Checks if the navigation goal could not be reached.
+      * \brief Checks if the action goal could not be reached.
       *
       * This function must be used to know the type of failure, once the last
       * goal has finished. Specifically, it checks if it failed because the
-      * navigation goal could not be reached.
+      * action goal could not be reached.
       *
-      * \return a BT:NodeStatus indicating whether the navigation goal could not
+      * \return a BT:NodeStatus indicating whether the action goal could not
       * be reached (BT:NodeStatus::SUCCESS) or if it failed due to another
       * reason (BT:NodeStatus::FAILURE).
       *
@@ -413,14 +413,14 @@ class CTiagoGripperModuleBT
     BT::NodeStatus is_tiago_gripper_module_aborted(void);
 
     /**
-      * \brief Checks if the current navigation goal has been cancelled by
+      * \brief Checks if the current action goal has been cancelled by
       * another goal.
       *
       * This function must be used to know the type of failure, once the last
       * goal has finished. Specifically, it checks if it failed because the
-      * current navigation goal has been cancelled by another goal.
+      * current action goal has been cancelled by another goal.
       *
-      * \return a BT:NodeStatus indicating whether the current navigation goal
+      * \return a BT:NodeStatus indicating whether the current action goal
       * has been cancelled by another goal (BT:NodeStatus::SUCCESS) or if it
       * failed due to another reason (BT:NodeStatus::FAILURE).
       *
@@ -442,11 +442,10 @@ class CTiagoGripperModuleBT
     BT::NodeStatus is_tiago_gripper_module_rejected(void);
 
     /**
-      * \brief Checks if the goal information is not valid.
+      * \brief Checks if the grasping action failed.
       *
       * This function must be used to know the type of failure, once the last
-      * goal has finished. Specifically, it checks if it failed because the
-      * goal information is not valid and the goal will not be executed.
+      * goal has finished. Specifically, it checks if the grasping action has failed.
       *
       * \return a BT:NodeStatus indicating whether the goal information is not
       * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason

src/tiago_gripper_module_bt.cpp

@@ -95,7 +95,6 @@ BT::NodeStatus CTiagoGripperModuleBT::async_close_tiago_gripper(BT::TreeNode& se
     else
       duration_goal = duration.value();
 
-    self.setOutput("new_goal", false);
     this->tiago_gripper_module.close(duration_goal);
   }
   if (this->tiago_gripper_module.is_finished())
@@ -106,6 +105,8 @@ BT::NodeStatus CTiagoGripperModuleBT::async_close_tiago_gripper(BT::TreeNode& se
     else
       return BT::NodeStatus::FAILURE;
   }
+  if (new_goal)
+    self.setOutput("new_goal", false);
   return BT::NodeStatus::RUNNING;
 }
 
@@ -144,7 +145,6 @@ BT::NodeStatus CTiagoGripperModuleBT::async_open_tiago_gripper(BT::TreeNode& sel
     else
       duration_goal = duration.value();
 
-    self.setOutput("new_goal", false);
     this->tiago_gripper_module.open(duration_goal);
   }
   if (this->tiago_gripper_module.is_finished())
@@ -155,6 +155,8 @@ BT::NodeStatus CTiagoGripperModuleBT::async_open_tiago_gripper(BT::TreeNode& sel
     else
       return BT::NodeStatus::FAILURE;
   }
+  if (new_goal)
+    self.setOutput("new_goal", false);
   return BT::NodeStatus::RUNNING;
 }
 
@@ -217,7 +219,6 @@ BT::NodeStatus CTiagoGripperModuleBT::async_move_fingers_tiago_gripper(BT::TreeN
     else
       duration_goal = duration.value();
 
-    self.setOutput("new_goal", false);
     this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
   }
   if (this->tiago_gripper_module.is_finished())
@@ -228,6 +229,8 @@ BT::NodeStatus CTiagoGripperModuleBT::async_move_fingers_tiago_gripper(BT::TreeN
     else
       return BT::NodeStatus::FAILURE;
   }
+  if (new_goal)
+    self.setOutput("new_goal", false);
   return BT::NodeStatus::RUNNING;
 }
 
@@ -280,7 +283,6 @@ BT::NodeStatus CTiagoGripperModuleBT::async_move_fingers_multi_tiago_gripper(BT:
     right_finger_goal = right_finger.value();
     duration_goal = duration.value();
 
-    self.setOutput("new_goal", false);
     this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
   }
   if (this->tiago_gripper_module.is_finished())
@@ -291,6 +293,8 @@ BT::NodeStatus CTiagoGripperModuleBT::async_move_fingers_multi_tiago_gripper(BT:
     else
       return BT::NodeStatus::FAILURE;
   }
+  if (new_goal)
+    self.setOutput("new_goal", false);
   return BT::NodeStatus::RUNNING;
 }
 
@@ -349,7 +353,6 @@ BT::NodeStatus CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper(BT::T
     else
       duration_goal = duration.value();
 
-    self.setOutput("new_goal", false);
     this->tiago_gripper_module.fingers_distance(distance_goal, duration_goal);
   }
   if (this->tiago_gripper_module.is_finished())
@@ -360,6 +363,8 @@ BT::NodeStatus CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper(BT::T
     else
       return BT::NodeStatus::FAILURE;
   }
+  if (new_goal)
+    self.setOutput("new_goal", false);
   return BT::NodeStatus::RUNNING;
 }