diff --git a/CMakeLists.txt b/CMakeLists.txt index 5cfbd15cedba579d9a2ed70ef8b2966fb1725a5d..e87f5798a6554b17709f70f419453bf560aec946 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ add_definitions(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm) +find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm iri_behaviortree) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -99,10 +99,12 @@ generate_dynamic_reconfigure_options( ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need +set(module_name tiago_play_motion_module) +set(module_bt_name tiago_play_motion_module_bt) catkin_package( INCLUDE_DIRS include - LIBRARIES tiago_play_motion_module - CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm + LIBRARIES ${module_name} ${module_bt_name} + CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm iri_behaviortree # DEPENDS system_lib ) @@ -118,7 +120,8 @@ include_directories(${catkin_INCLUDE_DIRS}) include_directories(${iriutils_INCLUDE_DIR}) ## Declare a C++ library -set(module_name tiago_play_motion_module) +# Module + add_library(${module_name} src/tiago_play_motion_module.cpp ) @@ -137,6 +140,7 @@ add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS}) ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/humanoid_modules_node.cpp) +# Module Client set(client_name tiago_play_motion_client) add_executable(${client_name} src/tiago_play_motion_client_alg.cpp src/tiago_play_motion_client_alg_node.cpp) @@ -147,6 +151,13 @@ target_link_libraries(${client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_ add_dependencies(${client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name}) add_dependencies(${client_name} ${tiago_play_motion_client_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +#BT_module +add_library(${module_bt_name} src/tiago_play_motion_bt_module.cpp) +target_link_libraries(${module_bt_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so) +target_link_libraries(${module_bt_name} ${catkin_LIBRARIES}) +target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES}) +add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${module_name}) + ############# ## Install ## ############# diff --git a/include/tiago_play_motion_module/tiago_play_motion_bt_module.h b/include/tiago_play_motion_module/tiago_play_motion_bt_module.h new file mode 100644 index 0000000000000000000000000000000000000000..aad41ac54a83861067671094b943c88f6f989287 --- /dev/null +++ b/include/tiago_play_motion_module/tiago_play_motion_bt_module.h @@ -0,0 +1,280 @@ +#ifndef _IRI_TIAGO_PLAY_MOTION_MODULE_BT_H +#define _IRI_TIAGO_PLAY_MOTION_MODULE_BT_H + +#include <iri_base_algorithm/iri_base_algorithm.h> + +#include "iri_bt_factory.h" +#include <tiago_play_motion_module/tiago_play_motion_module.h> + +class CTiagoPlayMotionModuleBT +{ + private: + // object of tiago_play_motion_module + CTiagoPlayMotionModule tiago_play_motion_module; + + public: + /** + * \brief Constructor + * + */ + CTiagoPlayMotionModuleBT(); + + /** + * \brief Constructor with module name. + * + * \param module_name The module name. + * + */ + CTiagoPlayMotionModuleBT(std::string &module_name); + + /** + * \brief Register all conditions and actions needed for using the module + * + * This function registers all conditions and actions needed for using the module + * to the factory provided including all input and output ports required + * by actions. + * + * \param factory (IriBehaviorTreeFactory) + * + */ + void init(IriBehaviorTreeFactory &factory); + + /** + * \brief Destructor + * + * This destructor frees all necessary dynamic memory allocated within this + * this class. + */ + ~CTiagoPlayMotionModuleBT(); + + protected: + + /** + * \brief Synchronized execute_motion TIAGo play_motion function. + * + * This function calls execute_motion of tiago_play_motion_module. As this is a + * synchronous action is_tiago_play_motion_finished() must be used to know when the action + * has actually finished. + * + * It has the following input ports: + * + * motion_name (std::string): The motion name to be executed. + * + * \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_play_motion_execute_motion(BT::TreeNode& self); + + /** + * \brief Async execute_motion TIAGo play_motion function. + * + * This function calls execute_motion of tiago_play_motion_module. As this is + * an asynchronous action is_tiago_play_motion_finished() is checked to know when the action + * has actually finished. + * + * It has the following input ports: + * + * motion_name (std::string): The motion name to be executed. + * + * 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_play_motion_execute_motion(BT::TreeNode& self); + + /** + * \brief Synchronized stop TIAGo play_motion function. + * + * This function calls stop of tiago_play_motion_module. As this is a + * synchronous action is_tiago_play_motion_finished() must be used to know when the action + * has actually finished. + * + * \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_cancel_tiago_play_motion_action(void); + + /** + * \brief Async stop TIAGo play_motion function. + * + * This function calls stop of tiago_play_motion_module. As this is + * an asynchronous action is_tiago_play_motion_finished() is checked to know when the action + * has actually finished. + * + * \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_cancel_tiago_play_motion_action(void); + + /** + * \brief Checks if the module is idle and there isn't any new movement request. + * + * This function must be used when any sync action is called. + * + * \return a BT:NodeStatus indicating whether the last movement has + * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE), + * regardless of its success or not. + * + */ + BT::NodeStatus is_tiago_play_motion_finished(void); + + /** + * \brief Checks if the module is idle and there isn't any new movement request. + * + * This function must be used when any sync action is called and + * a BT timeout is used. + * + * \return a BT:NodeStatus indicating whether the last movement has + * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::RUNNING), + * regardless of its success or not. + * + */ + BT::NodeStatus async_is_tiago_play_motion_finished(void); + + ///TIAGo play_motion module status + /** + * \brief Checks if the module is active and operating properly. + * + * This function must be used to know whether the module is active and + * operating properly, or not. + * + * \return a BT:NodeStatus indicating whether the module is active and + * operating properly (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE). + * + */ + BT::NodeStatus is_tiago_play_motion_module_running(void); + + /** + * \brief Checks if the last action goal has been reached successfully. + * + * This function must be used to know whether the last action goal has + * been reached successfully or not, once it has finished. + * + * \return a BT:NodeStatus indicating whether the last action goal has + * been reached successfully (BT:NodeStatus::SUCCESS) or not + * (BT:NodeStatus::FAILURE), regardless of how it has failed. + * + */ + BT::NodeStatus is_tiago_play_motion_module_success(void); + + /** + * \brief Checks if the goal could not be started. + * + * 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 could not be started. + * + * \return a BT:NodeStatus indicating whether the goal could not be started + * (BT:NodeStatus::SUCCESS) or if it failed due to another reason + * (BT:NodeStatus::FAILURE). + * + */ + BT::NodeStatus is_tiago_play_motion_module_action_start_fail(void); + + /** + * \brief Checks if the goal did not complete in the allowed time. + * + * 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 did not complete in the allowed time. + * + * \return a BT:NodeStatus indicating whether the goal did not complete in + * the allowed time (BT:NodeStatus::SUCCESS) or if it failed due to another + * reason (BT:NodeStatus::FAILURE). + * + */ + BT::NodeStatus is_tiago_play_motion_module_timeout(void); + + /** + * \brief Checks if the feedback has not been received for a long time. + * + * 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 + * feedback has not been received for a long time. + * + * \return a BT:NodeStatus indicating whether the feedback has not been + * received for a long time (BT:NodeStatus::SUCCESS) or if it failed due + * to another reason (BT:NodeStatus::FAILURE). + * + */ + BT::NodeStatus is_tiago_play_motion_module_fb_watchdog(void); + + /** + * \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 + * action goal could not be reached. + * + * \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). + * + */ + BT::NodeStatus is_tiago_play_motion_module_aborted(void); + + /** + * \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 action goal has been cancelled by another 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). + * + */ + BT::NodeStatus is_tiago_play_motion_module_preempted(void); + + /** + * \brief Checks if the goal information is not valid. + * + * 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. + * + * \return a BT:NodeStatus indicating whether the goal information is not + * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason + * (BT:NodeStatus::FAILURE). + * + */ + BT::NodeStatus is_tiago_play_motion_module_rejected(void); + + /** + * \brief Checks if execution_motion function has failed because an unknown id. + * + * This function must be used to know the type of failure, once the last + * goal has finished. Specifically, it checks if it failed because + * set_param function 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 + * (BT:NodeStatus::FAILURE). + * + */ + BT::NodeStatus is_tiago_play_motion_module_invalid_id(void); + + }; + + #endif diff --git a/package.xml b/package.xml index 0ff43a3caf205e37cdcd7724f18dc52d04e4a8fe..6bf92af93aade2e6a330dbea17892c73be69ab05 100644 --- a/package.xml +++ b/package.xml @@ -46,6 +46,7 @@ <build_depend>actionlib</build_depend> <build_depend>play_motion_msgs</build_depend> <build_depend>iri_base_algorithm</build_depend> + <build_depend>iri_behaviortree</build_depend> <!-- new dependencies --> <!--<build_depend>new build dependency</build_depend>--> <run_depend>iri_ros_tools</run_depend> @@ -54,6 +55,7 @@ <run_depend>actionlib</run_depend> <run_depend>play_motion_msgs</run_depend> <run_depend>iri_base_algorithm</run_depend> + <run_depend>iri_behaviortree</run_depend> <!-- new dependencies --> <!--<run_depend>new run dependency</run_depend>--> diff --git a/src/tiago_play_motion_bt_module.cpp b/src/tiago_play_motion_bt_module.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a2bf1f069f5acc0f8354633ed84d4d34bdfe3926 --- /dev/null +++ b/src/tiago_play_motion_bt_module.cpp @@ -0,0 +1,225 @@ +#include <tiago_play_motion_module/tiago_play_motion_bt_module.h> +#include <tiago_play_motion_module/tiago_play_motion_module.h> + +CTiagoPlayMotionModuleBT::CTiagoPlayMotionModuleBT() : + tiago_play_motion_module("tiago_play_motion_module",ros::this_node::getName()) +{ + +} + +CTiagoPlayMotionModuleBT::CTiagoPlayMotionModuleBT(std::string &module_name) : + tiago_play_motion_module(module_name,ros::this_node::getName()) +{ + +} + +void CTiagoPlayMotionModuleBT::init(IriBehaviorTreeFactory &factory) +{ + //Definition of ports + BT::PortsList sync_execute_motion_ports = {BT::InputPort<std::string>("motion_name")}; + BT::PortsList async_execute_motion_ports = {BT::InputPort<std::string>("motion_name"), BT::BidirectionalPort<bool>("new_goal")}; + + + //Registration of conditions + factory.registerSimpleCondition("is_tiago_play_motion_finished", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_finished, this)); + + factory.registerSimpleCondition("is_tiago_play_motion_module_running", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_running, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_success", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_success, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_action_start_fail", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_action_start_fail, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_timeout", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_timeout, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_fb_watchdog", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_fb_watchdog, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_aborted", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_aborted, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_preempted", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_preempted, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_rejected", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_rejected, this)); + factory.registerSimpleCondition("is_tiago_play_motion_module_invalid_id", std::bind(&CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_rejected, this)); + + // //Registration of actions + factory.registerIriAsyncAction("async_is_tiago_play_motion_finished", std::bind(&CTiagoPlayMotionModuleBT::async_is_tiago_play_motion_finished, this)); + + factory.registerSimpleAction("sync_cancel_tiago_play_motion_action", std::bind(&CTiagoPlayMotionModuleBT::sync_cancel_tiago_play_motion_action, this)); + factory.registerSimpleAction("async_cancel_tiago_play_motion_action", std::bind(&CTiagoPlayMotionModuleBT::async_cancel_tiago_play_motion_action, this)); + + factory.registerSimpleAction("sync_tiago_play_motion_execute_motion", std::bind(&CTiagoPlayMotionModuleBT::sync_tiago_play_motion_execute_motion, this, std::placeholders::_1), sync_execute_motion_ports); + factory.registerIriAsyncAction("async_tiago_play_motion_execute_motion", std::bind(&CTiagoPlayMotionModuleBT::async_tiago_play_motion_execute_motion, this, std::placeholders::_1), async_execute_motion_ports); + +} + +CTiagoPlayMotionModuleBT::~CTiagoPlayMotionModuleBT(void) +{ + // [free dynamic memory] +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::sync_tiago_play_motion_execute_motion(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoPlayMotionModuleBT::sync_tiago_play_motion_execute_motion-> sync_tiago_play_motion_execute_motion"); + BT::Optional<std::string> motion_name = self.getInput<std::string>("motion_name"); + + std::string motion_name_goal; + if (!motion_name) + { + ROS_ERROR("CTiagoPlayMotionModuleBT::sync_tiago_play_motion_execute_motion-> Incorrect or missing input. It needs the following input ports: motion_name(std::String)"); + return BT::NodeStatus::FAILURE; + } + + motion_name_goal = motion_name.value(); + this->tiago_play_motion_module.execute_motion(motion_name_goal); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::async_tiago_play_motion_execute_motion(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoPlayMotionModuleBT::async_tiago_play_motion_execute_motion-> async_tiago_play_motion_execute_motion"); + BT::Optional<std::string> motion_name = self.getInput<std::string>("motion_name"); + 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 motion_name_goal; + if (!motion_name) + { + ROS_ERROR("CTiagoPlayMotionModuleBT::async_tiago_play_motion_execute_motion-> Incorrect or missing input. It needs the following input ports: motion_name(std::String)"); + return BT::NodeStatus::FAILURE; + } + + motion_name_goal = motion_name.value(); + this->tiago_play_motion_module.execute_motion(motion_name_goal); + } + if (this->tiago_play_motion_module.is_finished()) + { + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_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 CTiagoPlayMotionModuleBT::sync_cancel_tiago_play_motion_action(void) +{ + ROS_DEBUG("CTiagoPlayMotionModuleBT::sync_cancel_tiago_play_motion_action-> sync_cancel_tiago_play_motion_action"); + this->tiago_play_motion_module.stop(); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::async_cancel_tiago_play_motion_action(void) +{ + ROS_DEBUG("CTiagoPlayMotionModuleBT::async_cancel_tiago_play_motion_action-> async_cancel_tiago_play_motion_action"); + this->tiago_play_motion_module.stop(); + if (this->tiago_play_motion_module.is_finished()) + { + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_SUCCESS) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; + } + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_finished(void) +{ + if (this->tiago_play_motion_module.is_finished()) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::async_is_tiago_play_motion_finished(void) +{ + if (this->tiago_play_motion_module.is_finished()) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_running(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_RUNNING) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_success(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_SUCCESS) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_action_start_fail(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_ACTION_START_FAIL) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_timeout(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_TIMEOUT) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_fb_watchdog(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_FB_WATCHDOG) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_aborted(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_ABORTED) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_preempted(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_PREEMPTED) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_rejected(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_REJECTED) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoPlayMotionModuleBT::is_tiago_play_motion_module_invalid_id(void) +{ + tiago_play_motion_module_status_t tiago_play_motion_module_status = this->tiago_play_motion_module.get_status(); + + if (tiago_play_motion_module_status == TIAGO_PLAY_MOTION_MODULE_INVALID_ID) + return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; +} + + +