Used the genric handling of dynamic reconfigure parameters for the module,...

Used the genric handling of dynamic reconfigure parameters for the module, module service and module action classes.

cfg/TiagoGripperModule.cfg

@@ -34,23 +34,16 @@
 PACKAGE='tiago_gripper_module'
 
 from dynamic_reconfigure.parameter_generator_catkin import *
+from iri_ros_tools.dyn_params import add_module_service_params,add_module_action_params,add_module_params
 
 gen = ParameterGenerator()
 
-motion_action = gen.add_group("Gripper 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)
-
-motion_action.add("action_max_retries",int_t, 0,                               "Maximum number of retries for the action start",1,1,10)
-motion_action.add("feedback_watchdog_time_s",double_t,    0,                   "Maximum time between feedback messages",    10,   0.01,600)
-motion_action.add("enable_watchdog",bool_t,   0,                               "Enable action watchdog",          True)
-motion_action.add("timeout_s",     double_t,  0,                               "Maximum time allowed to complete the action",1,   0.1,30)
-motion_action.add("enable_timeout",bool_t,    0,                               "Enable action timeout",           True)
+add_module_params(gen,"gripper_module")
+add_module_service_params(gen,"grasp")
+motion_action=add_module_action_params(gen,"move")
 motion_action.add("close_dist",    double_t,  0,                               "Close position distance",         0.01,     0.0,  0.045)
 motion_action.add("open_dist",     double_t,  0,                               "Open position distance",          0.035,    0.0,  0.045)
 motion_action.add("default_duration",double_t,0,                               "Default gripper motion duration ",1.0,      0.1,  10.0)
-motion_action.add("enabled",       bool_t,    0,                               "Enable action execution",         True)
 
 exit(gen.generate(PACKAGE, "TiagoGripperModuleAlgorithm", "TiagoGripperModule"))

config/tiago_gripper_module_default.yaml

-rate_hz: 1.0
+gripper_module_rate_hz: 1.0
+
+grasp_num_retries: 1
+grasp_enabled: True
+
+move_num_retries: 1
+move_feedback_watchdog_time_s: 10.0
+move_enable_watchdog: True
+move_timeout_s: 1.0
+move_enable_timeout: True
+move_enabled: True
 
-action_max_retries: 1
-feedback_watchdog_time_s: 10.0
-enable_watchdog: True
-timeout_s: 1.0
-enable_timeout: True
 close_dist: 0.01
 open_dist: 0.035
 default_duration: 1.0
-enabled: True
 

include/tiago_gripper_module/tiago_gripper_module.h

@@ -42,7 +42,7 @@ class CTiagoGripperModule : public CModule<tiago_gripper_module::TiagoGripperMod
     /****************Services*****************/
 
     /****************Actions*****************/
-    CModuleAction<control_msgs::FollowJointTrajectoryAction> gripper_action;
+    CModuleAction<control_msgs::FollowJointTrajectoryAction,tiago_gripper_module::TiagoGripperModuleConfig> gripper_action;
     control_msgs::FollowJointTrajectoryGoal goal;
     /****************Actions*****************/
 

src/tiago_gripper_module.cpp

@@ -149,21 +149,11 @@ void CTiagoGripperModule::reconfigure_callback(tiago_gripper_module::TiagoGrippe
   this->lock();
   this->config=config;
   /* set the module rate */
-  this->set_rate(config.rate_hz);
+  this->dynamic_reconfigure(config,"gripper_module");
+  /* module service parameters */
+  this->grasp_service.dynamic_reconfigure(config,"grasp");
   /* motion action parameters */
-  this->gripper_action.set_max_num_retries(config.action_max_retries);
-  if(config.enable_watchdog)
-    this->gripper_action.enable_watchdog(config.feedback_watchdog_time_s);
-  else
-    this->gripper_action.disable_watchdog();
-  if(this->config.enable_timeout)
-    this->gripper_action.enable_timeout(config.timeout_s);
-  else
-    this->gripper_action.disable_timeout();
-  if(this->config.enabled)
-    this->gripper_action.enable();
-  else
-    this->gripper_action.disable();
+  this->gripper_action.dynamic_reconfigure(config,"move");
   this->unlock();
 }