Initial commit

launch/tiago_nav_client.launch

+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="node_name" default="nav_client" />
+  <arg name="output" default="screen" />
+  <arg name="launch_prefix" default="" />
+  <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
+  <arg name="move_base_action" default="/move_base"/>
+  <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
+  <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
+  <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
+  <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
+  <arg name="change_map_service" default="/pal_map_manager/change_map"/>
+  <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
+  <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
+
+  <!-- launch the play motion client node -->
+  <node name="$(arg node_name)"
+        pkg="tiago_nav_module"
+        type="tiago_nav_client"
+        output="$(arg output)"
+        launch-prefix="$(arg launch_prefix)">
+    <remap from="~/nav_module/move_base"
+             to="$(arg move_base_action)"/>
+    <remap from="~/nav_module/move_poi"
+             to="$(arg move_poi_action)"/>
+    <remap from="~/nav_module/move_waypoint"
+             to="$(arg move_waypoint_action)"/>
+    <remap from="~/nav_module/odom"
+             to="$(arg odom_topic)"/>
+    <remap from="~/nav_module/path"
+             to="$(arg path_topic)"/>
+    <remap from="~/nav_module/clear_costmaps"
+             to="$(arg clear_costmaps_service)"/>
+    <remap from="~/nav_module/change_map"
+             to="$(arg change_map_service)"/>
+    <remap from="~/nav_module/set_op_mode"
+             to="$(arg set_op_mode_service)"/>
+    <remap from="~/nav_module/move_base_reconf"
+             to="$(arg move_base_dyn_reconf)"/>
+
+    <rosparam file="$(arg config_file)" command="load" ns="nav_module" />
+  </node>
+
+  <!-- launch dynamic reconfigure -->
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+    output="screen"/>
+
+</launch>
+

launch/tiago_nav_client_sim.launch

+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="node_name" default="nav_client" />
+  <arg name="output" default="screen" />
+  <arg name="launch_prefix" default="" />
+  <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
+  <arg name="move_base_action" default="/move_base"/>
+  <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
+  <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
+  <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
+  <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
+  <arg name="change_map_service" default="/pal_map_manager/change_map"/>
+  <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
+  <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
+
+  <include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
+    <arg name="public_sim" value="true" />
+    <arg name="robot" value="steel" />
+  </include>
+
+  <include file="$(find tiago_nav_module)/launch/tiago_nav_client.launch">
+    <arg name="node_name"              value="$(arg node_name)" />
+    <arg name="output"                 value="$(arg output)" />
+    <arg name="launch_prefix"          value="$(arg launch_prefix)" />
+    <arg name="config_file"            value="$(arg config_file)" />
+    <arg name="move_base_action"       value="$(arg move_base_action)"/>
+    <arg name="move_poi_action"        value="$(arg move_poi_action)"/>
+    <arg name="move_waypoint_action"   value="$(arg move_waypoint_action)"/>
+    <arg name="odom_topic"             value="$(arg odom_topic)"/>
+    <arg name="path_topic"             value="$(arg path_topic)"/>
+    <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
+    <arg name="change_map_service"     value="$(arg change_map_service)"/>
+    <arg name="set_op_mode_service"    value="$(arg set_op_mode_service)"/>
+    <arg name="move_base_dyn_reconf"   value="$(arg move_base_dyn_reconf)"/>
+  </include>
+
+</launch>
+

package.xml

+<?xml version="1.0"?>
+<package>
+  <name>adc_nav_module</name>
+  <version>0.0.0</version>
+  <description>Module adc_nav_module 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 multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/humanoid_modules</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <author email="shernand@iri.upc.edu">Sergi Hernandez Juan</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>message_generation</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>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>iri_ros_tools</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>move_base_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>iri_base_bt_client</build_depend>
+  <build_depend>iri_behaviortree</build_depend>
+  <!-- new dependencies -->
+  <!--<build_depend>new build dependency</build_depend>-->
+  <run_depend>iri_ros_tools</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>std_srvs</run_depend>
+  <run_depend>move_base_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>iri_base_bt_client</run_depend>
+  <run_depend>iri_behaviortree</run_depend>
+  <!-- new dependencies -->
+  <!--<run_depend>new run dependency</run_depend>-->
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/adc_nav_bt_client_alg_node.cpp

