Finished BT module

include/tiago_head_module/tiago_head_bt_module.h

@@ -121,6 +121,217 @@ class CTiagoHeadModuleBT
       */
     BT::NodeStatus async_tiago_head_follow_target_pointstamped(BT::TreeNode& self);
 
+    /**
+      * \brief Synchronized point_to TIAGo head function.
+      *
+      * This function calls point_to of tiago_head_module. As this is a
+      * synchronous action is_tiago_head_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   x (double): The target Point x coordenate.
+      *
+      *   y (double): The target Point y coordenate.
+      *
+      *   z (double): The target Point z coordenate.
+      *
+      *   frame_id (std::string): The target Point reference frame id.
+      *
+      *   time (ros:Time): Time at which the desired coordinate was defined
+      *
+      *   duration (double): [Optional] Time from start to move the head.
+      *
+      * \param self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus sync_tiago_head_point_to(BT::TreeNode& self);
+
+    /**
+      * \brief Async point_to TIAGo head function.
+      *
+      * This function calls point_to of tiago_head_module. 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:
+      *
+      *   x (double): The target Point x coordenate.
+      *
+      *   y (double): The target Point y coordenate.
+      *
+      *   z (double): The target Point z coordenate.
+      *
+      *   frame_id (std::string): The target Point reference frame id.
+      *
+      *   time (ros:Time): Time at which the desired coordinate was defined
+      *
+      *   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_point_to(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:
+      *
+      *   x (double): The target Point x coordenate.
+      *
+      *   y (double): The target Point y coordenate.
+      *
+      *   z (double): The target Point z coordenate.
+      *
+      *   frame_id (std::string): The target Point reference frame id.
+      *
+      *   time (ros:Time): Time at which the desired coordinate was defined
+      *
+      *   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(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized move_to TIAGo head function.
+      *
+      * This function calls move_to of tiago_head_module. As this is a
+      * synchronous action is_tiago_head_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   pan (double): Desired angle for the pan joint in radians.
+      *
+      *   tilt (double): Desired angle for the tilt joint in radians.
+      *
+      *   duration (double): [Optional] Time from start to move the head.
+      *
+      * \param self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus sync_tiago_head_move_to(BT::TreeNode& self);
+
+    /**
+      * \brief Async move_to TIAGo head function.
+      *
+      * This function calls move_to of tiago_head_module. 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:
+      *
+      *   pan (double): Desired angle for the pan joint in radians.
+      *
+      *   tilt (double): Desired angle for the tilt joint in radians.
+      *
+      *   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_move_to(BT::TreeNode& self);
+
+/**
+      * \brief Synchronized move_to TIAGo head function.
+      *
+      * This function calls move_to of tiago_head_module. As this is a
+      * synchronous action is_tiago_head_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   pan (std::vector<double>): Desired angles for the pan joint in radians.
+      *
+      *   tilt (std::vector<double>): Desired angles for the tilt joint in radians.
+      *
+      *   duration (std::vector<double>): Times from start to move the head.
+      *
+      * \param self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus sync_tiago_head_move_to_trajectory(BT::TreeNode& self);
+
+    /**
+      * \brief Async move_to TIAGo head function.
+      *
+      * This function calls move_to of tiago_head_module. 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:
+      *
+      *   pan (std::vector<double>): Desired angles for the pan joint in radians.
+      *
+      *   tilt (std::vector<double>): Desired angles for the tilt joint in radians.
+      *
+      *   duration (std::vector<double>): Times 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_move_to_trajectory(BT::TreeNode& self);
+
     /**
       * \brief Synchronized set_workspace TIAGo head function.
       *

src/tiago_head_bt_module.cpp

@@ -12,6 +12,13 @@ void CTiagoHeadModuleBT::init(IriBehaviorTreeFactory &factory)
   //Definition of ports
   BT::PortsList sync_head_point_to_pointstamped_ports = {BT::InputPort<geometry_msgs::PointStamped>("point"), BT::InputPort<double>("duration")};
   BT::PortsList async_head_point_to_pointstamped_ports = {BT::InputPort<geometry_msgs::PointStamped>("point"), BT::InputPort<double>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+  BT::PortsList sync_head_point_to_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("z"), BT::InputPort<std::string>("frame_id"), BT::InputPort<ros::Time>("time"), BT::InputPort<double>("duration")};
+  BT::PortsList async_head_point_to_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("z"), BT::InputPort<std::string>("frame_id"), BT::InputPort<ros::Time>("time"), BT::InputPort<double>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+  BT::PortsList sync_head_move_to_ports = {BT::InputPort<double>("pan"), BT::InputPort<double>("tilt"), BT::InputPort<double>("duration")};
+  BT::PortsList async_head_move_to_ports = {BT::InputPort<double>("pan"), BT::InputPort<double>("tilt"), BT::InputPort<double>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+  BT::PortsList sync_head_move_to_trajectory_ports = {BT::InputPort<std::vector<double> >("pan"), BT::InputPort<std::vector<double> >("tilt"), BT::InputPort<std::vector<double> >("duration")};
+  BT::PortsList async_head_move_to_trajectory_ports = {BT::InputPort<std::vector<double> >("pan"), BT::InputPort<std::vector<double> >("tilt"), BT::InputPort<std::vector<double> >("duration"), BT::BidirectionalPort<bool>("new_goal")};
+
 
   BT::PortsList set_max_velocity_ports = {BT::InputPort<double>("vel")};
 
@@ -36,6 +43,13 @@ 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("sync_tiago_head_point_to",  std::bind(&CTiagoHeadModuleBT::sync_tiago_head_point_to, this, std::placeholders::_1), sync_head_point_to_ports);
+  factory.registerIriAsyncAction("async_tiago_head_point_to",  std::bind(&CTiagoHeadModuleBT::async_tiago_head_point_to, this, std::placeholders::_1), async_head_point_to_ports);
+  factory.registerIriAsyncAction("async_tiago_head_follow_target",  std::bind(&CTiagoHeadModuleBT::async_tiago_head_follow_target, this, std::placeholders::_1), async_head_point_to_ports);
+  factory.registerSimpleAction("sync_tiago_head_move_to",  std::bind(&CTiagoHeadModuleBT::sync_tiago_head_move_to, this, std::placeholders::_1), sync_head_move_to_ports);
+  factory.registerIriAsyncAction("async_tiago_head_move_to",  std::bind(&CTiagoHeadModuleBT::async_tiago_head_move_to, this, std::placeholders::_1), async_head_move_to_ports);
+  factory.registerSimpleAction("sync_tiago_head_move_to_trajectory",  std::bind(&CTiagoHeadModuleBT::sync_tiago_head_move_to_trajectory, this, std::placeholders::_1), sync_head_move_to_trajectory_ports);
+  factory.registerIriAsyncAction("async_tiago_head_move_to_trajectory",  std::bind(&CTiagoHeadModuleBT::async_tiago_head_move_to_trajectory, this, std::placeholders::_1), async_head_move_to_trajectory_ports);
   
   factory.registerSimpleAction("set_tiago_head_max_velocity",  std::bind(&CTiagoHeadModuleBT::set_tiago_head_max_velocity, this, std::placeholders::_1), set_max_velocity_ports);
 }
@@ -150,6 +164,275 @@ BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_follow_target_pointstamped(B
   return BT::NodeStatus::RUNNING;
 }
 
+BT::NodeStatus CTiagoHeadModuleBT::sync_tiago_head_point_to(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::sync_tiago_head_point_to-> sync_tiago_head_point_to");
+  BT::Optional<double> x = self.getInput<double>("x");
+  BT::Optional<double> y = self.getInput<double>("y");
+  BT::Optional<double> z = self.getInput<double>("z");
+  BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
+  BT::Optional<ros::Time> time = self.getInput<ros::Time>("time");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+
+  double x_goal, y_goal, z_goal, duration_goal;
+  std::string frame_id_goal;
+  ros::Time time_goal;
+
+  if (!x || !y || !z || !frame_id || !time)
+  {
+    ROS_ERROR("CTiagoHeadModuleBT::sync_tiago_head_point_to-> Incorrect or missing input. It needs the following input ports: x(double), y(double), z(double), frame_id(std::string), time(ros::Time), and [Optional] duration(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  x_goal = x.value();
+  y_goal = y.value();
+  z_goal = z.value();
+  frame_id_goal = frame_id.value();
+  time_goal = time.value();
+  if (!duration)
+    this->tiago_head_module.point_to(x_goal, y_goal, z_goal, frame_id_goal, time_goal);
+  else
+  {
+    duration_goal = duration.value();
+    this->tiago_head_module.point_to(x_goal, y_goal, z_goal, frame_id_goal, time_goal, duration_goal);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_point_to(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::async_tiago_head_point_to-> async_tiago_head_point_to");
+  BT::Optional<double> x = self.getInput<double>("x");
+  BT::Optional<double> y = self.getInput<double>("y");
+  BT::Optional<double> z = self.getInput<double>("z");
+  BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
+  BT::Optional<ros::Time> time = self.getInput<ros::Time>("time");
+  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)
+  {
+    double x_goal, y_goal, z_goal, duration_goal;
+    std::string frame_id_goal;
+    ros::Time time_goal;
+    if (!x || !y || !z || !frame_id || !time)
+    {
+      ROS_ERROR("CTiagoHeadModuleBT::async_tiago_head_point_to-> Incorrect or missing input. It needs the following input ports: x(double), y(double), z(double), frame_id(std::string), time(ros::Time), and [Optional] duration(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    x_goal = x.value();
+    y_goal = y.value();
+    z_goal = z.value();
+    frame_id_goal = frame_id.value();
+    time_goal = time.value();
+    if (!duration)
+      this->tiago_head_module.point_to(x_goal, y_goal, z_goal, frame_id_goal, time_goal);
+    else
+    {
+      duration_goal = duration.value();
+      this->tiago_head_module.point_to(x_goal, y_goal, z_goal, frame_id_goal, time_goal, duration_goal);
+    }
+  }
+  if (this->tiago_head_module.is_finished())
+  {
+    tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+    if (tiago_head_module_status == TIAGO_HEAD_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  if (new_goal)
+    self.setOutput("new_goal", false);
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_follow_target(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::async_tiago_head_follow_target-> async_tiago_head_follow_target");
+  BT::Optional<double> x = self.getInput<double>("x");
+  BT::Optional<double> y = self.getInput<double>("y");
+  BT::Optional<double> z = self.getInput<double>("z");
+  BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
+  BT::Optional<ros::Time> time = self.getInput<ros::Time>("time");
+  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)
+  {
+    double x_goal, y_goal, z_goal, duration_goal;
+    std::string frame_id_goal;
+    ros::Time time_goal;
+    if (!x || !y || !z || !frame_id || !time)
+    {
+      ROS_ERROR("CTiagoHeadModuleBT::async_tiago_head_follow_target-> Incorrect or missing input. It needs the following input ports:  x(double), y(double), z(double), frame_id(std::string), time(ros::Time), and [Optional] duration(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+    x_goal = x.value();
+    y_goal = y.value();
+    z_goal = z.value();
+    frame_id_goal = frame_id.value();
+    time_goal = time.value();
+    if (!duration)
+      this->tiago_head_module.point_to(x_goal, y_goal, z_goal, frame_id_goal, time_goal);
+    else
+    {
+      duration_goal = duration.value();
+      this->tiago_head_module.point_to(x_goal, y_goal, z_goal, frame_id_goal, time_goal, duration_goal);
+    }
+  }
+  if (new_goal)
+    self.setOutput("new_goal", false);
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::sync_tiago_head_move_to(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::sync_tiago_head_move_to-> sync_tiago_head_move_to");
+  BT::Optional<double> pan = self.getInput<double>("pan");
+  BT::Optional<double> tilt = self.getInput<double>("tilt");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+
+  double pan_goal, tilt_goal, duration_goal;
+
+  if (!pan || !tilt)
+  {
+    ROS_ERROR("CTiagoHeadModuleBT::sync_tiago_head_move_to-> Incorrect or missing input. It needs the following input ports: pan(double), tilt(double), and [Optional] duration(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  pan_goal = pan.value();
+  tilt_goal = tilt.value();
+  if (!duration)
+    this->tiago_head_module.move_to(pan_goal, tilt_goal);
+  else
+  {
+    duration_goal = duration.value();
+    this->tiago_head_module.move_to(pan_goal, tilt_goal, duration_goal);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_move_to(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::async_tiago_head_move_to-> async_tiago_head_move_to");
+  BT::Optional<double> pan = self.getInput<double>("pan");
+  BT::Optional<double> tilt = self.getInput<double>("tilt");
+  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)
+  {
+    double pan_goal, tilt_goal, duration_goal;
+    if (!pan || !tilt)
+    {
+      ROS_ERROR("CTiagoHeadModuleBT::async_tiago_head_move_to-> Incorrect or missing input. It needs the following input ports: pan(double), tilt(double), and [Optional] duration(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    pan_goal = pan.value();
+    tilt_goal = tilt.value();
+    if (!duration)
+      this->tiago_head_module.move_to(pan_goal, tilt_goal);
+    else
+    {
+      duration_goal = duration.value();
+      this->tiago_head_module.move_to(pan_goal, tilt_goal, duration_goal);
+    }
+  }
+  if (this->tiago_head_module.is_finished())
+  {
+    tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+    if (tiago_head_module_status == TIAGO_HEAD_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  if (new_goal)
+    self.setOutput("new_goal", false);
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::sync_tiago_head_move_to_trajectory(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::sync_tiago_head_move_to_trajectory-> sync_tiago_head_move_to_trajectory");
+  BT::Optional<std::vector<double> > pan = self.getInput<std::vector<double> >("pan");
+  BT::Optional<std::vector<double> > tilt = self.getInput<std::vector<double> >("tilt");
+  BT::Optional<std::vector<double> > duration = self.getInput<std::vector<double> >("duration");
+
+  std::vector<double> pan_goal, tilt_goal, duration_goal;
+
+  if (!pan || !tilt || !duration)
+  {
+    ROS_ERROR("CTiagoHeadModuleBT::sync_tiago_head_move_to_trajectory-> Incorrect or missing input. It needs the following input ports: pan(std::vector<double>), tilt(std::vector<double>), and duration(std::vector<double>)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  pan_goal = pan.value();
+  tilt_goal = tilt.value();
+  duration_goal = duration.value();
+  this->tiago_head_module.move_to(pan_goal, tilt_goal, duration_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_move_to_trajectory(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::async_tiago_head_move_to_trajectory-> async_tiago_head_move_to_trajectory");
+  BT::Optional<std::vector<double> > pan = self.getInput<std::vector<double> >("pan");
+  BT::Optional<std::vector<double> > tilt = self.getInput<std::vector<double> >("tilt");
+  BT::Optional<std::vector<double> > duration = self.getInput<std::vector<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)
+  {
+    std::vector<double> pan_goal, tilt_goal, duration_goal;
+    if (!pan || !tilt || !duration)
+    {
+      ROS_ERROR("CTiagoHeadModuleBT::async_tiago_head_move_to_trajectory-> Incorrect or missing input. It needs the following input ports: pan(std::vector<double>), tilt(std::vector<double>), and duration(std::vector<double>)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    pan_goal = pan.value();
+    tilt_goal = tilt.value();
+    duration_goal = duration.value();
+    this->tiago_head_module.move_to(pan_goal, tilt_goal, duration_goal);
+  }
+  if (this->tiago_head_module.is_finished())
+  {
+    tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+    if (tiago_head_module_status == TIAGO_HEAD_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  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");