diff --git a/CMakeLists.txt b/CMakeLists.txt index 140b8d80b22466f588c6ad9dd6acc65cb1a00976..f72af6762d2cde4109578bb5306b8a38a9d15152 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 pal_interaction_msgs iri_base_algorithm iri_behaviortree) +find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib pal_interaction_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/TiagoTTSModule.cfg cfg/TiagoTTSClient.cfg + cfg/TiagoTTSBtClient.cfg ) ################################### @@ -105,7 +106,7 @@ set(module_bt_name tiago_tts_module_bt) catkin_package( INCLUDE_DIRS include LIBRARIES ${module_name} - CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib pal_interaction_msgs iri_base_algorithm iri_behaviortree + CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib pal_interaction_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree # DEPENDS system_lib ) @@ -157,6 +158,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_tts_bt_client) +add_executable(${bt_client_name} src/tiago_tts_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/TiagoTTSBtClient.cfg b/cfg/TiagoTTSBtClient.cfg new file mode 100755 index 0000000000000000000000000000000000000000..0274cfc84144bc34c0f500bedf24a3d261773778 --- /dev/null +++ b/cfg/TiagoTTSBtClient.cfg @@ -0,0 +1,53 @@ +#! /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_tts_module' + +from dynamic_reconfigure.parameter_generator_catkin import * +from iri_base_bt_client.submodule import add_bt_client_params + +gen = ParameterGenerator() + +# Name Type Reconfiguration level Description Default Min Max +#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0) +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("set_tts_say_and_speaker_input", bool_t, 0, "Move tts to position", False) +gen.add("speaker", str_t, 0, "Desired speaker", "") +gen.add("language", str_t, 0, "Desired language", "en_GB") +gen.add("text", str_t, 0, "Text to speak", "") +gen.add("delay", double_t, 0, "Time to wait before speaking", 0.0, 0.0, 10.0) +# gen.add("cancel_motion", bool_t, 0, "Cancel current motion", False) + +exit(gen.generate(PACKAGE, "TiagoTTSModuleBtClientAlgNode", "TiagoTTSBtClient")) diff --git a/include/tiago_tts_bt_client_alg_node.h b/include/tiago_tts_bt_client_alg_node.h new file mode 100644 index 0000000000000000000000000000000000000000..8ea978c08daf642ba42f58676cfbc75de5c301aa --- /dev/null +++ b/include/tiago_tts_bt_client_alg_node.h @@ -0,0 +1,137 @@ +// 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_tts_module_bt_client_alg_node_h_ +#define _tiago_tts_module_bt_client_alg_node_h_ + +#include <iri_base_bt_client/iri_base_bt_client.h> +#include <tiago_tts_module/TiagoTTSBtClientConfig.h> + +#include "tiago_tts_module/tiago_tts_bt_module.h" + +// [publisher subscriber headers] + +// [service client headers] + +// [action server client headers] + +/** + * \brief IRI ROS Specific Algorithm Class + * + */ +class TiagoTTSModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<tiago_tts_module::TiagoTTSBtClientConfig> +{ + private: + // object of module + CTiagoTTSModuleBT tiago_tts_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] + + bool set_tts_say_and_speaker_input; + + /** + * \brief Async function to set the tiago_tts_say and tiago_tts_set_speaker input ports. + * + * It takes the values from Dynamic Reconfigure. + * + * It has the following output ports: + * + * speaker (std::string): The desired speaker. + * + * text (std::string): the text to speak. + * + * language (std::string): the language identifier to use. + * + * delay (duoble): the time before start speaking in seconds. + * + * new_goal (bool): If it's a new_goal. + * + * \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_tts_say_and_speaker_inputs(BT::TreeNode& self); + + public: + /** + * \brief Constructor + * + * This constructor initializes specific class attributes and all ROS + * communications variables to enable message exchange. + */ + TiagoTTSModuleBtClientAlgNode(void); + + /** + * \brief Destructor + * + * This destructor frees all necessary dynamic memory allocated within this + * this class. + */ + ~TiagoTTSModuleBtClientAlgNode(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_tts_module::TiagoTTSBtClientConfig &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/tts_bt_client.launch b/launch/tts_bt_client.launch new file mode 100644 index 0000000000000000000000000000000000000000..70099433bffe764b986880bcf9f0c6bb5bf1b941 --- /dev/null +++ b/launch/tts_bt_client.launch @@ -0,0 +1,30 @@ +<?xml version="1.0" encoding="UTF-8"?> +<launch> + <arg name="node_name" default="tts_client"/> + <arg name="output" default="screen"/> + <arg name="launch_prefix" default=""/> + <arg name="config_file" default="$(find tiago_tts_module)/config/tiago_tts_module_default.yaml"/> + <arg name="tts_topic" default="/tts"/> + + <arg name="tree_path" default="$(find tiago_tts_module)/src/xml" /> + <arg name="tree_file" default="bt_test" /> + <arg name="bt_client_rate" default="10" /> + + <!-- launch the play motion client node --> + <node name="$(arg node_name)" + pkg="tiago_tts_module" + type="tiago_tts_bt_client" + output="$(arg output)" + launch-prefix="$(arg launch_prefix)"> + <remap from="~/tts_module/tts" + to="$(arg tts_topic)"/> + <rosparam file="$(arg config_file)" command="load" ns="tts_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/tts_client.launch b/launch/tts_client.launch index dfbdcd3b390012e409424aabc5a2a8c79c0b8285..efc88e9c7f1b932dd10c9241e30176c48f8cdbf7 100644 --- a/launch/tts_client.launch +++ b/launch/tts_client.launch @@ -1,3 +1,4 @@ +<?xml version="1.0" encoding="UTF-8"?> <launch> <arg name="node_name" default="tts_client"/> <arg name="output" default="screen"/> diff --git a/package.xml b/package.xml index 4d29f3274ff7c62a6fd6fc416fa95e7e8a106a1d..3e1264c6cb048d2a5b64ecdc7a26cf4fd64d34d3 100644 --- a/package.xml +++ b/package.xml @@ -46,6 +46,7 @@ <build_depend>dynamic_reconfigure</build_depend> <build_depend>actionlib</build_depend> <build_depend>pal_interaction_msgs</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>--> @@ -55,6 +56,7 @@ <run_depend>dynamic_reconfigure</run_depend> <run_depend>actionlib</run_depend> <run_depend>pal_interaction_msgs</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_tts_bt_client_alg_node.cpp b/src/tiago_tts_bt_client_alg_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..91e5bcbf13f5a01d2a723dd591b2507d6589aa29 --- /dev/null +++ b/src/tiago_tts_bt_client_alg_node.cpp @@ -0,0 +1,91 @@ +#include "tiago_tts_bt_client_alg_node.h" + +TiagoTTSModuleBtClientAlgNode::TiagoTTSModuleBtClientAlgNode(void) : + behaviortree_base::IriBaseBTClient<tiago_tts_module::TiagoTTSBtClientConfig>(), + tiago_tts_module_bt() +{ + // [init publishers] + + // [init subscribers] + + // [init services] + + // [init clients] + + // [init action servers] + + // [init action clients] + + this->set_tts_say_and_speaker_input = false; + this->tiago_tts_module_bt.init(this->factory); + + // Init local functions + BT::PortsList tts_say_and_speaker_ports = {BT::OutputPort<std::string>("speaker"), BT::OutputPort<std::string>("text"), BT::OutputPort<std::string>("language"), BT::OutputPort<double>("delay"), BT::OutputPort<bool>("new_goal")}; + + this->factory.registerIriAsyncAction("set_tiago_tts_say_and_speaker_inputs", std::bind(&TiagoTTSModuleBtClientAlgNode::set_tiago_tts_say_and_speaker_inputs, this, std::placeholders::_1), tts_say_and_speaker_ports); +} + +TiagoTTSModuleBtClientAlgNode::~TiagoTTSModuleBtClientAlgNode(void) +{ + // [free dynamic memory] +} + +/* [subscriber callbacks] */ + +/* [service callbacks] */ + +/* [action callbacks] */ + +/* [action requests] */ + + +void TiagoTTSModuleBtClientAlgNode::node_config_update(tiago_tts_module::TiagoTTSBtClientConfig &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_tts_say_and_speaker_input) + { + this->set_tts_say_and_speaker_input = true; + config.set_tts_say_and_speaker_input = false; + } + + this->config_=config; + this->unlock(); +} + +BT::NodeStatus TiagoTTSModuleBtClientAlgNode::set_tiago_tts_say_and_speaker_inputs(BT::TreeNode& self) +{ + if (this->set_tts_say_and_speaker_input) + { + this->set_tts_say_and_speaker_input = false; + self.setOutput("speaker", this->config_.speaker); + self.setOutput("text", this->config_.text); + self.setOutput("language", this->config_.language); + self.setOutput("delay", this->config_.delay); + self.setOutput("new_goal", true); + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::RUNNING; +} + +void TiagoTTSModuleBtClientAlgNode::addNodeDiagnostics(void) +{ +} + +/* main function */ +int main(int argc,char *argv[]) +{ + return behaviortree_base::main<TiagoTTSModuleBtClientAlgNode>(argc, argv, "tiago_tts_module_bt_client_alg_node"); +} diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml new file mode 100644 index 0000000000000000000000000000000000000000..f969c88084d27d5c723e0c28531d6bbcfb95b9d0 --- /dev/null +++ b/src/xml/bt_test.xml @@ -0,0 +1,71 @@ +<?xml version="1.0"?> +<root main_tree_to_execute="BehaviorTree"> + <!-- ////////// --> + <BehaviorTree ID="BehaviorTree"> + <SequenceStar> + <Action ID="async_is_start_tree"/> + <SubTree ID="say"/> + </SequenceStar> + </BehaviorTree> + <!-- ////////// --> + <BehaviorTree ID="say"> + <SequenceStar> + <Action ID="set_tiago_tts_say_and_speaker_inputs" speaker="{speaker}" text="{text}" language="{language}" delay="{delay}" new_goal="{new_goal}"/> + <Action ID="set_tiago_tts_speaker" speaker="{speaker}"/> + <Action ID="async_tiago_tts_say" text="{text}" language="{language}" delay="{delay}" 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_tts_module --> + <Action ID="sync_cancel_tiago_tts_action"/> + <Action ID="async_cancel_tiago_tts_action"/> + <Action ID="async_is_tiago_tts_finished"/> + <Action ID="sync_tiago_tts_say"> + <input_port name="text"> the text to speak</input_port> + <input_port name="language"> the language identifier to use</input_port> + <input_port name="delay"> the time before start speaking in seconds</input_port> + </Action> + <Action ID="async_tiago_tts_say"> + <input_port name="text"> the text to speak</input_port> + <input_port name="language"> the language identifier to use</input_port> + <input_port name="delay"> the time before start speaking in seconds</input_port> + <input_port default="True" name="new_goal"> If it's a new_goal</input_port> + </Action> + <Action ID="set_tiago_tts_speaker"> + <input_port name="speaker"> The desired speaker</input_port> + </Action> + <Condition ID="is_tiago_tts_finished"/> + <Condition ID="is_tiago_tts_module_running"/> + <Condition ID="is_tiago_tts_module_success"/> + <Condition ID="is_tiago_tts_module_action_start_fail"/> + <Condition ID="is_tiago_tts_module_timeout"/> + <Condition ID="is_tiago_tts_module_fb_watchdog"/> + <Condition ID="is_tiago_tts_module_aborted"/> + <Condition ID="is_tiago_tts_module_preempted"/> + <Condition ID="is_tiago_tts_module_rejected"/> + <!-- Client --> + <Action ID="set_tiago_tts_say_and_speaker_inputs"> + <output_port name="speaker"> The desired speaker</output_port> + <output_port name="text"> the text to speak</output_port> + <output_port name="language"> the language identifier to use</output_port> + <output_port name="delay"> the time before start speaking in seconds</output_port> + <output_port default="True" name="new_goal"> If it's a new_goal</output_port> + </Action> + <!-- Tree --> + <SubTree ID="say"/> + </TreeNodesModel> + <!-- ////////// --> +</root> + +