diff --git a/cfg/TiagoGripperModule.cfg b/cfg/TiagoGripperModule.cfg index 656bb9b70796beea204c0fadc585a6fe301bedde..b905384bc40fdcab6d9fe137ff7bbff8edd99701 100755 --- a/cfg/TiagoGripperModule.cfg +++ b/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")) diff --git a/config/tiago_gripper_module_default.yaml b/config/tiago_gripper_module_default.yaml index 7662c4707f07d9b88dcefd1741f933fc77a3b8c5..cbee91dedd34fb4860cbe9310b23e4a03212ed36 100644 --- a/config/tiago_gripper_module_default.yaml +++ b/config/tiago_gripper_module_default.yaml @@ -1,12 +1,16 @@ -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 diff --git a/include/tiago_gripper_module/tiago_gripper_module.h b/include/tiago_gripper_module/tiago_gripper_module.h index 80bd74547b45c870a9b989eeba9a03c4a4709431..9749fe7f3109218b61989d24818a001379357a6f 100644 --- a/include/tiago_gripper_module/tiago_gripper_module.h +++ b/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*****************/ diff --git a/src/tiago_gripper_module.cpp b/src/tiago_gripper_module.cpp index 3ec41ab6b1e963d5ff93e68654d4e9cda8cd62f8..fe1407496b39ea9d047cbb813614ecfe8543f072 100644 --- a/src/tiago_gripper_module.cpp +++ b/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(); }