+#include "adc_nav_bt_client_alg_node.h"
+
+ADCNavModuleBtClientAlgNode::ADCNavModuleBtClientAlgNode(void) :
+  behaviortree_base::IriBaseBTClient<adc_nav_module::ADCNavBtClientConfig>(),
+  adc_nav_module_bt()
+{
+  // [init publishers]
+
+  // [init subscribers]
+
+  // [init services]
+
+  // [init clients]
+
+  // [init action servers]
+
+  // [init action clients]
+
+  this->set_go_to_pose = false;
+  this->adc_nav_module_bt.init(this->factory);
+
+  // Init local functions
+  BT::PortsList async_go_to_pose_inputs_ports = {BT::OutputPort<double>("x"), BT::OutputPort<double>("y"), BT::OutputPort<double>("yaw"), BT::OutputPort<double>("heading_tol"), BT::OutputPort<double>("x_y_pos_tol"), BT::OutputPort<bool>("new_goal")};
+  BT::PortsList set_goal_frame_inputs_ports = {BT::OutputPort<std::string>("frame_id")};
+  
+  this->factory.registerIriAsyncAction("set_adc_nav_go_to_pose_inputs",  std::bind(&ADCNavModuleBtClientAlgNode::set_adc_nav_go_to_pose_inputs, this, std::placeholders::_1), async_go_to_pose_inputs_ports);
+  this->factory.registerIriAsyncAction("set_adc_nav_goal_frame_inputs",  std::bind(&ADCNavModuleBtClientAlgNode::set_adc_nav_goal_frame_inputs, this, std::placeholders::_1), set_goal_frame_inputs_ports);
+
+}
+
+ADCNavModuleBtClientAlgNode::~ADCNavModuleBtClientAlgNode(void)
+{
+  // [free dynamic memory]
+}
+
+/*  [subscriber callbacks] */
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+
+void ADCNavModuleBtClientAlgNode::node_config_update(adc_nav_module::ADCNavBtClientConfig &config, uint32_t level)
+{
+  this->lock();
+
+  if(config.START)
+  {
+    this->core.start_tree();
+    config.START=false;
+  }
+
+  if(config.RESTART)
+  {
+    this->core.stop_tree();
+    config.RESTART=false;
+  }
+
+  if (config.set_go_to_input)
+  {
+    this->set_go_to_pose = true;
+    config.set_go_to_input = false;
+  }
+
+  this->config_=config;
+  this->unlock();
+}
+
+BT::NodeStatus ADCNavModuleBtClientAlgNode::set_adc_nav_go_to_pose_inputs(BT::TreeNode& self)
+{
+  if (this->set_go_to_pose)
+  {
+    this->set_go_to_pose = false;
+    self.setOutput("x", this->config_.mb_x_pos);
+    self.setOutput("y", this->config_.mb_y_pos);
+    self.setOutput("yaw", this->config_.mb_yaw);
+    self.setOutput("heading_tol", this->config_.mb_xy_tol);
+    self.setOutput("x_y_pos_tol", this->config_.mb_yaw_tol);
+    self.setOutput("new_goal", true);
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+BT::NodeStatus ADCNavModuleBtClientAlgNode::set_adc_nav_goal_frame_inputs(BT::TreeNode& self)
+{
+  if (this->set_go_to_pose)
+  {
+    // this->set_go_to_pose = false;
+    self.setOutput("frame_id", this->config_.mb_frame_id);
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+
+void ADCNavModuleBtClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return behaviortree_base::main<ADCNavModuleBtClientAlgNode>(argc, argv, "adc_nav_module_bt_client_alg_node");
+}

src/adc_nav_client_alg.cpp

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

src/adc_nav_client_alg_node.cpp

+#include "adc_nav_client_alg_node.h"
+#include <adc_nav_module/adc_nav_module.h>
+
+ADCNavClientAlgNode::ADCNavClientAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<ADCNavClientAlgorithm>(),
+  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]
+}
+
+ADCNavClientAlgNode::~ADCNavClientAlgNode(void)
+{
+  // [free dynamic memory]
+}
+
+void ADCNavClientAlgNode::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 ADCNavClientAlgNode::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;
+  }
+  if (config.print_goal_distance)
+  {
+    double dist = this->nav.getGoalDistance();
+    ROS_INFO_STREAM("Goal distance = " << dist);
+    config.print_goal_distance = false;
+  }
+  this->alg_.unlock();
+}
+
+void ADCNavClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<ADCNavClientAlgNode>(argc, argv, "adc_nav_client_alg_node");
+}

