Added async_tiago_head_follow_target_pointstamped

include/tiago_head_module/tiago_head_bt_module.h

@@ -92,6 +92,35 @@ class CTiagoHeadModuleBT
       */
     BT::NodeStatus async_tiago_head_point_to_pointstamped(BT::TreeNode& self);
 
+    /**
+      * \brief Async point_to TIAGo head function.
+      *
+      * This function calls point_to of tiago_head_module. Is the same than 
+      * async_tiago_head_point_to_pointstamped but returning always running. As this is
+      * an asynchronous action is_tiago_head_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   point (geometry_msgs::PointStamped): The target Point.
+      *
+      *   duration (double): [Optional] Time from start to move the head.
+      *
+      * It also has a bidirectional port:
+      *
+      *   new_goal (bool): If it's a new_goal.
+      *
+      * \param self node with the required inputs defined as follows:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
+      * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
+      * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect
+      * it also returns BT:NodeStatus::FAILURE
+      *
+      */
+    BT::NodeStatus async_tiago_head_follow_target_pointstamped(BT::TreeNode& self);
+
     /**
       * \brief Synchronized set_workspace TIAGo head function.
       *

src/tiago_head_bt_module.cpp

@@ -35,6 +35,7 @@ void CTiagoHeadModuleBT::init(IriBehaviorTreeFactory &factory)
 
   factory.registerSimpleAction("sync_tiago_head_point_to_pointstamped",  std::bind(&CTiagoHeadModuleBT::sync_tiago_head_point_to_pointstamped, this, std::placeholders::_1), sync_head_point_to_pointstamped_ports);
   factory.registerIriAsyncAction("async_tiago_head_point_to_pointstamped",  std::bind(&CTiagoHeadModuleBT::async_tiago_head_point_to_pointstamped, this, std::placeholders::_1), async_head_point_to_pointstamped_ports);
+  factory.registerIriAsyncAction("async_tiago_head_follow_target_pointstamped",  std::bind(&CTiagoHeadModuleBT::async_tiago_head_follow_target_pointstamped, this, std::placeholders::_1), async_head_point_to_pointstamped_ports);
   
   factory.registerSimpleAction("set_tiago_head_max_velocity",  std::bind(&CTiagoHeadModuleBT::set_tiago_head_max_velocity, this, std::placeholders::_1), set_max_velocity_ports);
 }
@@ -88,7 +89,7 @@ BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_point_to_pointstamped(BT::Tr
     double duration_goal;
     if (!point)
     {
-      ROS_ERROR("CTiagoHeadModuleBT::sync_tiago_head_move_to_joints-> Incorrect or missing input. It needs the following input ports: point(geometry_msgs::PointStamped) and [Optional] duration(double)");
+      ROS_ERROR("CTiagoHeadModuleBT::async_tiago_head_point_to_pointstamped-> Incorrect or missing input. It needs the following input ports: point(geometry_msgs::PointStamped) and [Optional] duration(double)");
       return BT::NodeStatus::FAILURE;
     }
 
@@ -114,6 +115,41 @@ BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_point_to_pointstamped(BT::Tr
   return BT::NodeStatus::RUNNING;
 }
 
+BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_follow_target_pointstamped(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::async_tiago_head_follow_target_pointstamped-> async_tiago_head_follow_target_pointstamped");
+  BT::Optional<geometry_msgs::PointStamped> point = self.getInput<geometry_msgs::PointStamped>("point");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+  BT::Optional<bool> new_goal_in = self.getInput<bool>("new_goal");
+  bool new_goal;
+  if (!new_goal_in)
+    new_goal = false;
+  else
+    new_goal = new_goal_in.value();
+
+  if (new_goal)
+  {
+    geometry_msgs::PointStamped point_goal;
+    double duration_goal;
+    if (!point)
+    {
+      ROS_ERROR("CTiagoHeadModuleBT::async_tiago_head_follow_target_pointstamped-> Incorrect or missing input. It needs the following input ports: point(geometry_msgs::PointStamped) and [Optional] duration(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    point_goal = point.value();
+    if (!duration)
+      this->tiago_head_module.point_to(point_goal);
+    else
+    {
+      duration_goal = duration.value();
+      this->tiago_head_module.point_to(point_goal, duration_goal);
+    }
+  }
+  if (new_goal)
+    self.setOutput("new_goal", false);
+  return BT::NodeStatus::RUNNING;
+}
+
 BT::NodeStatus CTiagoHeadModuleBT::set_tiago_head_max_velocity(BT::TreeNode& self)
 {
   ROS_DEBUG("CTiagoHeadModuleBT::set_tiago_head_max_velocity-> set_tiago_head_max_velocity");