Merge remote-tracking branch 'client/master'

CMakeLists.txt

@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -87,6 +87,7 @@ find_package(iriutils REQUIRED)
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/TiagoPlayMotionModule.cfg
+  cfg/TiagoPlayMotionClient.cfg
 )
 
 ###################################
@@ -100,8 +101,8 @@ generate_dynamic_reconfigure_options(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES play_motion_module
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs
+ LIBRARIES tiago_play_motion_module
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm
 
 #  DEPENDS system_lib
 )
@@ -136,21 +137,15 @@ add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS})
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/humanoid_modules_node.cpp)
+set(client_name tiago_play_motion_client)
+add_executable(${client_name} src/tiago_play_motion_client_alg.cpp src/tiago_play_motion_client_alg_node.cpp)
 
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+target_link_libraries(${client_name} ${catkin_LIBRARIES})
+target_link_libraries(${client_name} ${iriutils_LIBRARY})
+target_link_libraries(${client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
 
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
+add_dependencies(${client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name})
+add_dependencies(${client_name} ${tiago_play_motion_client_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 #############
 ## Install ##

cfg/TiagoPlayMotionClient.cfg

+#! /usr/bin/env python
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of the Willow Garage nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='tiago_play_motion_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       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("motion_name",                  str_t,     0,                          "Motion name to execute",         "")
+gen.add("start_motion",                 bool_t,    0,                          "Execute selected motion",        False)
+gen.add("cancel_motion",                bool_t,    0,                          "Cancel current motion",          False)
+
+exit(gen.generate(PACKAGE, "TiagoPlayMotionClient", "TiagoPlayMotionClient"))

include/tiago_play_motion_client_alg.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _play_motion_client_alg_h_
+#define _play_motion_client_alg_h_
+
+#include <tiago_play_motion_module/TiagoPlayMotionClientConfig.h>
+
+//include humanoid_common_play_motion_client_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class TiagoPlayMotionClientAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the TiagoPlayMotionClientConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    pthread_mutex_t access_;    
+
+    // private attributes and methods
+
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the TiagoPlayMotionClientConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef tiago_play_motion_module::TiagoPlayMotionClientConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    TiagoPlayMotionClientAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { pthread_mutex_lock(&this->access_); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    * 
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void) 
+    { 
+      if(pthread_mutex_trylock(&this->access_)==0)
+        return true;
+      else
+        return false;
+    };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& new_cfg, uint32_t level=0);
+
+    // here define all play_motion_client_alg interface methods to retrieve and set
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~TiagoPlayMotionClientAlgorithm(void);
+};
+
+#endif

include/tiago_play_motion_client_alg_node.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _play_motion_client_alg_node_h_
+#define _play_motion_client_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "tiago_play_motion_client_alg.h"
+
+// [publisher subscriber headers]
+
+// [service client headers]
+
+// [play_motion server client headers]
+
+#include <tiago_play_motion_module/tiago_play_motion_module.h>
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class TiagoPlayMotionClientAlgNode : public algorithm_base::IriBaseAlgorithm<TiagoPlayMotionClientAlgorithm>
+{
+  private:
+    // [publisher attributes]
+
+    // [subscriber attributes]
+
+    // [service attributes]
+
+    // [client attributes]
+
+    // [play_motion server attributes]
+
+    // [play_motion client attributes]
+
+    CTiagoPlayMotionModule play_motion;
+
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    TiagoPlayMotionClientAlgNode(void);
+
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~TiagoPlayMotionClientAlgNode(void);
+
+  protected:
+   /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the algorithm is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
+
+   /**
+    * \brief dynamic reconfigure server callback
+    * 
+    * This method is called whenever a new configuration is received through
+    * the dynamic reconfigure. The derivated generic algorithm class must 
+    * implement it.
+    *
+    * \param config an object with new configuration from all algorithm 
+    *               parameters defined in the config file.
+    * \param level  integer referring the level in which the configuration
+    *               has been changed.
+    */
+    void node_config_update(Config &config, uint32_t level);
+
+   /**
+    * \brief node add diagnostics
+    *
+    * In this abstract function additional ROS diagnostics applied to the 
+    * specific algorithms may be added.
+    */
+    void addNodeDiagnostics(void);
+
+    // [diagnostic functions]
+    
+    // [test functions]
+};
+
+#endif

launch/play_motion_client.launch

+<launch>
+
+  <!-- launch the play motion client node -->
+  <node name="play_motion_client"
+        pkg="play_motion_client"
+        type="play_motion_client"
+        output="screen"
+        ns="/tiago">
+    <remap from="~/play_motion_module/play_motion"
+             to="/play_motion"/>
+
+    <rosparam file="$(find tiago_modules)/config/play_motion_module_default.yaml" command="load" ns="play_motion_module" />
+  </node>
+
+  <!-- launch dynamic reconfigure -->
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+    output="screen"/>
+
+</launch>
+

launch/play_motion_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 play_motion_client)/launch/play_motion_client.launch"/>
+
+</launch>
+

package.xml

@@ -45,6 +45,7 @@
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>play_motion_msgs</build_depend>
+  <build_depend>iri_base_algorithm</build_depend>
   <!-- new dependencies -->
   <!--<build_depend>new build dependency</build_depend>-->
   <run_depend>iri_ros_tools</run_depend>
@@ -52,6 +53,7 @@
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>actionlib</run_depend>
   <run_depend>play_motion_msgs</run_depend>
+  <run_depend>iri_base_algorithm</run_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->
 

src/tiago_play_motion_client_alg.cpp

+#include "tiago_play_motion_client_alg.h"
+
+TiagoPlayMotionClientAlgorithm::TiagoPlayMotionClientAlgorithm(void)
+{
+  pthread_mutex_init(&this->access_,NULL);
+}
+
+TiagoPlayMotionClientAlgorithm::~TiagoPlayMotionClientAlgorithm(void)
+{
+  pthread_mutex_destroy(&this->access_);
+}
+
+void TiagoPlayMotionClientAlgorithm::config_update(Config& new_cfg, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=new_cfg;
+  
+  this->unlock();
+}
+
+// TiagoPlayMotionClientAlgorithm Public API

src/tiago_play_motion_client_alg_node.cpp

+#include "tiago_play_motion_client_alg_node.h"
+#include <tiago_play_motion_module/tiago_play_motion_module.h>
+
+TiagoPlayMotionClientAlgNode::TiagoPlayMotionClientAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<TiagoPlayMotionClientAlgorithm>(),
+  play_motion("play_motion_module",ros::this_node::getName())
+{
+  //init class attributes if necessary
+  //this->loop_rate_ = 2;//in [Hz]
+
+  // [init publishers]
+  
+  // [init subscribers]
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init play_motion servers]
+  
+  // [init play_motion clients]
+}
+
+TiagoPlayMotionClientAlgNode::~TiagoPlayMotionClientAlgNode(void)
+{
+  // [free dynamic memory]
+}
+
+void TiagoPlayMotionClientAlgNode::mainNodeThread(void)
+{
+  // [fill msg structures]
+  
+  // [fill srv structure and make request to the server]
+
+  // [fill play_motion structure and make request to the play_motion server]
+
+  // [publish messages]
+}
+
+/*  [subscriber callbacks] */
+
+/*  [service callbacks] */
+
+/*  [play_motion callbacks] */
+
+void TiagoPlayMotionClientAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  this->alg_.lock();
+  if(config.start_motion)
+  {
+    if(!this->play_motion.is_finished())
+      ROS_WARN("Impossible to execute play_motion because the previous play_motion is not finished yet");
+    else
+      this->play_motion.execute_motion(config.motion_name);
+    config.start_motion=false;
+  }
+  else if(config.cancel_motion)
+  {
+    if(!this->play_motion.is_finished())
+      this->play_motion.stop();
+    config.cancel_motion=false;
+  }
+  this->alg_.unlock();
+}
+
+void TiagoPlayMotionClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<TiagoPlayMotionClientAlgNode>(argc, argv, "play_motion_client_alg_node");
+}

src/tiago_play_motion_module.cpp

@@ -111,7 +111,7 @@ void CTiagoPlayMotionModule::execute_motion(std::string motion_name)
 {
   XmlRpc::XmlRpcValue symbols;
 
-  if(!ros::param::get(this->config_.motions_param_path.c_str(), symbols))
+  if(!ros::param::get(this->config.motions_param_path.c_str(), symbols))
     return;
   else
   {
@@ -119,7 +119,7 @@ void CTiagoPlayMotionModule::execute_motion(std::string motion_name)
       return;
     for (XmlRpc::XmlRpcValue::iterator i=symbols.begin(); i!=symbols.end(); ++i) 
     {
-      std::cout << i->second[i] << std::endl;
+      std::cout << i->second << std::endl;
     }
   }
 /*