diff --git a/cfg/TiagoTTSBtClient.cfg b/cfg/TiagoTTSBtClient.cfg
index 0274cfc84144bc34c0f500bedf24a3d261773778..a28d700cee1a90047b9616854b20732147f42f4e 100755
--- a/cfg/TiagoTTSBtClient.cfg
+++ b/cfg/TiagoTTSBtClient.cfg
@@ -41,8 +41,6 @@ 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")
diff --git a/src/tiago_tts_bt_client_alg_node.cpp b/src/tiago_tts_bt_client_alg_node.cpp
index d3d84a3e13fd42cc44dfdf0fadc7486588eb9b38..48b4d7c3e8911a73bba6a95237a0aafdd26ed9a7 100644
--- a/src/tiago_tts_bt_client_alg_node.cpp
+++ b/src/tiago_tts_bt_client_alg_node.cpp
@@ -44,18 +44,6 @@ void TiagoTTSModuleBtClientAlgNode::node_config_update(tiago_tts_module::TiagoTT
{
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;
diff --git a/src/tiago_tts_bt_module.cpp b/src/tiago_tts_bt_module.cpp
index 317f202ca78130a3cc5772c0f73ab5c96375b2d6..1a000a8ef6e2b02f59b744d71cdd1f5d400b055a 100644
--- a/src/tiago_tts_bt_module.cpp
+++ b/src/tiago_tts_bt_module.cpp
@@ -107,6 +107,7 @@ BT::NodeStatus CTiagoTTSModuleBT::async_tiago_tts_say(BT::TreeNode& self)
if (new_goal)
{
+ self.setOutput("new_goal", false);
if (!text || (!language && delay))
{
ROS_ERROR("CTiagoNavModuleBT::sync_tiago_tts_say-> Incorrect or missing input. It needs the following input ports: text (std::string), [Optional] language (std::string) and [Optional] delay (double)");
@@ -137,8 +138,6 @@ BT::NodeStatus CTiagoTTSModuleBT::async_tiago_tts_say(BT::TreeNode& self)
else
return BT::NodeStatus::FAILURE;
}
- if (new_goal)
- self.setOutput("new_goal", false);
return BT::NodeStatus::RUNNING;
}
@@ -155,11 +154,7 @@ BT::NodeStatus CTiagoTTSModuleBT::async_cancel_tiago_tts_action(void)
this->tiago_tts_module.stop();
if (this->tiago_tts_module.is_finished())
{
- tiago_tts_module_status_t tiago_tts_module_status = this->tiago_tts_module.get_status();
- if (tiago_tts_module_status == TIAGO_TTS_MODULE_SUCCESS)
- return BT::NodeStatus::SUCCESS;
- else
- return BT::NodeStatus::FAILURE;
+ return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::RUNNING;
}
diff --git a/src/tiago_tts_module.cpp b/src/tiago_tts_module.cpp
index 9594b704722ed66d5d9c7d45ecddb248687265dc..5b155e283b42faff048eded705b98e394fcb21c4 100644
--- a/src/tiago_tts_module.cpp
+++ b/src/tiago_tts_module.cpp
@@ -55,34 +55,28 @@ void CTiagoTTSModule::state_machine(void)
case ACTION_SUCCESS: ROS_INFO("CTiagoTTSModule : action ended successfully");
this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_SUCCESS;
- this->tts_action.stop_timeout();
break;
case ACTION_TIMEOUT: ROS_ERROR("CTiagoTTSModule : action did not finish in the allowed time");
this->tts_action.cancel();
this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_TIMEOUT;
- this->tts_action.stop_timeout();
break;
case ACTION_FB_WATCHDOG: ROS_ERROR("CTiagoTTSModule : No feeback received for a long time");
this->tts_action.cancel();
this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_FB_WATCHDOG;
- this->tts_action.stop_timeout();
break;
case ACTION_ABORTED: ROS_ERROR("CTiagoTTSModule : Action failed to complete");
this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_ABORTED;
- this->tts_action.stop_timeout();
break;
case ACTION_PREEMPTED: ROS_ERROR("CTiagoTTSModule : Action was interrupted by another request");
this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_PREEMPTED;
- this->tts_action.stop_timeout();
break;
case ACTION_REJECTED: ROS_ERROR("CTiagoTTSModule : Action was rejected by server");
this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_REJECTED;
- this->tts_action.stop_timeout();
break;
}
if(this->cancel_pending)
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
new file mode 100644
index 0000000000000000000000000000000000000000..bb46f70b0665166cf7020125d541503173fd4cec
--- /dev/null
+++ b/src/xml/bt_definitions.xml
@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+ <!-- ////////// -->
+ <BehaviorTree ID="BehaviorTree">
+ <Action ID="RUNNING"/>
+ </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="RUNNING"/>
+ <Action ID="is_variable_true">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="is_variable_false">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="transform_pose">
+ <input_port name="pose_in"> (geometry_msgs::PoseStamped): Pose to be transformed.</input_port>
+ <input_port name="target_frame"> (std::string): Frame to transform the pose.</input_port>
+ <output_port name="pose_out"> (geometry_msgs::PoseStamped): Transformed pose.</output_port>
+ </Action>
+ <Action ID="compute_distance">
+ <input_port name="pose1"> (geometry_msgs::PoseStamped): Pose 1.</input_port>
+ <input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
+ <output_port name="distance"> (double): Distance.</output_port>
+ </Action>
+ <Action ID="compare_bigger">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="compare_smaller">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="print_msg">
+ <input_port name="msg"> (std::string): Msg to print.</input_port>
+ <input_port name="type"> (std::string): It must be info, warn, error or debug.</input_port>
+ </Action>
+ <!-- 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>
+ <inout_port name="new_goal"> If it's a new_goal</inout_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 name="new_goal"> If it's a new_goal</output_port>
+ </Action>
+ <!-- Tree -->
+ <SubTree ID="say"/>
+ </TreeNodesModel>
+ <!-- ////////// -->
+</root>
\ No newline at end of file
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
index 6de98953216d384197f9849c9f398cf19bfa420c..678bc35cbf6e2d6dd457353c8ff5e6c9ab3a9996 100644
--- a/src/xml/bt_test.xml
+++ b/src/xml/bt_test.xml
@@ -3,7 +3,6 @@
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<SequenceStar>
- <Action ID="async_is_start_tree"/>
<SubTree ID="say"/>
</SequenceStar>
</BehaviorTree>
@@ -18,15 +17,35 @@
<!-- ////////// -->
<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"/>
+ <Action ID="RUNNING"/>
+ <Action ID="is_variable_true">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="is_variable_false">
+ <input_port name="variable"> (bool): Bool variable to check.</input_port>
+ </Action>
+ <Action ID="transform_pose">
+ <input_port name="pose_in"> (geometry_msgs::PoseStamped): Pose to be transformed.</input_port>
+ <input_port name="target_frame"> (std::string): Frame to transform the pose.</input_port>
+ <output_port name="pose_out"> (geometry_msgs::PoseStamped): Transformed pose.</output_port>
+ </Action>
+ <Action ID="compute_distance">
+ <input_port name="pose1"> (geometry_msgs::PoseStamped): Pose 1.</input_port>
+ <input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
+ <output_port name="distance"> (double): Distance.</output_port>
+ </Action>
+ <Action ID="compare_bigger">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="compare_smaller">
+ <input_port name="value1"> (double): Value 1.</input_port>
+ <input_port name="value2"> (double): Value 2.</input_port>
+ </Action>
+ <Action ID="print_msg">
+ <input_port name="msg"> (std::string): Msg to print.</input_port>
+ <input_port name="type"> (std::string): It must be info, warn, error or debug.</input_port>
+ </Action>
<!-- tiago_tts_module -->
<Action ID="sync_cancel_tiago_tts_action"/>
<Action ID="async_cancel_tiago_tts_action"/>
@@ -40,7 +59,7 @@
<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 name="new_goal"> If it's a new_goal</input_port>
+ <inout_port name="new_goal"> If it's a new_goal</inout_port>
</Action>
<Action ID="set_tiago_tts_speaker">
<input_port name="speaker"> The desired speaker</input_port>