Added navigation params for Task4

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

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"))

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,

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>-->

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();
 }