diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg index 690959b0f4918fff361578150820bf810cf472b0..07e40d3752d72edb8d9dd4460e2a146d4d8d0f55 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("out_std_mesages_conf" , bool_t, 0, "if true we se the internal std_out mesages.", 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/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp index 57909853cea7bb69ae6f600f4dc785c63b7844db..0e0fd4a38e1fca71badcc893d8bf19fd5458212f 100644 --- a/src/iri_state_machine_companion_alg_node.cpp +++ b/src/iri_state_machine_companion_alg_node.cpp @@ -91,7 +91,8 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void) this->tibi_akp_status_UInt64_msg_.data =0; // case no status. - + out_std_mesages_=config_.out_std_mesages_conf; + unsigned int actual_numCompanionPeople=save_companionStatus_mesage_global_.numCompanionPeople; //ROS_INFO("save_companionStatus_mesage_global_.numCompanionPeople=%d",save_companionStatus_mesage_global_.numCompanionPeople); if(actual_numCompanionPeople==0){