From a5231b5031e2d27f1366eeed16363e2eaab9b124 Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Mon, 17 May 2021 11:10:07 +0000
Subject: [PATCH] intento de juntar state machine para 2 personas y state
machine para 1 persona, mejor usar-las por separado. La de una persona ira en
el docker.
---
cfg/IriStateMachineCompanion.cfg | 1 +
.../iri_state_machine_companion_alg_node.h | 4 ++++
src/iri_state_machine_companion_alg_node.cpp | 19 +++++++++++--------
3 files changed, 16 insertions(+), 8 deletions(-)
diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg
index b4fdc7f..b40d409 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 49cc748..9ace8c1 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 ddd8228..c0ea245 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]
--
GitLab