Merge branch 'kinetic_migration'

CMakeLists.txt

 cmake_minimum_required(VERSION 2.8.3)
 project(iri_base_algorithm)
 
+# set(CMAKE_BUILD_TYPE "DEBUG")
+
+## Add support for C++11, supported in ROS Kinetic and newer
+# 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)
+find_package(catkin REQUIRED COMPONENTS
+	roscpp
+	dynamic_reconfigure
+	diagnostic_updater
+)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED COMPONENTS system thread)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -76,9 +86,10 @@ find_package(catkin REQUIRED)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include
-#  LIBRARIES ${PROJECT_NAME}
+  LIBRARIES ${PROJECT_NAME}
   CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater
 #  DEPENDS system_lib
+  DEPENDS Boost
 )
 
 ###########
@@ -88,6 +99,9 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 # include_directories(include)
+include_directories(
+	${catkin_INCLUDE_DIRS}
+)
 
 ## Declare a cpp library
 # add_library(foo

README.md

+# Description
+This project is intended to be used as a base class for ROS nodes that have to implement an algorithm. The next diagram shows the basic structure of the IRI ROS base algorithm node:
+
+<img src="doc/images/iri_base_algorithm_diagram.png" alt="Basic structure of the IRI ROS base algorithm node" width="998" height="630">
+
+The structure of this node is mainly the same as the [iri_base_driver](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_base_driver) nodes, with the exception of the state machine used to handle the operation of the low level driver.
+
+This node is structured into two parts, both implemented as C++ classes:
+
+## Algorithm
+This part is intended for the implementation of the algorithm itself. The use of an external library to implement the algorithm is strongly recommended to separate the algorithm functionality from the ROS middleware. However it is also possible to implement the algorithm here. 
+
+The algorithm part provides a mutex to protect the access to critical information. This mutex can be used internally by the algorithm functions and also by the algorithm node through a set of public functions of the algorithm class (lock(), unlock() and try_enter()). Make sure the mutex is left unlocked after use to avoid blocking the normal operation of the node.
+
+## Algorithm node
+This part is intended for the implementation of the ROS interfaces (topics, services and actions) and other ROS tools. It has an object of the algorithm class to have full access to the algorithm functionalities. The main features of this part are listed next, and a detailed description can be found below.
+
+* Independent thread with configurable rate.
+* Dynamic reconfigure server.
+* Diagnostics publisher.
+* Public and private ROS node handlers.
+
+<details><summary>Detailed features description</summary>
+<p>
+
+### Threads
+The algorithm node provides three separate threads:
+
+* The spin thread is used to call the ROS spin() function. The single threaded version of this function is used, so that all callbacks are executed, sequentially, in the same thread.
+* The user thread is used to provide the ability to execute code in parallel and avoid blocking the callback functions. This thread is handled internally when the class is created and destroyed and a single function is provided to the user (mainNodeThread) which is called at a desired rate. The thread rate can be handled with the *setRate()* and *getRate()* functions. 
+* The main thread is used to publish the node diagnostics data.
+
+### Dynamic reconfigure
+A dynamic reconfigure server is initialized by default. It has an associated .cfg file located in the *cfg* folder where the user can define all the parameters that can be updated at run-time. Whenever a parameter value is changed (using the [rqt_reconfigure](http://wiki.ros.org/rqt_reconfigure)) application) two callback functions are called:
+
+* reconfigure callback at the algorithm part (config_update).
+* reconfigure callback at the algorithm node part (node_config_update)
+
+Each callback receives the whole dynamic reconfigure data structure, but should only use its associated parameters.
+
+All node parameters can be defined this way, but it is recommended to only use it for run-time dynamic parameters.
+
+### Diagnostic publisher
+A diagnostic publisher is also initialized by default. It provides the ability to collect status data from the algorithm and the algorithm node, and publishes it. The [rqt_runtime_monitor](http://wiki.ros.org/rqt_runtime_monitor) application can be used to monitor the diagnostics.
+
+By default no diagnostic data is published. Diagnostics can be added at star-up by the user with the addNodeDiagnostics() function which can be implemented in the inherited class. For further information on ROS diagnostics feature go [here](http://wiki.ros.org/diagnostics).
+
+### ROS node Handlers
+The algorithm node provides two separate ROS NodeHandlers:
+
+* the public node handle is initialized by a node handle provided to the constructor of the algorithm node. If no node handle is defined, "/" will be used.
+* the private node handle is initialized to "~" by default.
+
+These node handlers can be used to get/set ROS parameters and to set-up ROS interfaces. Depending on the node handle used, the namespace of the corresponding parameter or interface will be different.
+
+</p>
+</details>
+
+# ROS Interface
+
+### Service servers
+- *~/set_parameters* ([dynamic_reconfigure/Reconfigure.srv](http://docs.ros.org/melodic/api/dynamic_reconfigure/html/srv/Reconfigure.html)):
+    Service to dynamically change the parameters of the node. Only the parameters defined in the .cfg file can be modified this way. 
+
+### topic publishers
+- */diagnostics* ([diagnostic_msgs/DiagnosticArray](http://docs.ros.org/api/diagnostic_msgs/html/msg/DiagnosticArray.html)):
+   Topic with all the diagnostics published by the node.
+
+### Parameters
+- **rate** *(double; Default: 10.0; Max: 1000.0; Min: 0.1)*:
+    The rate of execution of the user thread in Hz.
+
+# Dependencies
+This base node has no dependencies.
+
+# How to use it
+
+This ROS node is not designed to be used on its own, an inherited class is always required. 
+
+To simplify the process of inheriting from this class, there is an script (*create_algorithm_package*) which creates a ROS algorithm package using this node as a base class. 
+
+Also, there exist other scripts to easily add ROS interfaces (topic publishers or subscribers, service servers or clients and action servers or clients) and also TF listener or broadcaster.
+
+Check the [IRI ROS scripts](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_ros_scripts) page for further information on all these scripts.
+
+## Disclaimer
+
+Copyright (C) Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+Mantainer IRI labrobotics (labrobotica@iri.upc.edu)
+
+This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair  or correction.
+
+In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
+
+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/>
+

include/iri_base_algorithm/iri_base_algorithm.h

@@ -35,21 +35,6 @@
 namespace algorithm_base
 {
 
-/**
- * \brief Abstract Class for Ctrl+C management
- *
- */
-class AbstractAlgorithmNode
-{
-  protected:
-    
-    static void hupCalled(int sig)
-    {
-      ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
-    }
-    
-};
-
 /**
  * \brief IRI ROS Algorithm Base Node Class
  *
@@ -75,7 +60,7 @@ class AbstractAlgorithmNode
  * mainNodeThread() function defined in the inherit node class is called.
  */
 template <class Algorithm>
-class IriBaseAlgorithm : public AbstractAlgorithmNode
+class IriBaseAlgorithm
 {
   public:
    /**
@@ -125,16 +110,6 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
     */
     ros::NodeHandle private_node_handle_;
 
-   /**
-    * \brief main thread loop rate
-    * 
-    * This value determines the loop frequency of the node main thread function
-    * mainThread() in HZ. It is initialized at construction time. This variable 
-    * may be modified in the node implementation constructor if a desired
-    * frequency is required.
-    */
-    ros::Rate loop_rate_;
-
    /**
     * \brief default main thread frequency
     * 
@@ -159,7 +134,7 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
     * loop_rate_ and the diagnostic updater. It also instantiates the main 
     * thread of the class and the dynamic reconfigure callback.
     */
-    IriBaseAlgorithm(void);
+    IriBaseAlgorithm(const ros::NodeHandle &nh = ros::NodeHandle("~"));
 
    /**
     * \brief destructor
@@ -194,7 +169,31 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
     */
     dynamic_reconfigure::Server<Config> dsrv_;
 
+   /**
+    * \brief main thread loop rate
+    * 
+    * This value determines the loop frequency of the node main thread function
+    * mainThread() in HZ. It is initialized at construction time. This variable 
+    * may be modified in the node implementation constructor if a desired
+    * frequency is required.
+    */
+    ros::Rate loop_rate_;
+
   protected:
+
+    /**
+    * \brief 
+    * 
+    */
+    void setRate(double rate_hz);
+
+
+    /**
+    * \brief 
+    * 
+    */
+    double getRate(void);
+
    /**
     * \brief dynamic reconfigure server callback
     * 
@@ -239,13 +238,6 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
     */
     virtual void addNodeDiagnostics(void) = 0;
 
-//     void addOpenedTests(void);
-//     void addStoppedTests(void);
-//     void addRunningTests(void);
-//     virtual void addNodeOpenedTests(void) = 0;
-//     virtual void addNodeStoppedTests(void) = 0;
-//     virtual void addNodeRunningTests(void) = 0;
-
    /**
     * \brief main node thread
     *
@@ -268,22 +260,23 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
     * have to be detailed.
     */
     virtual void mainNodeThread(void) = 0;
+
+    static void hupCalled(int sig);
 };
 
 
 template <class Algorithm>
-IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() : 
-  public_node_handle_(ros::this_node::getName()),
+IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(const ros::NodeHandle &nh) : 
+  public_node_handle_(nh),
   private_node_handle_("~"), 
   loop_rate_(DEFAULT_RATE),
   diagnostic_(),
-  dsrv_(private_node_handle_)
+  dsrv_(public_node_handle_)
 {
   ROS_DEBUG("IriBaseAlgorithm::Constructor");
 
-//   // assign callback to dynamic reconfigure server
-//   dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
-
+  // allow Ctrl+C management
+  signal(SIGHUP, &IriBaseAlgorithm<Algorithm>::hupCalled);
 }
 
 template <class Algorithm>
@@ -294,6 +287,18 @@ IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
   pthread_join(this->thread,NULL);
 }
 
+template <class Algorithm>
+void IriBaseAlgorithm<Algorithm>::setRate(double rate_hz)
+{
+  this->loop_rate_=ros::Rate(rate_hz);
+}
+
+template <class Algorithm>
+double IriBaseAlgorithm<Algorithm>::getRate(void)
+{
+  return 1.0/this->loop_rate_.expectedCycleTime().toSec();
+}
+
 template <class Algorithm>
 void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level)
 {
@@ -330,6 +335,12 @@ void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
   pthread_exit(NULL);
 }
 
+template <class Algorithm>
+void IriBaseAlgorithm<Algorithm>::hupCalled(int sig)
+{
+  ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
+}
+
 template <class Algorithm>
 int IriBaseAlgorithm<Algorithm>::spin(void)
 {
@@ -390,9 +401,6 @@ int main(int argc, char **argv, std::string node_name)
   // ROS initialization
   ros::init(argc, argv, node_name);
 
-  // allow Ctrl+C management
-  signal(SIGHUP, &AbstractAlgorithmNode::hupCalled);
-
   // define and launch generic algorithm implementation object
   AlgImplTempl algImpl;
   algImpl.spin();

package.xml

 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>iri_base_algorithm</name>
   <version>1.0.0</version>
   <description>The iri_base_algorithm package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="labrobotica@iri.upc.edu">labrobotica</maintainer>
 
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
   <license>LGPL</license>
 
-
-  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
   <url type="website">http://wiki.ros.org/iri_base_algorithm</url>
 
-
-  <!-- Author tags are optional, mutiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintianers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
   <author>Joan Perez</author>
 
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <build_depend>roscpp</build_depend>
-  <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>diagnostic_updater</build_depend>
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use run_depend for packages you need at runtime: -->
-  <run_depend>roscpp</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>diagnostic_updater</run_depend>
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>diagnostic_updater</depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
 </package>