From fa97cdd2d710a22f85ea6aa8ea3fec1a5114393c Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 14 Dec 2017 07:41:20 +0000
Subject: [PATCH] Used the ros::this_node::getName() for the module namespace.
 Added a launch file for the real robot. Modified the simulation launch file
 to use the new launch file. Used the YAML configuration file.

---
 launch/head_client.launch     | 22 ++++++++++++++++++++++
 launch/head_client_sim.launch | 32 +-------------------------------
 src/head_client_alg_node.cpp  |  2 +-
 3 files changed, 24 insertions(+), 32 deletions(-)
 create mode 100644 launch/head_client.launch

diff --git a/launch/head_client.launch b/launch/head_client.launch
new file mode 100644
index 0000000..307ec67
--- /dev/null
+++ b/launch/head_client.launch
@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- launch the play motion client node -->
+  <node name="head_client"
+        pkg="head_client"
+        type="head_client"
+        output="screen"
+        ns="/tiago">
+    <remap from="~/head_module/move_head"
+             to="/head_controller/follow_joint_trajectory"/>
+    <remap from="~/head_module/point_head"
+             to="/head_controller/point_head_action"/>
+
+    <rosparam file="$(find tiago_modules)/config/head_module_default.yaml" command="load" ns="head_module" />
+  </node>
+
+  <!-- launch dynamic reconfigure -->
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+    output="screen"/>
+
+</launch>
+
diff --git a/launch/head_client_sim.launch b/launch/head_client_sim.launch
index 25f6a44..90ad8b8 100644
--- a/launch/head_client_sim.launch
+++ b/launch/head_client_sim.launch
@@ -5,37 +5,7 @@
     <arg name="robot" value="steel" />
   </include>
 
-  <!-- launch the play motion client node -->
-  <node name="head_client"
-        pkg="head_client"
-        type="head_client"
-        output="screen"
-        ns="/tiago">
-    <remap from="~/head_module/move_head"
-             to="/head_controller/follow_joint_trajectory"/>
-    <remap from="~/head_module/point_head"
-             to="/head_controller/point_head_action"/>
-    <param name="~/head_module/rate_hz" value="10"/>
-    <param name="~/head_module/move_max_retries" value="5"/>
-    <param name="~/head_module/move_enable_timeout" value="false"/>
-    <param name="~/head_module/move_feedback_watchdog_time_s" value="20"/>
-    <param name="~/head_module/move_timeout_s" value="10"/>
-    <param name="~/head_module/move_default_duration" value="1.0"/>
-    <param name="~/head_module/move_enabled" value="true"/>
-
-    <param name="~/head_module/point_max_retries" value="5"/>
-    <param name="~/head_module/point_enable_timeout" value="false"/>
-    <param name="~/head_module/point_feedback_watchdog_time_s" value="20"/>
-    <param name="~/head_module/point_timeout_s" value="10"/>
-    <param name="~/head_module/point_default_duration" value="1.0"/>
-    <param name="~/head_module/point_max_velocity" value="1.0"/>
-    <param name="~/head_module/point_min_velocity" value="0.25"/>
-    <param name="~/head_module/point_enabled" value="true"/>
-  </node>
-
-  <!-- launch dynamic reconfigure -->
-  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
-    output="screen"/>
+  <include file="$(find head_client)/launch/head_client.launch">
 
 </launch>
 
diff --git a/src/head_client_alg_node.cpp b/src/head_client_alg_node.cpp
index 9690570..4d618cb 100644
--- a/src/head_client_alg_node.cpp
+++ b/src/head_client_alg_node.cpp
@@ -3,7 +3,7 @@
 
 HeadClientAlgNode::HeadClientAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<HeadClientAlgorithm>(),
-  head("head_module")
+  head("head_module",ros::this_node::getName())
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
-- 
GitLab