diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg index b40d409e820b607cef96bf1090a185e9e26ac33e..c85c42217a9204ca7b9b335cfc8c6b53113f0d52 100755 --- a/cfg/IriStateMachineCompanion.cfg +++ b/cfg/IriStateMachineCompanion.cfg @@ -46,6 +46,8 @@ gen.add("frame_robot_footprint", str_t, 0, "frame to transform poses to", gen.add("robot", str_t, 0, "frame to transform poses to", "tiago") gen.add("conf_bool_test_case_head1" , bool_t, 0, "test head left or right.", True) gen.add("out_std_mesages_conf" , bool_t, 0, "if true we se the internal std_out mesages.", False) +gen.add("out_state_machine_status_mesage_conf" , bool_t, 0, "if true we se the final state of this state machine, the state that the message outputs from this node.", False) + gen.add("threshold_max_dist_between_robot_and_person_conf", double_t, 0, "Maximum distance between robot and person to talk (meters)", 4.5, 0.0, 30.0) gen.add("threshold_less_max_vel_robot_conf", double_t, 0, "less max vel of person because robot needs 0.2m/s more to react", 0.2, 0.0, 3.0) gen.add("threshold_minimun_distance_not_collide_conf", double_t, 0, "collision distance of the save_cmd_vel== 0.3", 0.8, 0.0, 3.0) diff --git a/include/iri_state_machine_companion_alg_node.h b/include/iri_state_machine_companion_alg_node.h index 9ace8c18f0cb33390d8876903bb2e6564ac6f4a6..eccc8164a979667669da73208978916b3fd3bec0 100644 --- a/include/iri_state_machine_companion_alg_node.h +++ b/include/iri_state_machine_companion_alg_node.h @@ -103,6 +103,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm< double threshold_to_return_; bool out_std_mesages_; + bool out_state_machine_status_mesage_; bool bool_test_case_head1_; //differenciate states of group accompaniment and terrined project of 1 person accompaniment diff --git a/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp index c0ea24547fc6a8e202b8bda35484359a422ec88a..3c1c859777887d274b985610405e4310ee069159 100644 --- a/src/iri_state_machine_companion_alg_node.cpp +++ b/src/iri_state_machine_companion_alg_node.cpp @@ -94,6 +94,7 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void) this->tibi_akp_status_UInt64_msg_.data =0; // case no status. out_std_mesages_=config_.out_std_mesages_conf; + out_state_machine_status_mesage_=config_.out_state_machine_status_mesage_conf; //terrinet_project_=config_.terrinet_project_conf; //differenciate states of group accompaniment and terrined project of 1 person accompaniment @@ -137,18 +138,23 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void) if( diffangle(theta, angle) < 0 ) { this->tibi_akp_status_UInt64_msg_.data =11; // por ejemplo right=11. - std::cout<< " iri_state_machine; diff<0; status=11" << std::endl; + if(out_std_mesages_){ + std::cout<< " iri_state_machine; diff<0; status=11" << std::endl;} }else{ - std::cout<< " iri_state_machine; diff>0; status=12" << std::endl; + if(out_std_mesages_){ + std::cout<< " iri_state_machine; diff>0; status=12" << std::endl;} this->tibi_akp_status_UInt64_msg_.data =12; // por ejemplo left =12. } }else{ if( diffangle(theta, angle) < 0 ) { - std::cout<< " iri_state_machine; diff<0; status=12" << std::endl; - this->tibi_akp_status_UInt64_msg_.data =12; + if(out_std_mesages_){ + std::cout<< " iri_state_machine; diff<0; status=12" << std::endl;} + + this->tibi_akp_status_UInt64_msg_.data =12; }else{ - std::cout<< " iri_state_machine; diff>0; status=11" << std::endl; + if(out_std_mesages_){ + std::cout<< " iri_state_machine; diff>0; status=11" << std::endl;} this->tibi_akp_status_UInt64_msg_.data =11; } } @@ -303,8 +309,9 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void) //this->tibi_akp_status_UInt64_msg_.data = my_var; - if(out_std_mesages_){ + if(out_state_machine_status_mesage_){ //ROS_INFO("STATUS state machine!!! tibi_akp_status_=%d",this->tibi_akp_status_UInt64_msg_.data ); + std::cout<< "STATUS state machine!!! tibi_akp_status_=" << this->tibi_akp_status_UInt64_msg_.data << std::endl; } // [fill srv structure and make request to the server]