src/xml/bt_test.xml

+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <SequenceStar>
+            <Action ID="async_is_start_tree"/>
+            <SubTree ID="move_to_pose"/>
+            <SubTree ID="move_to_position"/>
+            <SubTree ID="move_to_orientation"/>
+        </SequenceStar>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="move_to_pose">
+        <SequenceStar>
+            <Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
+            <Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
+            <Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
+            <Action ID="async_tiago_nav_go_to_pose" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
+        </SequenceStar>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="move_to_position">
+        <SequenceStar>
+            <Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
+            <Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
+            <Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
+            <Action ID="async_tiago_nav_go_to_position" x="{x}" y="{y}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
+        </SequenceStar>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="move_to_orientation">
+        <SequenceStar>
+            <Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
+            <Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
+            <Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
+            <Action ID="async_tiago_nav_go_to_orientation" yaw="{yaw}" heading_tol="{heading_tol}" new_goal="{new_goal}"/>
+        </SequenceStar>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <!-- basic_nodes -->
+        <Action ID="NOP"/>
+        <Action ID="is_not_start_tree"/>
+        <Condition ID="async_is_start_tree"/>
+        <Action ID="is_not_stop_tree"/>
+        <Condition ID="async_is_stop_tree"/>
+        <Action ID="set_start_tree"/>
+        <Action ID="set_stop_tree"/>
+        <Action ID="reset_start_tree"/>
+        <Action ID="reset_stop_tree"/>
+        <!-- tiago_nav_module -->
+        <Action ID="sync_cancel_tiago_nav_action"/>
+        <Action ID="async_cancel_tiago_nav_action"/>
+        <Action ID="async_is_tiago_nav_finished"/>
+        <Action ID="sync_tiago_nav_go_to_pose">
+            <input_port name="x"> desired X positon with respect to the goal frame</input_port>
+            <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
+            <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
+        </Action>
+        <Action ID="async_tiago_nav_go_to_pose">
+            <input_port name="x"> desired X positon with respect to the goal frame</input_port>
+            <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
+            <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
+            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+        </Action>
+        <Action ID="sync_tiago_nav_go_to_orientation">
+            <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
+        </Action>
+        <Action ID="async_tiago_nav_go_to_orientation">
+            <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
+        </Action>
+        <Action ID="sync_tiago_nav_go_to_position">
+            <input_port name="x"> desired X positon with respect to the goal frame</input_port>
+            <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
+        </Action>
+        <Action ID="async_tiago_nav_go_to_position">
+            <input_port name="x"> desired X positon with respect to the goal frame</input_port>
+            <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
+        </Action>
+        <Action ID="sync_tiago_nav_go_to_poi">
+            <input_port name="poi"> name of the desired point of interest to move to</input_port>
+        </Action>
+        <Action ID="async_tiago_nav_go_to_poi">
+            <input_port name="poi"> name of the desired point of interest to move to</input_port>
+        </Action>
+        <Action ID="sync_tiago_nav_go_to_waypoints">
+            <input_port name="group"> Name of the desired group of points of interest</input_port>
+            <input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
+            <input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
+            <input_port name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
+        </Action>
+        <Action ID="async_tiago_nav_go_to_waypoints">
+            <input_port name="group"> Name of the desired group of points of interest</input_port>
+            <input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
+            <input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
+            <input_port name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
+        </Action>
+        <Action ID="get_tiago_nav_current_waypoint">
+            <output_port name="waypoint"> The index of the current waypoint being executed</output_port>
+        </Action>
+        <Action ID="set_tiago_nav_goal_frame">
+            <input_port name="frame_id"> The name of the new reference frame for the future goals</input_port>
+        </Action>
+        <Action ID="tiago_nav_map_change">
+            <input_port name="map_name"> name of the desired map</input_port>
+        </Action>
+        <Action ID="tiago_nav_costmaps_clear"/>
+        <Action ID="tiago_nav_costmaps_enable_auto_clear">
+            <input_port name="rate_hz"> the desired clearing rate in Hz. This value should be less than 1 Hz, normally in the range of 0.1 to 0.01 Hz</input_port>
+        </Action>
+        <Action ID="tiago_nav_costmaps_disable_auto_clear"/>
+        <Action ID="tiago_nav_set_bool_parameter">
+            <input_port name="name_space"> String with the parameter name space</input_port>
+            <input_port name="name"> String with the name of the parameter to change</input_port>
+            <input_port name="value"> the new value for the parameter</input_port>
+        </Action>
+        <Action ID="tiago_nav_set_int_parameter">
+            <input_port name="name_space"> String with the parameter name space</input_port>
+            <input_port name="name"> String with the name of the parameter to change</input_port>
+            <input_port name="value"> the new value for the parameter</input_port>
+        </Action>
+        <Action ID="tiago_nav_set_string_parameter">
+            <input_port name="name_space"> String with the parameter name space</input_port>
+            <input_port name="name"> String with the name of the parameter to change</input_port>
+            <input_port name="value"> the new value for the parameter</input_port>
+        </Action>
+        <Action ID="tiago_nav_set_double_parameter">
+            <input_port name="name_space"> String with the parameter name space</input_port>
+            <input_port name="name"> String with the name of the parameter to change</input_port>
+            <input_port name="value"> the new value for the parameter</input_port>
+        </Action>
+        <Action ID="get_tiago_nav_current_pose">
+            <output_port name="pose"> current pose</output_port>
+        </Action>
+        <Action ID="get_tiago_goal_distance">
+            <output_port name="distance"> current goal distance</output_port>
+        </Action>
+        <Condition ID="is_tiago_nav_finished"/>
+        <Condition ID="is_tiago_nav_module_running"/>
+        <Condition ID="is_tiago_nav_module_success"/>
+        <Condition ID="is_tiago_nav_module_action_start_fail"/>
+        <Condition ID="is_tiago_nav_module_timeout"/>
+        <Condition ID="is_tiago_nav_module_fb_watchdog"/>
+        <Condition ID="is_tiago_nav_module_aborted"/>
+        <Condition ID="is_tiago_nav_module_preempted"/>
+        <Condition ID="is_tiago_nav_module_rejected"/>
+        <Condition ID="is_tiago_nav_module_set_param_fail"/>
+        <Condition ID="is_tiago_nav_module_param_not_present"/>
+        <Condition ID="is_tiago_nav_module_no_odom"/>
+        <Condition ID="is_tiago_nav_module_no_transform"/>
+        <Condition ID="is_tiago_nav_module_unknown_map"/>
+        <Condition ID="is_tiago_nav_module_invalid_mode"/>
+        <!-- Client -->
+        <Action ID="set_tiago_nav_go_to_pose_inputs">
+            <output_port name="x"> desired X positon with respect to the goal frame</output_port>
+            <output_port name="y"> desired Y positon with respect to the goal frame</output_port>
+            <output_port name="yaw"> desired orientation with respect to the goal frame</output_port>
+            <output_port name="heading_tol"> heading tolerance for the goal</output_port>
+            <output_port name="x_y_pos_tol"> position tolerance for the goal</output_port>
+            <output_port name="new_goal"> If it's a new_goal</output_port>
+        </Action>
+        <Action ID="set_tiago_nav_goal_frame_inputs">
+            <output_port name="frame_id"> The name of the new reference frame for the future goals</output_port>
+        </Action>
+        <!-- Tree -->
+        <SubTree ID="move_to_pose"/>
+        <SubTree ID="move_to_position"/>
+        <SubTree ID="move_to_posorientation"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+
+