diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg index b4fdc7ff9e6dfabcc884e00985672926becbaa00..b40d409e820b607cef96bf1090a185e9e26ac33e 100755 --- a/cfg/IriStateMachineCompanion.cfg +++ b/cfg/IriStateMachineCompanion.cfg @@ -40,6 +40,7 @@ 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) # threshold_max_dist_between_robot_and_person_ +#gen.add("terrinet_project_conf" , bool_t, 0, "if true we omit the state-of-fake-person mode of the group accompaniment.", False) gen.add("frame_map", str_t, 0, "frame to transform poses to", "map") gen.add("frame_robot_footprint", str_t, 0, "frame to transform poses to", "tiago/base_link") gen.add("robot", str_t, 0, "frame to transform poses to", "tiago") diff --git a/include/iri_state_machine_companion_alg_node.h b/include/iri_state_machine_companion_alg_node.h index 49cc748b8bb612099ff22b9088d4dfae8d574a83..9ace8c18f0cb33390d8876903bb2e6564ac6f4a6 100644 --- a/include/iri_state_machine_companion_alg_node.h +++ b/include/iri_state_machine_companion_alg_node.h @@ -104,6 +104,10 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm< bool out_std_mesages_; bool bool_test_case_head1_; + + //differenciate states of group accompaniment and terrined project of 1 person accompaniment + //bool terrinet_project_; + // [service attributes] // [client attributes] diff --git a/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp index ddd822866b0cba80da658a568bd8a776af2722dd..c0ea24547fc6a8e202b8bda35484359a422ec88a 100644 --- a/src/iri_state_machine_companion_alg_node.cpp +++ b/src/iri_state_machine_companion_alg_node.cpp @@ -94,7 +94,8 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void) this->tibi_akp_status_UInt64_msg_.data =0; // case no status. out_std_mesages_=config_.out_std_mesages_conf; - + + //terrinet_project_=config_.terrinet_project_conf; //differenciate states of group accompaniment and terrined project of 1 person accompaniment use_x_=config_.use_X_bool_conf; @@ -289,20 +290,22 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void) } // TODO status fake person ==2 and status return other goal ==3 - - if(actual_numCompanionPeople==1){ - this->tibi_akp_status_UInt64_msg_.data =2; // status no people. - if(out_std_mesages_){ - ROS_INFO(" IN Fake person status (2) "); + //if(!terrinet_project_){ + if(actual_numCompanionPeople==1){ + this->tibi_akp_status_UInt64_msg_.data =2; // status no people. + if(out_std_mesages_){ + ROS_INFO(" IN Fake person status (2) "); + } } - } + //} // INI: implementar status return other goal ==3 //this->tibi_akp_status_UInt64_msg_.data = my_var; if(out_std_mesages_){ - ROS_INFO("STATUS state machine!!! tibi_akp_status_=%d",this->tibi_akp_status_UInt64_msg_.data ); + //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]