From f8831f07ee5345e698b79a4dfb7c3b5ad40b46ab Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 23 Oct 2020 13:35:07 +0200
Subject: [PATCH] Used the genric handling of dynamic reconfigure parameters
for the module, module service and module action classes.
---
cfg/TiagoGripperModule.cfg | 15 ++++-----------
config/tiago_gripper_module_default.yaml | 18 +++++++++++-------
.../tiago_gripper_module.h | 2 +-
src/tiago_gripper_module.cpp | 18 ++++--------------
4 files changed, 20 insertions(+), 33 deletions(-)
diff --git a/cfg/TiagoGripperModule.cfg b/cfg/TiagoGripperModule.cfg
index 656bb9b..b905384 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 7662c47..cbee91d 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 80bd745..9749fe7 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 3ec41ab..fe14074 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();
}
--
GitLab