diff --git a/include/tiago_nav_module/tiago_nav_bt_module.h b/include/tiago_nav_module/tiago_nav_bt_module.h
index e770b5e3e4a3d950c1f6717412f0c9c55ceadf3e..6aa21fdadc3c212fc831d23859f1188a5ed395e9 100644
--- a/include/tiago_nav_module/tiago_nav_bt_module.h
+++ b/include/tiago_nav_module/tiago_nav_bt_module.h
@@ -117,11 +117,254 @@ class CTiagoNavModuleBT
*/
BT::NodeStatus async_tiago_nav_go_to_pose(BT::TreeNode& self);
+ /**
+ * \brief Synchronized go_to_orientation TIAGo nav function.
+ *
+ * This function calls go_to_orientation of tiago_nav_module. As this is a
+ * synchronous action is_tiago_nav_finished() must be used to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * yaw (double): desired orientation with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * heading_tol (double): [Optional] heading tolerance for the goal.
+ *
+ * \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_nav_go_to_orientation(BT::TreeNode& self);
+
+ /**
+ * \brief Async go_to_orientation TIAGo nav function.
+ *
+ * This function calls go_to_orientation of tiago_nav_module. As this is
+ * an asynchronous action is_tiago_nav_finished() is checked to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * yaw (double): desired orientation with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * heading_tol (double): [Optional] heading tolerance for the goal.
+ *
+ * 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_nav_go_to_orientation(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized go_to_position TIAGo nav function.
+ *
+ * This function calls go_to_position of tiago_nav_module. As this is a
+ * synchronous action is_tiago_nav_finished() must be used to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * x (double): desired X positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * y (double): desired Y positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * x_y_pos_tol (double): [Optional] position tolerance for the goal.
+ *
+ * \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_nav_go_to_position(BT::TreeNode& self);
+
+ /**
+ * \brief Async go_to_position TIAGo nav function.
+ *
+ * This function calls go_to_position of tiago_nav_module. As this is
+ * an asynchronous action is_tiago_nav_finished() is checked to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * x (double): desired X positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * y (double): desired Y positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * x_y_pos_tol (double): [Optional] position tolerance for the goal.
+ *
+ * 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_nav_go_to_position(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized go_to_poi TIAGo nav function.
+ *
+ * This function calls go_to_poi of tiago_nav_module. As this is a
+ * synchronous action is_tiago_nav_finished() must be used to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * poi (std::string): name of the desired point of interest to move to.
+ *
+ * \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_nav_go_to_poi(BT::TreeNode& self);
+
+ /**
+ * \brief Async go_to_poi TIAGo nav function.
+ *
+ * This function calls go_to_poi of tiago_nav_module. As this is
+ * an asynchronous action is_tiago_nav_finished() is checked to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * poi (std::string): name of the desired point of interest to move to.
+ *
+ * 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_nav_go_to_poi(BT::TreeNode& self);
+
+#ifdef HAVE_WAYPOINTS
+ /**
+ * \brief Synchronized go_to_waypoints TIAGo nav function.
+ *
+ * This function calls go_to_waypoints of tiago_nav_module. As this is a
+ * synchronous action is_tiago_nav_finished() must be used to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * group (std::string): Name of the desired group of points of interest.
+ *
+ * first_index (unsigned int): The index of the first waypoint to execute inside
+ * the list of interest points inside the group.
+ *
+ * num (unsigned int): The number of waypoints to execute. If 0 is provided, all
+ * of them will be executed.
+ *
+ * continue_on_error (bool): [Optional] Whether to contiune on the next waypoint when
+ * the current one can not be reached (true) or
+ * not (false). By default it is set to not continue.
+ *
+ * \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_nav_go_to_waypoints(BT::TreeNode& self);
+
+ /**
+ * \brief Async go_to_waypoints TIAGo nav function.
+ *
+ * This function calls go_to_waypoints of tiago_nav_module. As this is
+ * an asynchronous action is_tiago_nav_finished() is checked to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * group (std::string): Name of the desired group of points of interest.
+ *
+ * first_index (unsigned int): The index of the first waypoint to execute inside
+ * the list of interest points inside the group.
+ *
+ * num (unsigned int): The number of waypoints to execute. If 0 is provided, all
+ * of them will be executed.
+ *
+ * continue_on_error (bool): [Optional] Whether to contiune on the next waypoint when
+ * the current one can not be reached (true) or
+ * not (false). By default it is set to not continue.
+ *
+ * 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_nav_go_to_waypoints(BT::TreeNode& self);
+
/**
* \brief Synchronized set_workspace TIAGo nav function.
*
* This function calls set_workspace of tiago_nav_module.
*
+ * It has the following output ports:
+ *
+ * waypoint (unsigned int): the index of the current waypoint being executed.
+ *
+ * \param self 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 get_tiago_nav_current_waypoint(BT::TreeNode& self);
+
+#endif
+
+ /**
+ * \brief Synchronized set_tiago_nav_goal_frame TIAGo nav function.
+ *
+ * This function calls set_tiago_nav_goal_frame of tiago_nav_module.
+ *
* It has the following input ports:
*
* frame_id (std::string): name of the new reference frame for the future goals.
diff --git a/src/tiago_nav_bt_module.cpp b/src/tiago_nav_bt_module.cpp
index c9ad640dc8f6d06f3e161989c7d86ed5eda636c5..06e1e18f7ab75787748692aaaee9ca37d2d2c688 100644
--- a/src/tiago_nav_bt_module.cpp
+++ b/src/tiago_nav_bt_module.cpp
@@ -18,6 +18,12 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
//Definition of ports
BT::PortsList sync_go_to_pose_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol")};
BT::PortsList async_go_to_pose_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("new_goal")};
+ BT::PortsList sync_go_to_orientation_ports = {BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol")};
+ BT::PortsList async_go_to_orientation_ports = {BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::BidirectionalPort<bool>("new_goal")};
+ BT::PortsList sync_go_to_position_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol")};
+ BT::PortsList async_go_to_position_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("new_goal")};
+ BT::PortsList sync_go_to_poi_ports = {BT::InputPort<std::string>("poi")};
+ BT::PortsList async_go_to_poi_ports = {BT::InputPort<std::string>("poi"), BT::BidirectionalPort<bool>("new_goal")};
BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")};
@@ -41,8 +47,22 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("sync_tiago_nav_go_to_pose", std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_pose, this, std::placeholders::_1), sync_go_to_pose_ports);
factory.registerIriAsyncAction("async_tiago_nav_go_to_pose", std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_pose, this, std::placeholders::_1), async_go_to_pose_ports);
-
+ factory.registerSimpleAction("sync_tiago_nav_go_to_orientation", std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_orientation, this, std::placeholders::_1), sync_go_to_orientation_ports);
+ factory.registerIriAsyncAction("async_tiago_nav_go_to_orientation", std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_orientation, this, std::placeholders::_1), async_go_to_orientation_ports);
+ factory.registerSimpleAction("sync_tiago_nav_go_to_position", std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_position, this, std::placeholders::_1), sync_go_to_position_ports);
+ factory.registerIriAsyncAction("async_tiago_nav_go_to_position", std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_position, this, std::placeholders::_1), async_go_to_position_ports);
+ factory.registerSimpleAction("sync_tiago_nav_go_to_poi", std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_poi, this, std::placeholders::_1), sync_go_to_poi_ports);
+ factory.registerIriAsyncAction("async_tiago_nav_go_to_poi", std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_poi, this, std::placeholders::_1), async_go_to_poi_ports);
+
factory.registerSimpleAction("set_tiago_nav_goal_frame", std::bind(&CTiagoNavModuleBT::set_tiago_nav_goal_frame, this, std::placeholders::_1), set_goal_frame_ports);
+ #ifdef HAVE_WAYPOINTS
+ BT::PortsList sync_go_to_waypoints_ports = {BT::InputPort<std::string>("group"), BT::InputPort<unsigned int>("first_index"), BT::InputPort<unsigned int>("num"), BT::InputPort<bool>("continue_on_error")};
+ BT::PortsList async_go_to_waypoints_ports = {BT::InputPort<std::string>("group"), BT::InputPort<unsigned int>("first_index"), BT::InputPort<unsigned int>("num"), BT::InputPort<bool>("continue_on_error"), BT::BidirectionalPort<bool>("new_goal")};
+ BT::PortsList get_current_waypoint_ports = {BT::InputPort<unsigned int>("waypoint")};
+ factory.registerSimpleAction("sync_tiago_nav_go_to_waypoints", std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_waypoints, this, std::placeholders::_1), sync_go_to_waypoints_ports);
+ factory.registerIriAsyncAction("async_tiago_nav_go_to_waypoints", std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints, this, std::placeholders::_1), async_go_to_waypoints_ports);
+ factory.registerSimpleAction("get_tiago_nav_current_waypoint", std::bind(&CTiagoNavModuleBT::get_tiago_nav_current_waypoint, this, std::placeholders::_1), get_current_waypoint_ports);
+ #endif
}
CTiagoNavModuleBT::~CTiagoNavModuleBT(void)
@@ -158,6 +178,310 @@ BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_pose(BT::TreeNode& self)
return BT::NodeStatus::RUNNING;
}
+BT::NodeStatus CTiagoNavModuleBT::sync_tiago_nav_go_to_orientation(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::sync_tiago_nav_go_to_orientation-> sync_tiago_nav_go_to_orientation");
+ BT::Optional<double> yaw = self.getInput<double>("yaw");
+ BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
+
+ double yaw_goal, heading_tol_goal;
+ if (!yaw)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_orientation-> Incorrect or missing input. It needs the following input ports: yaw(double) and [Optional] heading_tol(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ yaw_goal = yaw.value();
+ if (!heading_tol)
+ {
+ if (!this->tiago_nav_module.go_to_orientation (yaw_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ else
+ {
+ heading_tol_goal = heading_tol.value();
+ if (!this->tiago_nav_module.go_to_orientation (yaw_goal, heading_tol_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_orientation(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::async_tiago_nav_go_to_orientation-> async_tiago_nav_go_to_orientation");
+ BT::Optional<double> yaw = self.getInput<double>("yaw");
+ BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
+ 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 yaw_goal, heading_tol_goal;
+ if (!yaw)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::async_tiago_nav_go_to_orientation-> Incorrect or missing input. It needs the following input ports: yaw(double) and [Optional] heading_tol(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ yaw_goal = yaw.value();
+ if (!heading_tol)
+ {
+ if (!this->tiago_nav_module.go_to_orientation(yaw_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ else
+ {
+ heading_tol_goal = heading_tol.value();
+ if (!this->tiago_nav_module.go_to_orientation(yaw_goal, heading_tol_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+ if (this->tiago_nav_module.is_finished())
+ {
+ tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+ if (tiago_nav_module_status == TIAGO_NAV_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 CTiagoNavModuleBT::sync_tiago_nav_go_to_position(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::sync_tiago_nav_go_to_position-> sync_tiago_nav_go_to_position");
+ BT::Optional<double> x = self.getInput<double>("x");
+ BT::Optional<double> y = self.getInput<double>("y");
+ BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
+
+ double x_goal, y_goal, x_y_pos_tol_goal;
+ if (!x || !y)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_position-> Incorrect or missing input. It needs the following input ports: x(double), y(double) and [Optional] x_y_pos_tol(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ x_goal = x.value();
+ y_goal = y.value();
+ if (!x_y_pos_tol)
+ {
+ if (!this->tiago_nav_module.go_to_position(x_goal, y_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ else
+ {
+ x_y_pos_tol_goal = x_y_pos_tol.value();
+ if (!this->tiago_nav_module.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_position(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::async_tiago_nav_go_to_position-> async_tiago_nav_go_to_position");
+ BT::Optional<double> x = self.getInput<double>("x");
+ BT::Optional<double> y = self.getInput<double>("y");
+ BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
+ 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, x_y_pos_tol_goal;
+ if (!x || !y)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::async_tiago_nav_go_to_position-> Incorrect or missing input. It needs the following input ports: x(double), y(double) and [Optional] x_y_pos_tol(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ x_goal = x.value();
+ y_goal = y.value();
+ if (!x_y_pos_tol)
+ {
+ if (!this->tiago_nav_module.go_to_position(x_goal, y_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ else
+ {
+ x_y_pos_tol_goal = x_y_pos_tol.value();
+ if (!this->tiago_nav_module.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+ if (this->tiago_nav_module.is_finished())
+ {
+ tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+ if (tiago_nav_module_status == TIAGO_NAV_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 CTiagoNavModuleBT::sync_tiago_nav_go_to_poi(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::sync_tiago_nav_go_to_poi-> sync_tiago_nav_go_to_poi");
+ BT::Optional<std::string> poi = self.getInput<std::string>("poi");
+
+ std::string poi_goal;
+ if (!poi)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_poi-> Incorrect or missing input. It needs the following input ports: poi(std::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ poi_goal = poi.value();
+
+ this->tiago_nav_module.go_to_poi(poi_goal);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_poi(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::async_tiago_nav_go_to_poi-> async_tiago_nav_go_to_poi");
+ BT::Optional<std::string> poi = self.getInput<std::string>("poi");
+ 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::string poi_goal;
+ if (!poi)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::async_tiago_nav_go_to_poi-> Incorrect or missing input. It needs the following input ports: poi(std::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ poi_goal = poi.value();
+
+ this->tiago_nav_module.go_to_poi(poi_goal);
+ }
+ if (this->tiago_nav_module.is_finished())
+ {
+ tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+ if (tiago_nav_module_status == TIAGO_NAV_MODULE_SUCCESS)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
+ }
+ if (new_goal)
+ self.setOutput("new_goal", false);
+ return BT::NodeStatus::RUNNING;
+}
+
+
+#ifdef HAVE_WAYPOINTS
+BT::NodeStatus CTiagoNavModuleBT::sync_tiago_nav_go_to_waypoints(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::sync_tiago_nav_go_to_waypoints-> sync_tiago_nav_go_to_waypoints");
+ BT::Optional<std::string> group = self.getInput<std::string>("group");
+ BT::Optional<unsigned int> first_index = self.getInput<unsigned int>("first_index");
+ BT::Optional<unsigned int> num = self.getInput<unsigned int>("num");
+ BT::Optional<bool> continue_on_error = self.getInput<bool>("continue_on_error");
+
+ std::string group_goal;
+ unsigned int first_index_goal, num_goal;
+ bool continue_on_error_goal;
+ if (!group || !first_index || !num)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_waypoints-> Incorrect or missing input. It needs the following input ports: group(std::string), first_index(unsigned int), num(unsigned int) and [Optional] continue_on_error(bool)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ group_goal = group.value();
+ first_index_goal = first_index.value();
+ num_goal = num.value();
+ if (!continue_on_error)
+ this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+ else
+ {
+ continue_on_error_goal = continue_on_error.value();
+ this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+ }
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints-> async_tiago_nav_go_to_waypoints");
+ BT::Optional<std::string> group = self.getInput<std::string>("group");
+ BT::Optional<unsigned int> first_index = self.getInput<unsigned int>("first_index");
+ BT::Optional<unsigned int> num = self.getInput<unsigned int>("num");
+ BT::Optional<bool> continue_on_error = self.getInput<bool>("continue_on_error");
+ 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, x_y_pos_tol_goal;
+ if (!group || !first_index || !num)
+ {
+ ROS_ERROR("CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints-> Incorrect or missing input. It needs the following input ports: group(std::string), first_index(unsigned int), num(unsigned int) and [Optional] continue_on_error(bool)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ group_goal = group.value();
+ first_index_goal = first_index.value();
+ num_goal = num.value();
+ if (!continue_on_error)
+ this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+ else
+ {
+ continue_on_error_goal = continue_on_error.value();
+ this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+ }
+ }
+ if (this->tiago_nav_module.is_finished())
+ {
+ tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+ if (tiago_nav_module_status == TIAGO_NAV_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 CTiagoNavModuleBT::get_tiago_nav_current_waypoint(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoNavModuleBT::get_tiago_nav_current_waypoint-> get_tiago_nav_current_waypoint");
+
+ unsigned int waypoint_out = this->tiago_nav_module.get_current_waypoint();
+
+ self.setOutput("waypoint", waypoint_out);
+ return BT::NodeStatus::SUCCESS;
+}
+
+#endif
+
BT::NodeStatus CTiagoNavModuleBT::set_tiago_nav_goal_frame(BT::TreeNode& self)
{
ROS_DEBUG("CTiagoNavModuleBT::set_tiago_nav_goal_frame-> set_tiago_nav_goal_frame");