diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0accf0122a954b6712d18a3cb0fc584916633521..245db1733626ef042cbfd484a1cc73b250def55f 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 control_msgs geometry_msgs iri_base_algorithm iri_behaviortree)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs geometry_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -88,6 +88,7 @@ find_package(iriutils REQUIRED)
generate_dynamic_reconfigure_options(
cfg/TiagoHeadModule.cfg
cfg/TiagoHeadClient.cfg
+ cfg/TiagoHeadBtClient.cfg
)
###################################
@@ -105,7 +106,7 @@ set(module_bt_name tiago_head_module_bt)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${module_name} ${module_bt_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs geometry_msgs iri_base_algorithm iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs geometry_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
# DEPENDS system_lib
)
@@ -159,6 +160,14 @@ target_link_libraries(${module_bt_name} ${catkin_LIBRARIES})
target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES})
add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${module_name})
+#BT_client
+set(bt_client_name tiago_head_bt_client)
+add_executable(${bt_client_name} src/tiago_head_bt_client_alg_node.cpp)
+target_link_libraries(${bt_client_name} ${catkin_LIBRARIES})
+target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
+target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_bt_name}.so)
+add_dependencies(${bt_client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name} ${module_bt_name})
+
#############
## Install ##
#############
diff --git a/cfg/TiagoHeadBtClient.cfg b/cfg/TiagoHeadBtClient.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..c1e393d9bf315ebb320291d4998b28aada4ca79e
--- /dev/null
+++ b/cfg/TiagoHeadBtClient.cfg
@@ -0,0 +1,60 @@
+#! /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_head_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+from iri_base_bt_client.submodule import add_bt_client_params
+
+gen = ParameterGenerator()
+move_action = gen.add_group("Move head action")
+point_action = gen.add_group("Point head 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)
+add_bt_client_params(gen)
+gen.add("START", bool_t, 0, "synchronous start", False)
+gen.add("RESTART", bool_t, 0, "asynchronous restart of BT", False)
+gen.add("max_velocity", double_t, 0, "Maximum head velocity", 1.0, 0.1, 10)
+
+move_action.add("pan_angle", double_t, 0, "Pan angle for the regulat motion", 0.0, -3.14159, 3.14159)
+move_action.add("tilt_angle", double_t, 0, "Tilt angle for the regulat motion", 0.0, -3.14159, 3.14159)
+move_action.add("move_duration", double_t, 0, "Maximum time allowed for the motion", 1.0, 0.1, 10)
+
+point_action.add("x_pos", double_t, 0, "Target X position for pointing", 0.0, -10.0, 10.0)
+point_action.add("y_pos", double_t, 0, "Target Y position for pointing", 0.0, -10.0, 10.0)
+point_action.add("z_pos", double_t, 0, "Target Z position for pointing", 0.0, -10.0, 10.0)
+point_action.add("frame_id", str_t, 0, "Reference frame of the target point", "/xtion_rgb_optical_frame")
+point_action.add("point_duration", double_t, 0, "Maximum time allowed for the pointing",1.0, 0.1, 10)
+
+exit(gen.generate(PACKAGE, "TiagoHeadModuleBtClientAlgNode", "TiagoHeadBtClient"))
diff --git a/include/tiago_head_bt_client_alg_node.h b/include/tiago_head_bt_client_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..d17886efc0007532e63fdbbe39f1ea31d9b1e68d
--- /dev/null
+++ b/include/tiago_head_bt_client_alg_node.h
@@ -0,0 +1,174 @@
+// Copyright (C) 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_head_module_bt_client_alg_node_h_
+#define _tiago_head_module_bt_client_alg_node_h_
+
+#include <iri_base_bt_client/iri_base_bt_client.h>
+#include <tiago_head_module/TiagoHeadBtClientConfig.h>
+
+#include "tiago_head_module/tiago_head_bt_module.h"
+#include <geometry_msgs/PointStamped.h>
+
+// [publisher subscriber headers]
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class TiagoHeadModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<tiago_head_module::TiagoHeadBtClientConfig>
+{
+ private:
+ // object of module
+ CTiagoHeadModuleBT tiago_head_module_bt;
+
+ // Client actions and conditions (always return BT::NodeStatus::SUCCESS)
+
+ // [publisher attributes]
+
+ // [subscriber attributes]
+
+ // [service attributes]
+
+ // [client attributes]
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ /**
+ * \brief Sync function to set the tiago_head_move_to input ports.
+ *
+ * It takes the values from Dynamic Reconfigure.
+ *
+ * It has the following output ports:
+ *
+ * pan (double): Desired angle for the pan joint in radians.
+ *
+ * tilt (double): Desired angle for the tilt joint in radians.
+ *
+ * duration (double): [Optional] Time from start to move the head.
+ *
+ * new_goal (bool): If it's a new_goal (Only used by async_move_to function).
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus set_tiago_head_move_to_input(BT::TreeNode& self);
+
+ /**
+ * \brief Sync function to set the tiago_head_move_to_pointstamped input ports.
+ *
+ * It takes the values from Dynamic Reconfigure.
+ *
+ * It has the following output ports:
+ *
+ * point (geometry_msgs::PointStamped): The target Point.
+ *
+ * duration (double): [Optional] Time from start to move the head.
+ *
+ * new_goal (bool): If it's a new_goal (Only used by async_point_to function).
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus set_tiago_head_point_to_pointstamped_input(BT::TreeNode& self);
+
+ /**
+ * \brief Sync function to set the set_tiago_head_max_velocity input ports.
+ *
+ * It takes the values from Dynamic Reconfigure.
+ *
+ * It has the following output ports:
+ *
+ * vel (double): Head maximum velocity.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus set_tiago_head_max_velocity_input(BT::TreeNode& self);
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ TiagoHeadModuleBtClientAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~TiagoHeadModuleBtClientAlgNode(void);
+
+ protected:
+
+ /**
+ * \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(tiago_head_module::TiagoHeadBtClientConfig &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
\ No newline at end of file
diff --git a/launch/tiago_head_bt_client.launch b/launch/tiago_head_bt_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c8be306f3b1481e089bf0fbfb1464c5fed3ae0d6
--- /dev/null
+++ b/launch/tiago_head_bt_client.launch
@@ -0,0 +1,35 @@
+<?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"/>
+
+ <arg name="tree_path" default="$(find tiago_head_module)/src/xml" />
+ <arg name="tree_file" default="move_to_bt" />
+ <arg name="bt_client_rate" default="10" />
+
+ <node name="$(arg node_name)"
+ pkg="tiago_head_module"
+ type="tiago_head_bt_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" />
+ <param name="path" value="$(arg tree_path)"/>
+ <param name="tree_xml_file" value="$(arg tree_file)"/>
+ <param name="bt_client_rate" value="$(arg bt_client_rate)"/>
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/launch/tiago_head_bt_client_sim.launch b/launch/tiago_head_bt_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..cb92fdcc2badceb3ea0fa695e0c01046e359b48a
--- /dev/null
+++ b/launch/tiago_head_bt_client_sim.launch
@@ -0,0 +1,32 @@
+<?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"/>
+
+ <arg name="tree_path" default="$(find tiago_head_module)/src/xml" />
+ <arg name="tree_file" default="move_to_bt" />
+ <arg name="bt_client_rate" default="10" />
+
+ <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_bt_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_action" value="$(arg move_action)"/>
+ <arg name="point_action" value="$(arg point_action)"/>
+ <arg name="tree_path" value="$(arg tree_path)"/>
+ <arg name="tree_file" value="$(arg tree_file)"/>
+ <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
+ </include>
+
+</launch>
+
diff --git a/package.xml b/package.xml
index 27480ccad522b542c012f4061dd1cd046853115c..62490930cfd11d5f6848150cb456ea05af12ca7d 100644
--- a/package.xml
+++ b/package.xml
@@ -44,6 +44,7 @@
<build_depend>control_msgs</build_depend>
<build_depend>geometry_msgs</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>-->
@@ -54,6 +55,7 @@
<run_depend>control_msgs</run_depend>
<run_depend>geometry_msgs</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>-->
diff --git a/src/tiago_head_bt_client_alg_node.cpp b/src/tiago_head_bt_client_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..582416487c16a5f89e2f5ba45adc3ef2a29d1bbf
--- /dev/null
+++ b/src/tiago_head_bt_client_alg_node.cpp
@@ -0,0 +1,99 @@
+#include "tiago_head_bt_client_alg_node.h"
+
+TiagoHeadModuleBtClientAlgNode::TiagoHeadModuleBtClientAlgNode(void) :
+ behaviortree_base::IriBaseBTClient<tiago_head_module::TiagoHeadBtClientConfig>(),
+ tiago_head_module_bt()
+{
+ // [init publishers]
+
+ // [init subscribers]
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+ this->tiago_head_module_bt.init(factory);
+ BT::PortsList set_tiago_head_move_to_input_ports = {BT::OutputPort<double>("pan"), BT::OutputPort<double>("tilt"), BT::OutputPort<double>("duration"), BT::OutputPort<bool>("new_goal")};
+ BT::PortsList set_tiago_head_point_to_pointstamped_input_ports = {BT::OutputPort<geometry_msgs::PointStamped>("point"), BT::OutputPort<double>("duration"), BT::OutputPort<bool>("new_goal")};
+ BT::PortsList set_tiago_head_max_velocity_input_ports = {BT::OutputPort<double>("vel")};
+
+ this->factory.registerIriAsyncAction("set_tiago_head_move_to_input", std::bind(&TiagoHeadModuleBtClientAlgNode::set_tiago_head_move_to_input, this, std::placeholders::_1), set_tiago_head_move_to_input_ports);
+ this->factory.registerIriAsyncAction("set_tiago_head_point_to_pointstamped_input", std::bind(&TiagoHeadModuleBtClientAlgNode::set_tiago_head_point_to_pointstamped_input, this, std::placeholders::_1), set_tiago_head_point_to_pointstamped_input_ports);
+ this->factory.registerIriAsyncAction("set_tiago_head_max_velocity_input", std::bind(&TiagoHeadModuleBtClientAlgNode::set_tiago_head_max_velocity_input, this, std::placeholders::_1), set_tiago_head_max_velocity_input_ports);
+}
+
+TiagoHeadModuleBtClientAlgNode::~TiagoHeadModuleBtClientAlgNode(void)
+{
+ // [free dynamic memory]
+}
+
+BT::NodeStatus TiagoHeadModuleBtClientAlgNode::set_tiago_head_move_to_input(BT::TreeNode& self)
+{
+ self.setOutput("pan", this->config_.pan_angle);
+ self.setOutput("tilt", this->config_.tilt_angle);
+ self.setOutput("duration", this->config_.move_duration);
+ self.setOutput("new_goal", true);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus TiagoHeadModuleBtClientAlgNode::set_tiago_head_point_to_pointstamped_input(BT::TreeNode& self)
+{
+ geometry_msgs::PointStamped point;
+ point.header.frame_id = this->config_.frame_id;
+ point.header.stamp = ros::Time::now();
+ point.point.x = this->config_.x_pos;
+ point.point.y = this->config_.y_pos;
+ point.point.z = this->config_.z_pos;
+ self.setOutput("point", point);
+ self.setOutput("duration", this->config_.point_duration);
+ self.setOutput("new_goal", true);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus TiagoHeadModuleBtClientAlgNode::set_tiago_head_max_velocity_input(BT::TreeNode& self)
+{
+ self.setOutput("vel", this->config_.max_velocity);
+ return BT::NodeStatus::SUCCESS;
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+
+void TiagoHeadModuleBtClientAlgNode::node_config_update(tiago_head_module::TiagoHeadBtClientConfig &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;
+ }
+
+ this->config_=config;
+ this->unlock();
+}
+
+void TiagoHeadModuleBtClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return behaviortree_base::main<TiagoHeadModuleBtClientAlgNode>(argc, argv, "tiago_head_module_bt_client_alg_node");
+}
diff --git a/src/xml/move_to_bt.xml b/src/xml/move_to_bt.xml
new file mode 100644
index 0000000000000000000000000000000000000000..002a16c4119788d63a5219b1e999906098d1a2a0
--- /dev/null
+++ b/src/xml/move_to_bt.xml
@@ -0,0 +1,128 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+ <!-- ////////// -->
+ <BehaviorTree ID="BehaviorTree">
+ <SequenceStar>
+ <Action ID="async_is_start_tree"/>
+ <SubTree ID="move_to"/>
+ </SequenceStar>
+ </BehaviorTree>
+ <!-- ////////// -->
+ <BehaviorTree ID="move_to">
+ <SequenceStar>
+ <Action ID="set_tiago_head_max_velocity_input" vel="{vel}"/>
+ <Action ID="set_tiago_head_max_velocity" vel="{vel}"/>
+ <Action ID="set_tiago_head_move_to_input" pan="{pan}" tilt="{tilt}" duration="{duration}" new_goal="{new_goal}"/>
+ <Action ID="async_tiago_head_move_to" pan="{pan}" tilt="{tilt}" duration="{duration}" 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_head_module -->
+ <Action ID="sync_cancel_tiago_head_action"/>
+ <Action ID="async_cancel_tiago_head_action"/>
+ <Action ID="async_is_tiago_head_finished"/>
+ <Action ID="sync_tiago_head_point_to_pointstamped">
+ <input_port name="point">The target Point</input_port>
+ <input_port default="-1.0" name="duration">Time from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_point_to_pointstamped">
+ <input_port name="point"> The target Point</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="async_tiago_head_follow_target_pointstamped">
+ <input_port name="point"> The target Point</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="sync_tiago_head_point_to">
+ <input_port name="x"> The target Point x coordenate</input_port>
+ <input_port name="y"> The target Point y coordenate</input_port>
+ <input_port name="z"> The target Point z coordenate</input_port>
+ <input_port name="frame_id"> The target Point reference frame id</input_port>
+ <input_port name="time"> Time at which the desired coordinate was defined</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_point_to">
+ <input_port name="x"> The target Point x coordenate</input_port>
+ <input_port name="y"> The target Point y coordenate</input_port>
+ <input_port name="z"> The target Point z coordenate</input_port>
+ <input_port name="frame_id"> The target Point reference frame id</input_port>
+ <input_port name="time"> Time at which the desired coordinate was defined</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="async_tiago_head_follow_target">
+ <input_port name="x"> The target Point x coordenate</input_port>
+ <input_port name="y"> The target Point y coordenate</input_port>
+ <input_port name="z"> The target Point z coordenate</input_port>
+ <input_port name="frame_id"> The target Point reference frame id</input_port>
+ <input_port name="time"> Time at which the desired coordinate was defined</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="sync_tiago_head_move_to">
+ <input_port name="pan"> Desired angle for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angle for the tilt joint in radians</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_move_to">
+ <input_port name="pan"> Desired angle for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angle for the tilt joint in radians</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="sync_tiago_head_move_to_trajectory">
+ <input_port name="pan"> Desired angles for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angles for the tilt joint in radians</input_port>
+ <input_port name="duration"> Times from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_move_to_trajectory">
+ <input_port name="pan"> Desired angles for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angles for the tilt joint in radians</input_port>
+ <input_port name="duration"> Times from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="set_tiago_head_max_velocity">
+ <input_port name="vel"> Head maximum velocity</input_port>
+ </Action>
+ <Condition ID="is_tiago_head_finished"/>
+ <Condition ID="is_tiago_head_module_running"/>
+ <Condition ID="is_tiago_head_module_success"/>
+ <Condition ID="is_tiago_head_module_action_start_fail"/>
+ <Condition ID="is_tiago_head_module_timeout"/>
+ <Condition ID="is_tiago_head_module_fb_watchdog"/>
+ <Condition ID="is_tiago_head_module_aborted"/>
+ <Condition ID="is_tiago_head_module_preempted"/>
+ <Condition ID="is_tiago_head_module_rejected"/>
+ <!-- client -->
+ <Action ID="set_tiago_head_point_to_pointstamped_input">
+ <output_port name="point"> The target Point</output_port>
+ <output_port default="-1.0" name="duration"> Time from start to move the head</output_port>
+ <output_port default="True" name="new_goal"> If it's a new_goal</output_port>
+ </Action>
+ <Action ID="set_tiago_head_move_to_input">
+ <output_port name="pan"> Desired angle for the pan joint in radians</output_port>
+ <output_port name="tilt"> Desired angle for the tilt joint in radians</output_port>
+ <output_port default="-1.0" name="duration"> Time from start to move the head</output_port>
+ <output_port default="True" name="new_goal"> If it's a new_goal</output_port>
+ </Action>
+ <Action ID="set_tiago_head_max_velocity_input">
+ <output_port name="vel"> Head maximum velocity</output_port>
+ </Action>
+ </TreeNodesModel>
+ <!-- ////////// -->
+</root>
+
+
diff --git a/src/xml/point_to_bt.xml b/src/xml/point_to_bt.xml
new file mode 100644
index 0000000000000000000000000000000000000000..73a69f1a55939c820bff2ee433e11266546f82d2
--- /dev/null
+++ b/src/xml/point_to_bt.xml
@@ -0,0 +1,128 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+ <!-- ////////// -->
+ <BehaviorTree ID="BehaviorTree">
+ <SequenceStar>
+ <Action ID="async_is_start_tree"/>
+ <SubTree ID="point_to"/>
+ </SequenceStar>
+ </BehaviorTree>
+ <!-- ////////// -->
+ <BehaviorTree ID="point_to">
+ <SequenceStar>
+ <Action ID="set_tiago_head_max_velocity_input" vel="{vel}"/>
+ <Action ID="set_tiago_head_max_velocity" vel="{vel}"/>
+ <Action ID="set_tiago_head_point_to_pointstamped_input" point="{point}" duration="{duration}" new_goal="{new_goal}"/>
+ <Action ID="async_tiago_head_point_to_pointstamped" point="{point}" duration="{duration}" 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_head_module -->
+ <Action ID="sync_cancel_tiago_head_action"/>
+ <Action ID="async_cancel_tiago_head_action"/>
+ <Action ID="async_is_tiago_head_finished"/>
+ <Action ID="sync_tiago_head_point_to_pointstamped">
+ <input_port name="point">The target Point</input_port>
+ <input_port default="-1.0" name="duration">Time from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_point_to_pointstamped">
+ <input_port name="point"> The target Point</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="async_tiago_head_follow_target_pointstamped">
+ <input_port name="point"> The target Point</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="sync_tiago_head_point_to">
+ <input_port name="x"> The target Point x coordenate</input_port>
+ <input_port name="y"> The target Point y coordenate</input_port>
+ <input_port name="z"> The target Point z coordenate</input_port>
+ <input_port name="frame_id"> The target Point reference frame id</input_port>
+ <input_port name="time"> Time at which the desired coordinate was defined</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_point_to">
+ <input_port name="x"> The target Point x coordenate</input_port>
+ <input_port name="y"> The target Point y coordenate</input_port>
+ <input_port name="z"> The target Point z coordenate</input_port>
+ <input_port name="frame_id"> The target Point reference frame id</input_port>
+ <input_port name="time"> Time at which the desired coordinate was defined</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="async_tiago_head_follow_target">
+ <input_port name="x"> The target Point x coordenate</input_port>
+ <input_port name="y"> The target Point y coordenate</input_port>
+ <input_port name="z"> The target Point z coordenate</input_port>
+ <input_port name="frame_id"> The target Point reference frame id</input_port>
+ <input_port name="time"> Time at which the desired coordinate was defined</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="sync_tiago_head_move_to">
+ <input_port name="pan"> Desired angle for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angle for the tilt joint in radians</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_move_to">
+ <input_port name="pan"> Desired angle for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angle for the tilt joint in radians</input_port>
+ <input_port default="-1.0" name="duration"> Time from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="sync_tiago_head_move_to_trajectory">
+ <input_port name="pan"> Desired angles for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angles for the tilt joint in radians</input_port>
+ <input_port name="duration"> Times from start to move the head</input_port>
+ </Action>
+ <Action ID="async_tiago_head_move_to_trajectory">
+ <input_port name="pan"> Desired angles for the pan joint in radians</input_port>
+ <input_port name="tilt"> Desired angles for the tilt joint in radians</input_port>
+ <input_port name="duration"> Times from start to move the head</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="set_tiago_head_max_velocity">
+ <input_port name="vel"> Head maximum velocity</input_port>
+ </Action>
+ <Condition ID="is_tiago_head_finished"/>
+ <Condition ID="is_tiago_head_module_running"/>
+ <Condition ID="is_tiago_head_module_success"/>
+ <Condition ID="is_tiago_head_module_action_start_fail"/>
+ <Condition ID="is_tiago_head_module_timeout"/>
+ <Condition ID="is_tiago_head_module_fb_watchdog"/>
+ <Condition ID="is_tiago_head_module_aborted"/>
+ <Condition ID="is_tiago_head_module_preempted"/>
+ <Condition ID="is_tiago_head_module_rejected"/>
+ <!-- client -->
+ <Action ID="set_tiago_head_point_to_pointstamped_input">
+ <output_port name="point"> The target Point</output_port>
+ <output_port default="-1.0" name="duration"> Time from start to move the head</output_port>
+ <output_port default="True" name="new_goal"> If it's a new_goal</output_port>
+ </Action>
+ <Action ID="set_tiago_head_move_to_input">
+ <output_port name="pan"> Desired angle for the pan joint in radians</output_port>
+ <output_port name="tilt"> Desired angle for the tilt joint in radians</output_port>
+ <output_port default="-1.0" name="duration"> Time from start to move the head</output_port>
+ <output_port default="True" name="new_goal"> If it's a new_goal</output_port>
+ </Action>
+ <Action ID="set_tiago_head_max_velocity_input">
+ <output_port name="vel"> Head maximum velocity</output_port>
+ </Action>
+ </TreeNodesModel>
+ <!-- ////////// -->
+</root>
+
+