Skip to content
Snippets Groups Projects
Commit 7739edb8 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Update pending improvements

parent b90239c7
No related branches found
No related tags found
No related merge requests found
...@@ -41,8 +41,6 @@ gen = ParameterGenerator() ...@@ -41,8 +41,6 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max # 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) #gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
add_bt_client_params(gen) 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("set_tts_say_and_speaker_input", bool_t, 0, "Move tts to position", False)
gen.add("speaker", str_t, 0, "Desired speaker", "") gen.add("speaker", str_t, 0, "Desired speaker", "")
gen.add("language", str_t, 0, "Desired language", "en_GB") gen.add("language", str_t, 0, "Desired language", "en_GB")
......
...@@ -44,18 +44,6 @@ void TiagoTTSModuleBtClientAlgNode::node_config_update(tiago_tts_module::TiagoTT ...@@ -44,18 +44,6 @@ void TiagoTTSModuleBtClientAlgNode::node_config_update(tiago_tts_module::TiagoTT
{ {
this->lock(); 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) if (config.set_tts_say_and_speaker_input)
{ {
this->set_tts_say_and_speaker_input = true; this->set_tts_say_and_speaker_input = true;
......
...@@ -107,6 +107,7 @@ BT::NodeStatus CTiagoTTSModuleBT::async_tiago_tts_say(BT::TreeNode& self) ...@@ -107,6 +107,7 @@ BT::NodeStatus CTiagoTTSModuleBT::async_tiago_tts_say(BT::TreeNode& self)
if (new_goal) if (new_goal)
{ {
self.setOutput("new_goal", false);
if (!text || (!language && delay)) 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)"); 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) ...@@ -137,8 +138,6 @@ BT::NodeStatus CTiagoTTSModuleBT::async_tiago_tts_say(BT::TreeNode& self)
else else
return BT::NodeStatus::FAILURE; return BT::NodeStatus::FAILURE;
} }
if (new_goal)
self.setOutput("new_goal", false);
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} }
...@@ -155,11 +154,7 @@ BT::NodeStatus CTiagoTTSModuleBT::async_cancel_tiago_tts_action(void) ...@@ -155,11 +154,7 @@ BT::NodeStatus CTiagoTTSModuleBT::async_cancel_tiago_tts_action(void)
this->tiago_tts_module.stop(); this->tiago_tts_module.stop();
if (this->tiago_tts_module.is_finished()) if (this->tiago_tts_module.is_finished())
{ {
tiago_tts_module_status_t tiago_tts_module_status = this->tiago_tts_module.get_status(); return BT::NodeStatus::SUCCESS;
if (tiago_tts_module_status == TIAGO_TTS_MODULE_SUCCESS)
return BT::NodeStatus::SUCCESS;
else
return BT::NodeStatus::FAILURE;
} }
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} }
......
...@@ -55,34 +55,28 @@ void CTiagoTTSModule::state_machine(void) ...@@ -55,34 +55,28 @@ void CTiagoTTSModule::state_machine(void)
case ACTION_SUCCESS: ROS_INFO("CTiagoTTSModule : action ended successfully"); case ACTION_SUCCESS: ROS_INFO("CTiagoTTSModule : action ended successfully");
this->state=TIAGO_TTS_MODULE_IDLE; this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_SUCCESS; this->status=TIAGO_TTS_MODULE_SUCCESS;
this->tts_action.stop_timeout();
break; break;
case ACTION_TIMEOUT: ROS_ERROR("CTiagoTTSModule : action did not finish in the allowed time"); case ACTION_TIMEOUT: ROS_ERROR("CTiagoTTSModule : action did not finish in the allowed time");
this->tts_action.cancel(); this->tts_action.cancel();
this->state=TIAGO_TTS_MODULE_IDLE; this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_TIMEOUT; this->status=TIAGO_TTS_MODULE_TIMEOUT;
this->tts_action.stop_timeout();
break; break;
case ACTION_FB_WATCHDOG: ROS_ERROR("CTiagoTTSModule : No feeback received for a long time"); case ACTION_FB_WATCHDOG: ROS_ERROR("CTiagoTTSModule : No feeback received for a long time");
this->tts_action.cancel(); this->tts_action.cancel();
this->state=TIAGO_TTS_MODULE_IDLE; this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_FB_WATCHDOG; this->status=TIAGO_TTS_MODULE_FB_WATCHDOG;
this->tts_action.stop_timeout();
break; break;
case ACTION_ABORTED: ROS_ERROR("CTiagoTTSModule : Action failed to complete"); case ACTION_ABORTED: ROS_ERROR("CTiagoTTSModule : Action failed to complete");
this->state=TIAGO_TTS_MODULE_IDLE; this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_ABORTED; this->status=TIAGO_TTS_MODULE_ABORTED;
this->tts_action.stop_timeout();
break; break;
case ACTION_PREEMPTED: ROS_ERROR("CTiagoTTSModule : Action was interrupted by another request"); case ACTION_PREEMPTED: ROS_ERROR("CTiagoTTSModule : Action was interrupted by another request");
this->state=TIAGO_TTS_MODULE_IDLE; this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_PREEMPTED; this->status=TIAGO_TTS_MODULE_PREEMPTED;
this->tts_action.stop_timeout();
break; break;
case ACTION_REJECTED: ROS_ERROR("CTiagoTTSModule : Action was rejected by server"); case ACTION_REJECTED: ROS_ERROR("CTiagoTTSModule : Action was rejected by server");
this->state=TIAGO_TTS_MODULE_IDLE; this->state=TIAGO_TTS_MODULE_IDLE;
this->status=TIAGO_TTS_MODULE_REJECTED; this->status=TIAGO_TTS_MODULE_REJECTED;
this->tts_action.stop_timeout();
break; break;
} }
if(this->cancel_pending) if(this->cancel_pending)
......
<?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
...@@ -3,7 +3,6 @@ ...@@ -3,7 +3,6 @@
<!-- ////////// --> <!-- ////////// -->
<BehaviorTree ID="BehaviorTree"> <BehaviorTree ID="BehaviorTree">
<SequenceStar> <SequenceStar>
<Action ID="async_is_start_tree"/>
<SubTree ID="say"/> <SubTree ID="say"/>
</SequenceStar> </SequenceStar>
</BehaviorTree> </BehaviorTree>
...@@ -18,15 +17,35 @@ ...@@ -18,15 +17,35 @@
<!-- ////////// --> <!-- ////////// -->
<TreeNodesModel> <TreeNodesModel>
<!-- basic_nodes --> <!-- basic_nodes -->
<Action ID="NOP"/> <Action ID="RUNNING"/>
<Action ID="is_not_start_tree"/> <Action ID="is_variable_true">
<Condition ID="async_is_start_tree"/> <input_port name="variable"> (bool): Bool variable to check.</input_port>
<Action ID="is_not_stop_tree"/> </Action>
<Condition ID="async_is_stop_tree"/> <Action ID="is_variable_false">
<Action ID="set_start_tree"/> <input_port name="variable"> (bool): Bool variable to check.</input_port>
<Action ID="set_stop_tree"/> </Action>
<Action ID="reset_start_tree"/> <Action ID="transform_pose">
<Action ID="reset_stop_tree"/> <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 --> <!-- tiago_tts_module -->
<Action ID="sync_cancel_tiago_tts_action"/> <Action ID="sync_cancel_tiago_tts_action"/>
<Action ID="async_cancel_tiago_tts_action"/> <Action ID="async_cancel_tiago_tts_action"/>
...@@ -40,7 +59,7 @@ ...@@ -40,7 +59,7 @@
<input_port name="text"> the text to speak</input_port> <input_port name="text"> the text to speak</input_port>
<input_port name="language"> the language identifier to use</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="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>
<Action ID="set_tiago_tts_speaker"> <Action ID="set_tiago_tts_speaker">
<input_port name="speaker"> The desired speaker</input_port> <input_port name="speaker"> The desired speaker</input_port>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment