diff --git a/include/tiago_head_module/tiago_head_bt_module.h b/include/tiago_head_module/tiago_head_bt_module.h index 4e2932f3d10e6942a4047c84f978f182b9d1be7e..b58d0f7e9e71dcb113fd35f2d5c957ac8822903b 100644 --- a/include/tiago_head_module/tiago_head_bt_module.h +++ b/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. * diff --git a/src/tiago_head_bt_module.cpp b/src/tiago_head_bt_module.cpp index d240d1975e9d7c01e15aa93a9abfac1df52c5234..37077c352c3e85f1d4f2ee2cc836426ebae4fe0c 100644 --- a/src/tiago_head_bt_module.cpp +++ b/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");