diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6bf45555565502327947e332815361348d5cadc2..43cb1e78abb520d8f1b0e814ba564de356d390df 100644
--- a/CMakeLists.txt
+++ b/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 std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs iri_base_algorithm)
find_package(pal_waypoint_msgs)
if(${pal_waypoint_msgs_FOUND})
@@ -91,6 +91,7 @@ find_package(iriutils REQUIRED)
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/TiagoNavModule.cfg
+ cfg/TiagoNavClient.cfg
)
###################################
@@ -105,7 +106,7 @@ generate_dynamic_reconfigure_options(
catkin_package(
INCLUDE_DIRS include
LIBRARIES tiago_nav_module
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs iri_base_algorithm
# DEPENDS system_lib
)
@@ -136,24 +137,15 @@ target_link_libraries(${module_name} ${iriutils_LIBRARY})
add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS})
## Declare a C++ executable
-## 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)
-
-## 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 "")
-
-## 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}
-# )
+set(client_name tiago_nav_client)
+add_executable(${client_name} src/tiago_nav_client_alg.cpp src/tiago_nav_client_alg_node.cpp)
+
+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_dependencies(${client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name})
+add_dependencies(${client_name} ${tiago_nav_client_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
#############
## Install ##
diff --git a/cfg/TiagoNavClient.cfg b/cfg/TiagoNavClient.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..b5fc86e763479a7005d371e3bdb13f23680660b1
--- /dev/null
+++ b/cfg/TiagoNavClient.cfg
@@ -0,0 +1,79 @@
+#! /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_nav_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+move_base = gen.add_group("Move base action")
+costmaps = gen.add_group("Costmap")
+map = gen.add_group("Map")
+poi = gen.add_group("Got to point of interest action")
+waypoint = gen.add_group("Got to waypoints 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)
+move_base.add("mb_x_pos", double_t, 0, "Target X position", 0.0, -100.0, 100.0)
+move_base.add("mb_y_pos", double_t, 0, "Target Y position", 0.0, -100.0, 100.0)
+move_base.add("mb_xy_tol", double_t, 0, "Target XY tolerance", -1.0, -1.0, 10.0)
+move_base.add("mb_yaw", double_t, 0, "Target Yaw angle", 0.0, -3.14159,3.14159)
+move_base.add("mb_yaw_tol", double_t, 0, "Target Yaw tolerance", -1.0, -1.0, 1.0)
+move_base.add("mb_frame_id", str_t, 0, "Target pose frame identifier", "/base_link")
+
+map.add("map_name", str_t, 0, "Name of the map to use", "iri_map")
+map.add("map_change", bool_t, 0, "Update the desired map", False)
+
+costmaps.add("cm_enable_auto_clear",bool_t, 0, "Enable costmaps autoclear", False)
+costmaps.add("cm_disable_auto_clear",bool_t, 0, "Disable costmaps autoclear", False)
+costmaps.add("cm_auto_clear_rate_hz",double_t,0, "Costmap autoclear rate", 0.1, 0.01, 1.0)
+costmaps.add("cm_clear_costmaps", bool_t, 0, "Clear costmaps", False)
+
+move_base.add("start_pose_nav", bool_t, 0, "Start navigating towards a pose", False)
+move_base.add("start_position_nav",bool_t, 0, "Start navigating towards a position", False)
+move_base.add("start_orientation_nav",bool_t, 0, "Start navigating towards a orientation", False)
+move_base.add("stop_nav", bool_t, 0, "Stop the current navigation", False)
+
+poi.add("start_poi", bool_t, 0, "Start the POI navigation", False)
+poi.add("stop_poi", bool_t, 0, "Stop the POI navigation", False)
+poi.add("poi_name", str_t, 0, "Name of the POI", "")
+
+waypoint.add("start_wp", bool_t, 0, "Start the waypoint navigation", False)
+waypoint.add("stop_wp", bool_t, 0, "Stop the waypoint navigation", False)
+waypoint.add("wp_group", str_t, 0, "Name of the waypoint group", "")
+waypoint.add("wp_first", int_t, 0, "Index of the first waypoint", 0, 0, 100)
+waypoint.add("wp_num", int_t, 0, "Number of waypoint to execute", 0, 0, 100)
+waypoint.add("wp_continue_on_error",bool_t, 0, "Continue on error", False)
+
+exit(gen.generate(PACKAGE, "TiagoNavClient", "TiagoNavClient"))
diff --git a/include/tiago_nav_client_alg.h b/include/tiago_nav_client_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..90769b1449ad1125a583af7e5a6154742eec5bec
--- /dev/null
+++ b/include/tiago_nav_client_alg.h
@@ -0,0 +1,131 @@
+// 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 _tiago_nav_client_alg_h_
+#define _tiago_nav_client_alg_h_
+
+#include <tiago_nav_module/TiagoNavClientConfig.h>
+
+//include humanoid_common_tiago_nav_client_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class TiagoNavClientAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the TiagoNavClientConfig. 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 TiagoNavClientConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef tiago_nav_module::TiagoNavClientConfig 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.
+ */
+ TiagoNavClientAlgorithm(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 tiago_nav_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.
+ *
+ */
+ ~TiagoNavClientAlgorithm(void);
+};
+
+#endif
diff --git a/include/tiago_nav_client_alg_node.h b/include/tiago_nav_client_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..86cf5cd6f2d3bfbd589e7483453a82d962e169af
--- /dev/null
+++ b/include/tiago_nav_client_alg_node.h
@@ -0,0 +1,119 @@
+// 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 _tiago_nav_client_alg_node_h_
+#define _tiago_nav_client_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "tiago_nav_client_alg.h"
+
+// [publisher subscriber headers]
+
+// [service client headers]
+
+// [nav server client headers]
+
+#include <tiago_nav_module/tiago_nav_module.h>
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class TiagoNavClientAlgNode : public algorithm_base::IriBaseAlgorithm<TiagoNavClientAlgorithm>
+{
+ private:
+ // [publisher attributes]
+
+ // [subscriber attributes]
+
+ // [service attributes]
+
+ // [client attributes]
+
+ // [nav server attributes]
+
+ // [nav client attributes]
+
+ CTiagoNavModule nav;
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ TiagoNavClientAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~TiagoNavClientAlgNode(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
diff --git a/launch/nav_client.launch b/launch/nav_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..eb074c12fb423ce523867d4a2b35079fb841d4c8
--- /dev/null
+++ b/launch/nav_client.launch
@@ -0,0 +1,34 @@
+<launch>
+
+ <!-- launch the play motion client node -->
+ <node name="nav_client"
+ pkg="nav_client"
+ type="nav_client"
+ output="screen"
+ ns="/tiago">
+ <remap from="~/nav_module/move_base"
+ to="/move_base"/>
+ <remap from="~/nav_module/move_poi"
+ to="/poi_navigation_server/go_to_poi"/>
+ <remap from="~/nav_module/move_waypoint"
+ to="/pal_waypoint/navigate"/>
+ <remap from="~/nav_module/odom"
+ to="/mobile_base_controller/odom"/>
+ <remap from="~/nav_module/clear_costmaps"
+ to="/move_base/clear_costmaps"/>
+ <remap from="~/nav_module/change_map"
+ to="/pal_map_manager/change_map"/>
+ <remap from="~/nav_module/set_op_mode"
+ to="/pal_navigation_sm"/>
+ <remap from="~/nav_module/move_base_reconf"
+ to="/move_base/PalLocalPlanner/set_parameters"/>
+
+ <rosparam file="$(find tiago_modules)/config/nav_module_default.yaml" command="load" ns="nav_module" />
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/launch/nav_client_sim.launch b/launch/nav_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d2b7c7486e345191a60fbaede74dac13ac33bfd5
--- /dev/null
+++ b/launch/nav_client_sim.launch
@@ -0,0 +1,10 @@
+<launch>
+
+ <include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch"/>
+
+ <include file="$(find tiago_laser_sensors)/launch/rgbd_cloud_laser.launch"/>
+
+ <include file="$(find nav_client)/launch/nav_client.launch"/>
+
+</launch>
+
diff --git a/package.xml b/package.xml
index 23e3df65e89dd8ec915fbf89ec418a65a8e00f0a..b902432d97687fba55446aecb9b6f9e26c66969d 100644
--- a/package.xml
+++ b/package.xml
@@ -50,6 +50,7 @@
<build_depend>pal_navigation_msgs</build_depend>
<build_depend>pal_waypoint_msgs</build_depend>
<build_depend>tf</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>
@@ -62,6 +63,7 @@
<run_depend>pal_navigation_msgs</run_depend>
<run_depend>pal_waypoint_msgs</run_depend>
<run_depend>tf</run_depend>
+ <run_depend>iri_base_algorithm</run_depend>
<!-- new dependencies -->
<!--<run_depend>new run dependency</run_depend>-->
diff --git a/src/tiago_nav_client_alg.cpp b/src/tiago_nav_client_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50bd60bea87af3c1cb7e99f8d5c32f5ecbee966c
--- /dev/null
+++ b/src/tiago_nav_client_alg.cpp
@@ -0,0 +1,23 @@
+#include "tiago_nav_client_alg.h"
+
+TiagoNavClientAlgorithm::TiagoNavClientAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+TiagoNavClientAlgorithm::~TiagoNavClientAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void TiagoNavClientAlgorithm::config_update(Config& new_cfg, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=new_cfg;
+
+ this->unlock();
+}
+
+// TiagoNavClientAlgorithm Public API
diff --git a/src/tiago_nav_client_alg_node.cpp b/src/tiago_nav_client_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..722503043ded76de2b465c8761410b303573add4
--- /dev/null
+++ b/src/tiago_nav_client_alg_node.cpp
@@ -0,0 +1,125 @@
+#include "tiago_nav_client_alg_node.h"
+#include <tiago_nav_module/tiago_nav_module.h>
+
+TiagoNavClientAlgNode::TiagoNavClientAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<TiagoNavClientAlgorithm>(),
+ nav("nav_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 nav servers]
+
+ // [init nav clients]
+}
+
+TiagoNavClientAlgNode::~TiagoNavClientAlgNode(void)
+{
+ // [free dynamic memory]
+}
+
+void TiagoNavClientAlgNode::mainNodeThread(void)
+{
+ // [fill msg structures]
+
+ // [fill srv structure and make request to the server]
+
+ // [fill nav structure and make request to the nav server]
+
+ // [publish messages]
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [nav callbacks] */
+
+void TiagoNavClientAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+ if(config.start_pose_nav)
+ {
+ this->nav.set_goal_frame(config.mb_frame_id);
+ this->nav.go_to_pose(config.mb_x_pos,config.mb_y_pos,config.mb_yaw,config.mb_xy_tol,config.mb_yaw_tol);
+ config.start_pose_nav=false;
+ }
+ else if(config.start_position_nav)
+ {
+ this->nav.set_goal_frame(config.mb_frame_id);
+ this->nav.go_to_position(config.mb_x_pos,config.mb_y_pos,config.mb_xy_tol);
+ config.start_position_nav=false;
+ }
+ else if(config.start_orientation_nav)
+ {
+ this->nav.set_goal_frame(config.mb_frame_id);
+ this->nav.go_to_orientation(config.mb_yaw,config.mb_yaw_tol);
+ config.start_orientation_nav=false;
+ }
+ else if(config.stop_nav)
+ {
+ this->nav.stop();
+ config.stop_nav=false;
+ }
+ if(config.cm_clear_costmaps)
+ {
+ this->nav.costmaps_clear();
+ config.cm_clear_costmaps=false;
+ }
+ else if(config.cm_enable_auto_clear)
+ {
+ this->nav.costmaps_enable_auto_clear(config.cm_auto_clear_rate_hz);
+ config.cm_enable_auto_clear=false;
+ }
+ else if(config.cm_disable_auto_clear)
+ {
+ this->nav.costmaps_disable_auto_clear();
+ config.cm_disable_auto_clear=false;
+ }
+ else if(config.map_change)
+ {
+ this->nav.map_change(config.map_name);
+ config.map_change=false;
+ }
+ else if(config.start_poi)
+ {
+ this->nav.go_to_poi(config.poi_name);
+ config.start_poi=false;
+ }
+ else if(config.stop_poi)
+ {
+ this->nav.stop();
+ config.stop_poi=false;
+ }
+ else if(config.start_wp)
+ {
+#ifdef HAVE_WAYPOINTS
+ this->nav.go_to_waypoints(config.wp_group,config.wp_first,config.wp_num,config.wp_continue_on_error);
+#endif
+ config.start_wp=false;
+ }
+ else if(config.stop_wp)
+ {
+ this->nav.stop();
+ config.stop_wp=false;
+ }
+ this->alg_.unlock();
+}
+
+void TiagoNavClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<TiagoNavClientAlgNode>(argc, argv, "tiago_nav_client_alg_node");
+}