Modified the client for the new version of the module.

Updated the launch files with external arguments.
Added configuration and launch files for the IVO robot.

cfg/HeadClient.cfg → cfg/TiagoHeadClient.cfg

@@ -31,7 +31,7 @@
 
 # Author: 
 
-PACKAGE='head_client'
+PACKAGE='tiago_head_module'
 
 from dynamic_reconfigure.parameter_generator_catkin import *
 
@@ -58,4 +58,4 @@ point_action.add("max_velocity",   double_t,  0,                               "
 point_action.add("start_point",    bool_t,  0,                                 "Start Pointing to target",       False)
 point_action.add("stop_point",     bool_t,  0,                                 "Stop Pointing to target",        False)
 
-exit(gen.generate(PACKAGE, "HeadClient", "HeadClient"))
+exit(gen.generate(PACKAGE, "TiagoHeadClient", "TiagoHeadClient"))

cfg/TiagoHeadModule.cfg

@@ -49,5 +49,6 @@ point_action=add_module_action_params(gen,"point")
 point_action.add("point_default_duration",        double_t,  0,  "Default poiting time",                              1.0,   1.0,  10.0)
 point_action.add("point_max_velocity",            double_t,  0,  "Maximum poiting velocity",                          1.0,   0.1,  10.0)
 point_action.add("point_min_velocity",            double_t,  0,  "Minimum poiting velocity",                          0.1,   0.1,  10.0)
+point_action.add("point_frame_id",                str_t,     0,  "Frame ID used for pointing",                        "")
 
 exit(gen.generate(PACKAGE, "TiagoHeadModuleAlgorithm", "TiagoHeadModule"))

config/ivo_head_module_default.yaml

+head_module_rate_hz: 1.0
+
+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_default_duration: 1.0
+move_enabled: True
+
+point_num_retries: 1
+point_feedback_watchdog_time_s: 10.0
+point_enable_watchdog: True
+point_timeout_s: 1.0
+point_enable_timeout: True
+point_default_duration: 1.0
+point_max_velocity: 1.0
+point_min_velocity: 0.1
+point_enabled: True
+point_frame_id: head_camera_color_optical_frame

config/head_module_default.yaml → config/tiago_head_module_default.yaml

-rate_hz: 1.0
+head_module_rate_hz: 1.0
 
-move_max_retries: 1
-move_feedback_watchdog_time_s: 1.0
+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_default_duration: 1.0
 move_enabled: True
 
-point_max_retries: 1
-point_feedback_watchdog_time_s: 1.0
+point_num_retries: 1
+point_feedback_watchdog_time_s: 10.0
 point_enable_watchdog: True
 point_timeout_s: 1.0
 point_enable_timeout: True
@@ -17,4 +17,4 @@ point_default_duration: 1.0
 point_max_velocity: 1.0
 point_min_velocity: 0.1
 point_enabled: True
-
+point_frame_id: xtion_rgb_optical_frame

include/head_client_alg.h → include/tiago_head_client_alg.h

@@ -22,25 +22,25 @@
 // refer to the IRI wiki page for more information:
 // http://wikiri.upc.es/index.php/Robotics_Lab
 
-#ifndef _head_client_alg_h_
-#define _head_client_alg_h_
+#ifndef _tiago_head_client_alg_h_
+#define _tiago_head_client_alg_h_
 
-#include <head_client/HeadClientConfig.h>
+#include <tiago_head_module/TiagoHeadClientConfig.h>
 
-//include humanoid_common_head_client_alg main library
+//include humanoid_common_tiago_head_client_alg main library
 
 /**
  * \brief IRI ROS Specific Driver Class
  *
  *
  */
-class HeadClientAlgorithm
+class TiagoHeadClientAlgorithm
 {
   protected:
    /**
     * \brief define config type
     *
-    * Define a Config type with the HeadClientConfig. All driver implementations
+    * Define a Config type with the TiagoHeadClientConfig. All driver implementations
     * will then use the same variable type Config.
     */
     pthread_mutex_t access_;    
@@ -51,10 +51,10 @@ class HeadClientAlgorithm
    /**
     * \brief define config type
     *
-    * Define a Config type with the HeadClientConfig. All driver implementations
+    * Define a Config type with the TiagoHeadClientConfig. All driver implementations
     * will then use the same variable type Config.
     */
-    typedef head_client::HeadClientConfig Config;
+    typedef tiago_head_module::TiagoHeadClientConfig Config;
 
    /**
     * \brief config variable
@@ -72,7 +72,7 @@ class HeadClientAlgorithm
     * Attributes from the main node driver class IriBaseDriver such as loop_rate,
     * may be also overload here.
     */
-    HeadClientAlgorithm(void);
+    TiagoHeadClientAlgorithm(void);
 
    /**
     * \brief Lock Algorithm
@@ -116,7 +116,7 @@ class HeadClientAlgorithm
     */
     void config_update(Config& new_cfg, uint32_t level=0);
 
-    // here define all head_client_alg interface methods to retrieve and set
+    // here define all tiago_head_client_alg interface methods to retrieve and set
     // the driver parameters
 
    /**
@@ -125,7 +125,7 @@ class HeadClientAlgorithm
     * This destructor is called when the object is about to be destroyed.
     *
     */
-    ~HeadClientAlgorithm(void);
+    ~TiagoHeadClientAlgorithm(void);
 };
 
 #endif

include/head_client_alg_node.h → include/tiago_head_client_alg_node.h

@@ -22,11 +22,11 @@
 // refer to the IRI wiki page for more information:
 // http://wikiri.upc.es/index.php/Robotics_Lab
 
-#ifndef _head_client_alg_node_h_
-#define _head_client_alg_node_h_
+#ifndef _tiago_head_client_alg_node_h_
+#define _tiago_head_client_alg_node_h_
 
 #include <iri_base_algorithm/iri_base_algorithm.h>
-#include "head_client_alg.h"
+#include "tiago_head_client_alg.h"
 
 // [publisher subscriber headers]
 
@@ -34,13 +34,13 @@
 
 // [head server client headers]
 
-#include <tiago_modules/head_module.h>
+#include <tiago_head_module/tiago_head_module.h>
 
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
  */
-class HeadClientAlgNode : public algorithm_base::IriBaseAlgorithm<HeadClientAlgorithm>
+class TiagoHeadClientAlgNode : public algorithm_base::IriBaseAlgorithm<TiagoHeadClientAlgorithm>
 {
   private:
     // [publisher attributes]
@@ -55,7 +55,7 @@ class HeadClientAlgNode : public algorithm_base::IriBaseAlgorithm<HeadClientAlgo
 
     // [head client attributes]
 
-    CHeadModule head;
+    CTiagoHeadModule head;
 
   public:
    /**
@@ -64,7 +64,7 @@ class HeadClientAlgNode : public algorithm_base::IriBaseAlgorithm<HeadClientAlgo
     * This constructor initializes specific class attributes and all ROS
     * communications variables to enable message exchange.
     */
-    HeadClientAlgNode(void);
+    TiagoHeadClientAlgNode(void);
 
    /**
     * \brief Destructor
@@ -72,7 +72,7 @@ class HeadClientAlgNode : public algorithm_base::IriBaseAlgorithm<HeadClientAlgo
     * This destructor frees all necessary dynamic memory allocated within this
     * this class.
     */
-    ~HeadClientAlgNode(void);
+    ~TiagoHeadClientAlgNode(void);
 
   protected:
    /**

launch/head_client.launch

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

launch/head_client_sim.launch

-<launch>
-
-  <include file="$(find tiago_gazebo)/launch/tiago_gazebo.launch">
-    <arg name="public_sim" value="true" />
-    <arg name="robot" value="steel" />
-  </include>
-
-  <include file="$(find head_client)/launch/head_client.launch"/>
-
-</launch>
-

launch/tiago_head_client.launch

+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="node_name" default="head_client" />
+  <arg name="output" default="log" />
+  <arg name="launch_prefix" default="" />
+  <arg name="config_file" default="$(find tiago_head_module)/config/tiago_head_module_default.yaml" />
+  <arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
+  <arg name="point_action" default="/head_controller/point_head_action"/>
+
+  <!-- launch the play motion client node -->
+  <node name="$(arg node_name)"
+        pkg="tiago_head_module"
+        type="tiago_head_client"
+        output="$(arg output)"
+        launch-prefix="$(arg launch_prefix)">
+    <remap from="~/head_module/move_head"
+             to="$(arg move_action)"/>
+    <remap from="~/head_module/point_head"
+             to="$(arg point_action)"/>
+
+    <rosparam file="$(arg config_file)" command="load" ns="head_module" />
+  </node>
+
+  <!-- launch dynamic reconfigure -->
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+    output="screen"/>
+
+</launch>
+

launch/tiago_head_client_sim.launch

+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="node_name" default="head_client" />
+  <arg name="output" default="log" />
+  <arg name="launch_prefix" default="" />
+  <arg name="config_file" default="$(find tiago_head_module)/config/tiago_head_module_default.yaml" />
+  <arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
+  <arg name="point_action" default="/head_controller/point_head_action"/>
+
+  <include file="$(find tiago_gazebo)/launch/tiago_gazebo.launch">
+    <arg name="public_sim" value="true" />
+    <arg name="robot" value="steel" />
+  </include>
+
+  <include file="$(find tiago_head_module)/launch/tiago_head_client.launch">
+    <arg name="node_name"     default="$(arg node_name)" />
+    <arg name="output"        default="$(arg output)" />
+    <arg name="launch_prefix" default="$(arg launch_prefix)" />
+    <arg name="config_file"   default="$(arg config_file)" />
+    <arg name="move_action"   default="$(arg move_action)"/>
+    <arg name="point_action"  default="$(arg point_action)"/>
+  </include>
+
+</launch>
+

src/head_client_alg.cpp → src/tiago_head_client_alg.cpp

-#include "head_client_alg.h"
+#include "tiago_head_client_alg.h"
 
-HeadClientAlgorithm::HeadClientAlgorithm(void)
+TiagoHeadClientAlgorithm::TiagoHeadClientAlgorithm(void)
 {
   pthread_mutex_init(&this->access_,NULL);
 }
 
-HeadClientAlgorithm::~HeadClientAlgorithm(void)
+TiagoHeadClientAlgorithm::~TiagoHeadClientAlgorithm(void)
 {
   pthread_mutex_destroy(&this->access_);
 }
 
-void HeadClientAlgorithm::config_update(Config& new_cfg, uint32_t level)
+void TiagoHeadClientAlgorithm::config_update(Config& new_cfg, uint32_t level)
 {
   this->lock();
 
@@ -20,4 +20,4 @@ void HeadClientAlgorithm::config_update(Config& new_cfg, uint32_t level)
   this->unlock();
 }
 
-// HeadClientAlgorithm Public API
+// TiagoHeadClientAlgorithm Public API

src/head_client_alg_node.cpp → src/tiago_head_client_alg_node.cpp

-#include "head_client_alg_node.h"
-#include <tiago_modules/head_module.h>
+#include "tiago_head_client_alg_node.h"
+#include <tiago_head_module/tiago_head_module.h>
 
-HeadClientAlgNode::HeadClientAlgNode(void) :
-  algorithm_base::IriBaseAlgorithm<HeadClientAlgorithm>(),
+TiagoHeadClientAlgNode::TiagoHeadClientAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<TiagoHeadClientAlgorithm>(),
   head("head_module",ros::this_node::getName())
 {
   //init class attributes if necessary
@@ -21,12 +21,12 @@ HeadClientAlgNode::HeadClientAlgNode(void) :
   // [init head clients]
 }
 
-HeadClientAlgNode::~HeadClientAlgNode(void)
+TiagoHeadClientAlgNode::~TiagoHeadClientAlgNode(void)
 {
   // [free dynamic memory]
 }
 
-void HeadClientAlgNode::mainNodeThread(void)
+void TiagoHeadClientAlgNode::mainNodeThread(void)
 {
   // [fill msg structures]
   
@@ -43,7 +43,7 @@ void HeadClientAlgNode::mainNodeThread(void)
 
 /*  [head callbacks] */
 
-void HeadClientAlgNode::node_config_update(Config &config, uint32_t level)
+void TiagoHeadClientAlgNode::node_config_update(Config &config, uint32_t level)
 {
   static std::vector<double> pan_angles,tilt_angles;
   static std::vector<double> durations;
@@ -86,12 +86,12 @@ void HeadClientAlgNode::node_config_update(Config &config, uint32_t level)
   this->alg_.unlock();
 }
 
-void HeadClientAlgNode::addNodeDiagnostics(void)
+void TiagoHeadClientAlgNode::addNodeDiagnostics(void)
 {
 }
 
 /* main function */
 int main(int argc,char *argv[])
 {
-  return algorithm_base::main<HeadClientAlgNode>(argc, argv, "head_client_alg_node");
+  return algorithm_base::main<TiagoHeadClientAlgNode>(argc, argv, "tiago_head_client_alg_node");
 }

src/tiago_head_module.cpp

@@ -22,7 +22,7 @@ CTiagoHeadModule::CTiagoHeadModule(const std::string &name,const std::string &na
 
   /* point attributes */
   this->new_point=false;
-  this->point_goal.pointing_frame="/xtion_rgb_optical_frame";
+  this->point_goal.pointing_frame=this->config.point_frame_id;
   this->point_goal.pointing_axis.x=0.0;
   this->point_goal.pointing_axis.y=0.0;
   this->point_goal.pointing_axis.z=1.0;
@@ -185,6 +185,7 @@ void CTiagoHeadModule::reconfigure_callback(tiago_head_module::TiagoHeadModuleCo
   ROS_INFO("CTiagoHeadModule : reconfigure callback");
   this->lock();
   this->config=config;
+  this->point_goal.pointing_frame=this->config.point_frame_id;
   /* set the module rate */
   this->dynamic_reconfigure(config,"head_module");
   /* head move action parameters */