diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5036e2fe2f5f1749db0863f5feddff93d7749616..8037976b1ef05a3832fdd6952fd890dfa135f6ad 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -93,6 +93,7 @@ generate_dynamic_reconfigure_options(
cfg/HeadModule.cfg
cfg/NavModule.cfg
cfg/TTSModule.cfg
+ cfg/ArmModule.cfg
)
###################################
@@ -227,6 +228,21 @@ target_link_libraries(tts_module ${iriutils_LIBRARY})
## 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++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
diff --git a/cfg/PlayMotionModule.cfg b/cfg/PlayMotionModule.cfg
index 67ddc2461d92ea6f4f3a2431d16dea0cd0011806..0459d84ef84b6495f9cafa3e1fb57f3696d103f5 100755
--- a/cfg/PlayMotionModule.cfg
+++ b/cfg/PlayMotionModule.cfg
@@ -41,11 +41,12 @@ 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)
+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("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, 50)
-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("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"))
diff --git a/include/tiago_modules/play_motion_module.h b/include/tiago_modules/play_motion_module.h
index 16c4dd1532067bffa970802403d964bf355e7688..f383d44f5158a4ef614a152f92bdd09de8fab3a2 100644
--- a/include/tiago_modules/play_motion_module.h
+++ b/include/tiago_modules/play_motion_module.h
@@ -13,7 +13,8 @@
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_SUCCESS,
+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,
diff --git a/package.xml b/package.xml
index 6f243bd56f0ee4ab09823b9380794a618f5dcddd..4d4a9d2f1eecd3576a0d3f24404fd6e81b5bd9f1 100644
--- a/package.xml
+++ b/package.xml
@@ -53,6 +53,7 @@
<build_depend>pal_navigation_msgs</build_depend>
<build_depend>pal_waypoint_msgs</build_depend>
<build_depend>pal_interaction_msgs</build_depend>
+ <build_depend>moveit_msgs</build_depend>
<build_depend>tf</build_depend>
<!-- new dependencies -->
<!--<build_depend>new build dependency</build_depend>-->
@@ -69,6 +70,7 @@
<run_depend>pal_navigation_msgs</run_depend>
<run_depend>pal_waypoint_msgs</run_depend>
<run_depend>pal_interaction_msgs</run_depend>
+ <run_depend>moveit_msgs</run_depend>
<run_depend>tf</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
index 34e2368802cc81e4d569503571737c226290f54a..69d967600b4fb6f1cd935201c066f88d3ecfe105 100644
--- a/src/play_motion_module.cpp
+++ b/src/play_motion_module.cpp
@@ -2,7 +2,7 @@
CPlayMotionModule::CPlayMotionModule(const std::string &name,const std::string &name_space) : CModule(name,name_space),
- play_motion_action("play_motion",name)
+ play_motion_action("play_motion",this->module_nh.getNamespace())
{
this->start_operation();
@@ -24,6 +24,7 @@ void CPlayMotionModule::state_machine(void)
{
this->new_motion=false;
this->state=PLAY_MOTION_MODULE_START;
+ this->status=PLAY_MOTION_MODULE_RUNNING;
}
else
this->state=PLAY_MOTION_MODULE_IDLE;
@@ -39,6 +40,7 @@ void CPlayMotionModule::state_machine(void)
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;
}
@@ -107,6 +109,10 @@ void CPlayMotionModule::reconfigure_callback(play_motion_module::PlayMotionModul
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();
}