diff --git a/CMakeLists.txt b/CMakeLists.txt index 940d6a8c143c1e997635b3470142761948e035bd..d4134031aa93f9c8e8895f9ba81814a848e349ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(tiago_modules) +project(tiago_play_motion_module) ## Add support for C++11, supported in ROS Kinetic and newer add_definitions(-std=c++11) @@ -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 control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_interaction_msgs pal_waypoint_msgs moveit_ros_planning_interface) +find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -86,14 +86,7 @@ find_package(iriutils REQUIRED) ## Generate dynamic reconfigure parameters in the 'cfg' folder generate_dynamic_reconfigure_options( - cfg/GripperModule.cfg - cfg/TorsoModule.cfg - cfg/PlayMotionModule.cfg - cfg/HeadModule.cfg - cfg/NavModule.cfg - cfg/TTSModule.cfg - cfg/ArmModule.cfg - cfg/MovePlatformModule.cfg + cfg/TiagoPlayMotionModule.cfg ) ################################### @@ -107,8 +100,8 @@ generate_dynamic_reconfigure_options( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - LIBRARIES gripper_module play_motion_module head_module torso_module nav_module tts_module arm_module move_platform_module - CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs pal_interaction_msgs moveit_ros_planning_interface + LIBRARIES play_motion_module + CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs # DEPENDS system_lib ) @@ -124,125 +117,20 @@ include_directories(${catkin_INCLUDE_DIRS}) include_directories(${iriutils_INCLUDE_DIR}) ## Declare a C++ library -add_library(gripper_module - src/gripper_module.cpp +set(module_name tiago_play_motion_module) +add_library(${module_name} + src/tiago_play_motion_module.cpp ) -target_link_libraries(gripper_module ${catkin_LIBRARIES}) -target_link_libraries(gripper_module ${iriutils_LIBRARY}) +target_link_libraries(${module_name} ${catkin_LIBRARIES}) +target_link_libraries(${module_name} ${iriutils_LIBRARY}) ##Link to other modules ##target_link_libraries(new_module <module path>/lib<module_name>.so) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -add_dependencies(gripper_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Add dependencies to other modules -##add_dependencies(new_module <module_name>) - -add_library(torso_module - src/torso_module.cpp -) - -target_link_libraries(torso_module ${catkin_LIBRARIES}) -target_link_libraries(torso_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(torso_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(play_motion_module - src/play_motion_module.cpp -) - -target_link_libraries(play_motion_module ${catkin_LIBRARIES}) -target_link_libraries(play_motion_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(play_motion_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(head_module - src/head_module.cpp -) - -target_link_libraries(head_module ${catkin_LIBRARIES}) -target_link_libraries(head_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(head_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(nav_module - src/nav_module.cpp -) - -target_link_libraries(nav_module ${catkin_LIBRARIES}) -target_link_libraries(nav_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(nav_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(tts_module - src/tts_module.cpp -) - -target_link_libraries(tts_module ${catkin_LIBRARIES}) -target_link_libraries(tts_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(tts_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(arm_module - src/arm_module.cpp -) - -target_link_libraries(arm_module ${catkin_LIBRARIES}) -target_link_libraries(arm_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(arm_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(move_platform_module - src/move_platform_module.cpp -) - -target_link_libraries(move_platform_module ${catkin_LIBRARIES}) -target_link_libraries(move_platform_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(move_platform_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context diff --git a/cfg/PlayMotionModule.cfg b/cfg/TiagoPlayMotionModule.cfg similarity index 60% rename from cfg/PlayMotionModule.cfg rename to cfg/TiagoPlayMotionModule.cfg index 1be1b6f71a5d939b158e5ab3402b45713b7bc1d2..79d263b4d8d491ce0ad95851ac8e8d0842527a17 100755 --- a/cfg/PlayMotionModule.cfg +++ b/cfg/TiagoPlayMotionModule.cfg @@ -31,23 +31,15 @@ # Author: -PACKAGE='play_motion_module' +PACKAGE='tiago_play_motion_module' from dynamic_reconfigure.parameter_generator_catkin import * +from iri_ros_tools.dyn_params import add_module_action_params,add_module_params gen = ParameterGenerator() -play_action = gen.add_group("Play motion action") - # Name Type Reconfiguration level Description Default Min Max -#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0) -gen.add("rate_hz", double_t, 0, "Gripper module rate in Hz", 1.0, 1.0, 100.0) - -play_action.add("action_max_retries",int_t, 0, "Maximum number of retries for the action start", 1, 1, 10) -play_action.add("feedback_watchdog_time_s",double_t, 0, "Maximum time between feedback messages", 1, 0.01, 600) -play_action.add("enable_watchdog", bool_t, 0, "Enable action watchdog", True) -play_action.add("timeout_s", double_t, 0, "Maximum time allowed to complete the action", 1, 0.1, 30) -play_action.add("enable_timeout", bool_t, 0, "Enable action timeout", True) -play_action.add("enabled", bool_t, 0, "Enable action execution", True) - -exit(gen.generate(PACKAGE, "PlayMotionModuleAlgorithm", "PlayMotionModule")) +add_module_params(gen,"play_motion_module") +play_motion_action=add_module_action_params(gen,"play") +play_motion_action.add("motions_param_path",str_t,0, "Path of the motions in the parameter server","/play_motion/motions") +exit(gen.generate(PACKAGE, "TiagoPlayMotionModuleAlgorithm", "TiagoPlayMotionModule")) diff --git a/config/play_motion_module_default.yaml b/config/play_motion_module_default.yaml deleted file mode 100644 index 673142b12d4ec82b151858bc5d3937a0eb4001b5..0000000000000000000000000000000000000000 --- a/config/play_motion_module_default.yaml +++ /dev/null @@ -1,10 +0,0 @@ -rate_hz: 1.0 - -action_max_retries: 1 -feedback_watchdog_time_s: 1.0 -enable_watchdog: True -timeout_s: 1.0 -enable_timeout: True -enabled: True - - diff --git a/config/tiago_play_motion_module_default.yaml b/config/tiago_play_motion_module_default.yaml new file mode 100644 index 0000000000000000000000000000000000000000..bdfe3acd59f29c88293f4f6406c8550d433ca0e2 --- /dev/null +++ b/config/tiago_play_motion_module_default.yaml @@ -0,0 +1,9 @@ +play_motion_module_rate_hz: 1.0 + +play_num_retries: 1 +play_feedback_watchdog_time_s: 1.0 +play_enable_watchdog: True +play_timeout_s: 1.0 +play_enable_timeout: True +play_enabled: True + diff --git a/include/tiago_modules/play_motion_module.h b/include/tiago_modules/play_motion_module.h deleted file mode 100644 index 0ab39ae1c1463ac72eb8c93ebdf5ed94131a48af..0000000000000000000000000000000000000000 --- a/include/tiago_modules/play_motion_module.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef _PLAY_MOTION_MODULE_H -#define _PLAY_MOTION_MODULE_H - -#include <iri_ros_tools/module.h> -#include <iri_ros_tools/module_action.h> - -#include <tiago_modules/PlayMotionModuleConfig.h> - -//Action -#include <play_motion_msgs/PlayMotionAction.h> - -//States -typedef enum {PLAY_MOTION_MODULE_IDLE,PLAY_MOTION_MODULE_START,PLAY_MOTION_MODULE_WAIT} play_motion_module_state_t; - -//Status -typedef enum {PLAY_MOTION_MODULE_RUNNING, - PLAY_MOTION_MODULE_SUCCESS, - PLAY_MOTION_MODULE_ACTION_START_FAIL, - PLAY_MOTION_MODULE_TIMEOUT, - PLAY_MOTION_MODULE_FB_WATCHDOG, - PLAY_MOTION_MODULE_ABORTED, - PLAY_MOTION_MODULE_PREEMPTED, - PLAY_MOTION_MODULE_REJECTED, - PLAY_MOTION_MODULE_INVALID_ID} play_motion_module_status_t; - -// motion identifiers -typedef enum {HOME_MOTION=0, - UNFOLD_ARM_MOTION, - REACH_FLOOR_MOTION, - REACH_MAX_MOTION, - CLOSE_GRIPPER_MOTION, - CLOSE_GRIPPER_HALF_MOTION, - OPEN_GRIPPER_MOTION, - HEAD_TOUR_MOTION, - WAVE_MOTION, - PREGRASP_WEIGHT_MOTION, - DO_WEIGHTS_MOTION, - OFFER_GRIPPER_MOTION, - SHAKE_HANDS_MOTION, - PICK_FROM_FLOOR_MOTION} play_motion_ids; - -class CPlayMotionModule : public CModule<play_motion_module::PlayMotionModuleConfig> -{ - private: - - play_motion_module::PlayMotionModuleConfig config; - - CModuleAction<play_motion_msgs::PlayMotionAction> play_motion_action; - play_motion_msgs::PlayMotionGoal goal; - - //Variables - play_motion_module_state_t state; - play_motion_module_status_t status; - bool new_motion; - bool cancel_pending; - - protected: - void state_machine(void); - void reconfigure_callback(play_motion_module::PlayMotionModuleConfig &config, uint32_t level); - - public: - CPlayMotionModule(const std::string &name,const std::string &name_space=std::string("")); - void execute_motion(play_motion_ids motion_id); - void execute_motion(std::string motion_name); - void stop(void); - bool is_finished(void); - play_motion_module_status_t get_status(void); - - ~CPlayMotionModule(); - }; - - #endif diff --git a/include/tiago_play_motion_module/tiago_play_motion_module.h b/include/tiago_play_motion_module/tiago_play_motion_module.h new file mode 100644 index 0000000000000000000000000000000000000000..f414b30a90541fb16aa3b711a0ef5eff7432700e --- /dev/null +++ b/include/tiago_play_motion_module/tiago_play_motion_module.h @@ -0,0 +1,55 @@ +#ifndef _TIAGO_tiago_play_motion_MODULE_H +#define _TIAGO_tiago_play_motion_MODULE_H + +#include <iri_ros_tools/module.h> +#include <iri_ros_tools/module_action.h> + +#include <tiago_play_motion_module/TiagoPlayMotionModuleConfig.h> + +//Action +#include <play_motion_msgs/PlayMotionAction.h> + +//States +typedef enum {TIAGO_PLAY_MOTION_MODULE_IDLE,TIAGO_PLAY_MOTION_MODULE_START,TIAGO_PLAY_MOTION_MODULE_WAIT} tiago_play_motion_module_state_t; + +//Status +typedef enum {TIAGO_PLAY_MOTION_MODULE_RUNNING, + TIAGO_PLAY_MOTION_MODULE_SUCCESS, + TIAGO_PLAY_MOTION_MODULE_ACTION_START_FAIL, + TIAGO_PLAY_MOTION_MODULE_TIMEOUT, + TIAGO_PLAY_MOTION_MODULE_FB_WATCHDOG, + TIAGO_PLAY_MOTION_MODULE_ABORTED, + TIAGO_PLAY_MOTION_MODULE_PREEMPTED, + TIAGO_PLAY_MOTION_MODULE_REJECTED, + TIAGO_PLAY_MOTION_MODULE_INVALID_ID} tiago_play_motion_module_status_t; + +class CTiagoPlayMotionModule : public CModule<tiago_play_motion_module::TiagoPlayMotionModuleConfig> +{ + private: + + tiago_play_motion_module::TiagoPlayMotionModuleConfig config; + + CModuleAction<play_motion_msgs::PlayMotionAction,tiago_play_motion_module::TiagoPlayMotionModuleConfig> tiago_play_motion_action; + play_motion_msgs::PlayMotionGoal goal; + + //Variables + tiago_play_motion_module_state_t state; + tiago_play_motion_module_status_t status; + bool new_motion; + bool cancel_pending; + + protected: + void state_machine(void); + void reconfigure_callback(tiago_play_motion_module::TiagoPlayMotionModuleConfig &config, uint32_t level); + + public: + CTiagoPlayMotionModule(const std::string &name,const std::string &name_space=std::string("")); + void execute_motion(std::string motion_name); + void stop(void); + bool is_finished(void); + tiago_play_motion_module_status_t get_status(void); + + ~CTiagoPlayMotionModule(); + }; + + #endif diff --git a/package.xml b/package.xml index 41f80f3a0b6fc2767957bb3f77f5be2dcc15bdd4..84d2a58df1015a61efd4a5fc315f9793fddf5705 100644 --- a/package.xml +++ b/package.xml @@ -1,19 +1,19 @@ <?xml version="1.0"?> <package> - <name>tiago_modules</name> + <name>tiago_play_motion_module</name> <version>0.0.0</version> - <description>Module tiago_modules package</description> + <description>Module tiago_play_motion_module package</description> <!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- Example: --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> - <maintainer email="sergi@todo.todo">sergi</maintainer> + <maintainer email="labrobotica@iri.upc.edu">labrobotica</maintainer> <!-- One license tag required, multiple allowed, one license per tag --> <!-- Commonly used license strings: --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> - <license>TODO</license> + <license>LGPL</license> <!-- Url tags are optional, but multiple are allowed, one per tag --> @@ -25,7 +25,7 @@ <!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Authors do not have to be maintainers, but could be --> <!-- Example: --> - <!-- <author email="jane.doe@example.com">Jane Doe</author> --> + <author email="shernand@iri.upc.edu">Sergi Hernandez Juan</author> <!-- The *_depend tags are used to specify dependencies --> @@ -44,34 +44,14 @@ <build_depend>roscpp</build_depend> <build_depend>dynamic_reconfigure</build_depend> <build_depend>actionlib</build_depend> - <build_depend>std_srvs</build_depend> <build_depend>play_motion_msgs</build_depend> - <build_depend>control_msgs</build_depend> - <build_depend>geometry_msgs</build_depend> - <build_depend>move_base_msgs</build_depend> - <build_depend>nav_msgs</build_depend> - <build_depend>pal_navigation_msgs</build_depend> - <build_depend>pal_waypoint_msgs</build_depend> - <build_depend>pal_interaction_msgs</build_depend> - <build_depend>tf</build_depend> - <build_depend>moveit_ros_planning_interface</build_depend> <!-- new dependencies --> <!--<build_depend>new build dependency</build_depend>--> <run_depend>iri_ros_tools</run_depend> <run_depend>roscpp</run_depend> <run_depend>dynamic_reconfigure</run_depend> <run_depend>actionlib</run_depend> - <run_depend>std_srvs</run_depend> <run_depend>play_motion_msgs</run_depend> - <run_depend>control_msgs</run_depend> - <run_depend>geometry_msgs</run_depend> - <run_depend>move_base_msgs</run_depend> - <run_depend>nav_msgs</run_depend> - <run_depend>pal_navigation_msgs</run_depend> - <run_depend>pal_waypoint_msgs</run_depend> - <run_depend>pal_interaction_msgs</run_depend> - <run_depend>tf</run_depend> - <run_depend>moveit_ros_planning_interface</run_depend> <!-- new dependencies --> <!--<run_depend>new run dependency</run_depend>--> diff --git a/src/play_motion_module.cpp b/src/play_motion_module.cpp deleted file mode 100644 index ef176448c79939a8536d77cd8064848a43eacf44..0000000000000000000000000000000000000000 --- a/src/play_motion_module.cpp +++ /dev/null @@ -1,215 +0,0 @@ -#include <tiago_modules/play_motion_module.h> - - -CPlayMotionModule::CPlayMotionModule(const std::string &name,const std::string &name_space) : CModule(name,name_space), - play_motion_action("play_motion",this->module_nh.getNamespace()) -{ - this->start_operation(); - - //Variables - this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_SUCCESS; - this->new_motion=false; - this->cancel_pending=false; -} - -/* State machine */ -void CPlayMotionModule::state_machine(void) -{ -//Action - switch(this->state) - { - case PLAY_MOTION_MODULE_IDLE: ROS_DEBUG("CPlayMotionModule: Idle"); - if(this->new_motion) - { - this->new_motion=false; - this->state=PLAY_MOTION_MODULE_START; - this->status=PLAY_MOTION_MODULE_RUNNING; - } - else - this->state=PLAY_MOTION_MODULE_IDLE; - break; - - case PLAY_MOTION_MODULE_START: ROS_DEBUG("CPlayMotionModule: Start"); - switch(this->play_motion_action.make_request(this->goal)) - { - case ACT_SRV_SUCCESS: this->state=PLAY_MOTION_MODULE_WAIT; - ROS_DEBUG("CPlayMotionModule : goal start successfull"); - break; - case ACT_SRV_PENDING: this->state=PLAY_MOTION_MODULE_START; - ROS_WARN("CPlayMotionModule : goal start pending"); - break; - case ACT_SRV_FAIL: this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_ACTION_START_FAIL; - ROS_ERROR("CPlayMotionModule: goal start failed"); - break; - } - break; - - case PLAY_MOTION_MODULE_WAIT: ROS_DEBUG("CPlayMotionModule : state WAIT"); - switch(this->play_motion_action.get_state()) - { - case ACTION_IDLE: - case ACTION_RUNNING: ROS_DEBUG("CPlayMotionModule : action running"); - this->state=PLAY_MOTION_MODULE_WAIT; - break; - case ACTION_SUCCESS: ROS_INFO("CPlayMotionModule : action ended successfully"); - this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_SUCCESS; - this->play_motion_action.stop_timeout(); - break; - case ACTION_TIMEOUT: ROS_ERROR("CPlayMotionModule : action did not finish in the allowed time"); - this->play_motion_action.cancel(); - this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_TIMEOUT; - this->play_motion_action.stop_timeout(); - break; - case ACTION_FB_WATCHDOG: ROS_ERROR("CPlayMotionModule : No feeback received for a long time"); - this->play_motion_action.cancel(); - this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_FB_WATCHDOG; - this->play_motion_action.stop_timeout(); - break; - case ACTION_ABORTED: ROS_ERROR("CPlayMotionModule : Action failed to complete"); - this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_ABORTED; - this->play_motion_action.stop_timeout(); - break; - case ACTION_PREEMPTED: ROS_ERROR("CPlayMotionModule : Action was interrupted by another request"); - this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_PREEMPTED; - this->play_motion_action.stop_timeout(); - break; - case ACTION_REJECTED: ROS_ERROR("CPlayMotionModule : Action was rejected by server"); - this->state=PLAY_MOTION_MODULE_IDLE; - this->status=PLAY_MOTION_MODULE_REJECTED; - this->play_motion_action.stop_timeout(); - break; - } - if(this->cancel_pending) - { - this->cancel_pending=false; - this->play_motion_action.cancel(); - } - break; - } -} - -void CPlayMotionModule::reconfigure_callback(play_motion_module::PlayMotionModuleConfig &config, uint32_t level) -{ - ROS_INFO("CPlayMotionModule : reconfigure callback"); - this->lock(); - this->config=config; - /* set the module rate */ - this->set_rate(config.rate_hz); - /* motion action parameters */ - this->play_motion_action.set_max_num_retries(config.action_max_retries); - if(config.enable_watchdog) - this->play_motion_action.enable_watchdog(config.feedback_watchdog_time_s); - else - this->play_motion_action.disable_watchdog(); - if(this->config.enable_timeout) - this->play_motion_action.enable_timeout(config.timeout_s); - else - this->play_motion_action.disable_timeout(); - if(this->config.enabled) - this->play_motion_action.enable(); - else - this->play_motion_action.disable(); - this->unlock(); -} - -void CPlayMotionModule::execute_motion(play_motion_ids motion_id) -{ - if(this->is_finished()) - { - this->lock(); - switch(motion_id) - { - case HOME_MOTION: this->goal.motion_name="home"; - break; - case UNFOLD_ARM_MOTION: this->goal.motion_name="unfold_arm"; - break; - case REACH_FLOOR_MOTION: this->goal.motion_name="reach_floor"; - break; - case REACH_MAX_MOTION: this->goal.motion_name="reach_max"; - break; - case CLOSE_GRIPPER_MOTION: this->goal.motion_name="close_gripper"; - break; - case CLOSE_GRIPPER_HALF_MOTION: this->goal.motion_name="close_gripper_half"; - break; - case OPEN_GRIPPER_MOTION: this->goal.motion_name="open_gripper"; - break; - case HEAD_TOUR_MOTION: this->goal.motion_name="head_tour"; - break; - case WAVE_MOTION: this->goal.motion_name="wave"; - break; - case PREGRASP_WEIGHT_MOTION: this->goal.motion_name="pregrasp_weight"; - break; - case DO_WEIGHTS_MOTION: this->goal.motion_name="do_weights"; - break; - case OFFER_GRIPPER_MOTION: this->goal.motion_name="offer_gripper"; - break; - case SHAKE_HANDS_MOTION: this->goal.motion_name="shake_hands"; - break; - case PICK_FROM_FLOOR_MOTION: this->goal.motion_name="pick_from_floor"; - break; - default: this->status=PLAY_MOTION_MODULE_INVALID_ID; - this->unlock(); - return; - } - this->goal.skip_planning=false; - this->goal.priority=0; - this->new_motion=true; - this->unlock(); - } -} - -void CPlayMotionModule::execute_motion(std::string motion_name) -{ - std::string motion; - - if(this->is_finished()) - { - if(this->module_nh.getParam("/play_motion/motions/" + motion_name + "/meta/name", motion)) //If motion exists - { - this->goal.motion_name=motion_name; - this->goal.skip_planning=false; - this->goal.priority=0; - this->new_motion=true; - } - else - { - ROS_WARN("CPlayMotionModule: Motion name does not exist"); - this->status=PLAY_MOTION_MODULE_INVALID_ID; - } - } -} - -void CPlayMotionModule::stop(void) -{ - if(this->state!=PLAY_MOTION_MODULE_IDLE) - this->cancel_pending=true; -} - -bool CPlayMotionModule::is_finished(void) -{ - if(this->state==PLAY_MOTION_MODULE_IDLE && this->new_motion==false) - return true; - else - return false; -} - -play_motion_module_status_t CPlayMotionModule::get_status(void) -{ - return this->status; -} - -CPlayMotionModule::~CPlayMotionModule() -{ - if(!this->play_motion_action.is_finished()) - { - this->play_motion_action.cancel(); - while(!this->play_motion_action.is_finished()); - } -} diff --git a/src/tiago_play_motion_module.cpp b/src/tiago_play_motion_module.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d6d844596a77868c21e6ed0614a0f1fd0001a706 --- /dev/null +++ b/src/tiago_play_motion_module.cpp @@ -0,0 +1,171 @@ +#include <tiago_play_motion_module/tiago_play_motion_module.h> + + +CTiagoPlayMotionModule::CTiagoPlayMotionModule(const std::string &name,const std::string &name_space) : CModule(name,name_space), + tiago_play_motion_action("tiago_play_motion",this->module_nh.getNamespace()) +{ + this->start_operation(); + + //Variables + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_SUCCESS; + this->new_motion=false; + this->cancel_pending=false; +} + +/* State machine */ +void CTiagoPlayMotionModule::state_machine(void) +{ +//Action + switch(this->state) + { + case TIAGO_PLAY_MOTION_MODULE_IDLE: ROS_DEBUG("CTiagoPlayMotionModule: Idle"); + if(this->new_motion) + { + this->new_motion=false; + this->state=TIAGO_PLAY_MOTION_MODULE_START; + this->status=TIAGO_PLAY_MOTION_MODULE_RUNNING; + } + else + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + break; + + case TIAGO_PLAY_MOTION_MODULE_START: ROS_DEBUG("CTiagoPlayMotionModule: Start"); + switch(this->tiago_play_motion_action.make_request(this->goal)) + { + case ACT_SRV_SUCCESS: this->state=TIAGO_PLAY_MOTION_MODULE_WAIT; + ROS_DEBUG("CTiagoPlayMotionModule : goal start successfull"); + break; + case ACT_SRV_PENDING: this->state=TIAGO_PLAY_MOTION_MODULE_START; + ROS_WARN("CTiagoPlayMotionModule : goal start pending"); + break; + case ACT_SRV_FAIL: this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_ACTION_START_FAIL; + ROS_ERROR("CTiagoPlayMotionModule: goal start failed"); + break; + } + break; + + case TIAGO_PLAY_MOTION_MODULE_WAIT: ROS_DEBUG("CTiagoPlayMotionModule : state WAIT"); + switch(this->tiago_play_motion_action.get_state()) + { + case ACTION_IDLE: + case ACTION_RUNNING: ROS_DEBUG("CTiagoPlayMotionModule : action running"); + this->state=TIAGO_PLAY_MOTION_MODULE_WAIT; + break; + case ACTION_SUCCESS: ROS_INFO("CTiagoPlayMotionModule : action ended successfully"); + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_SUCCESS; + this->tiago_play_motion_action.stop_timeout(); + break; + case ACTION_TIMEOUT: ROS_ERROR("CTiagoPlayMotionModule : action did not finish in the allowed time"); + this->tiago_play_motion_action.cancel(); + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_TIMEOUT; + this->tiago_play_motion_action.stop_timeout(); + break; + case ACTION_FB_WATCHDOG: ROS_ERROR("CTiagoPlayMotionModule : No feeback received for a long time"); + this->tiago_play_motion_action.cancel(); + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_FB_WATCHDOG; + this->tiago_play_motion_action.stop_timeout(); + break; + case ACTION_ABORTED: ROS_ERROR("CTiagoPlayMotionModule : Action failed to complete"); + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_ABORTED; + this->tiago_play_motion_action.stop_timeout(); + break; + case ACTION_PREEMPTED: ROS_ERROR("CTiagoPlayMotionModule : Action was interrupted by another request"); + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_PREEMPTED; + this->tiago_play_motion_action.stop_timeout(); + break; + case ACTION_REJECTED: ROS_ERROR("CTiagoPlayMotionModule : Action was rejected by server"); + this->state=TIAGO_PLAY_MOTION_MODULE_IDLE; + this->status=TIAGO_PLAY_MOTION_MODULE_REJECTED; + this->tiago_play_motion_action.stop_timeout(); + break; + } + if(this->cancel_pending) + { + this->cancel_pending=false; + this->tiago_play_motion_action.cancel(); + } + break; + } +} + +void CTiagoPlayMotionModule::reconfigure_callback(tiago_play_motion_module::TiagoPlayMotionModuleConfig &config, uint32_t level) +{ + ROS_INFO("CTiagoPlayMotionModule : reconfigure callback"); + this->lock(); + this->config=config; + /* set the module rate */ + this->dynamic_reconfigure(config,"play_motion_module"); + /* motion action parameters */ + this->tiago_play_motion_action.dynamic_reconfigure(config,"play"); + this->unlock(); +} + +void CTiagoPlayMotionModule::execute_motion(std::string motion_name) +{ + XmlRpc::XmlRpcValue symbols; + + if(!ros::param::get(this->config_.motions_param_path.c_str(), symbols)) + return; + else + { + if(symbols.getType()==XmlRpc::XmlRpcValue::TypeArray) + return; + for (XmlRpc::XmlRpcValue::iterator i=symbols.begin(); i!=symbols.end(); ++i) + { + std::cout << i->second[i] << std::endl; + } + } +/* + std::string motion; + + if(this->is_finished()) + { + if(this->module_nh.getParam("/play_motion/motions/" + motion_name + "/meta/name", motion)) //If motion exists + { + this->goal.motion_name=motion_name; + this->goal.skip_planning=false; + this->goal.priority=0; + this->new_motion=true; + } + else + { + ROS_WARN("CTiagoPlayMotionModule: Motion name does not exist"); + this->status=TIAGO_PLAY_MOTION_MODULE_INVALID_ID; + } + }*/ +} + +void CTiagoPlayMotionModule::stop(void) +{ + if(this->state!=TIAGO_PLAY_MOTION_MODULE_IDLE) + this->cancel_pending=true; +} + +bool CTiagoPlayMotionModule::is_finished(void) +{ + if(this->state==TIAGO_PLAY_MOTION_MODULE_IDLE && this->new_motion==false) + return true; + else + return false; +} + +tiago_play_motion_module_status_t CTiagoPlayMotionModule::get_status(void) +{ + return this->status; +} + +CTiagoPlayMotionModule::~CTiagoPlayMotionModule() +{ + if(!this->tiago_play_motion_action.is_finished()) + { + this->tiago_play_motion_action.cancel(); + while(!this->tiago_play_motion_action.is_finished()); + } +}