diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg
index 0d1e1aaef52dc94d9e4299cbe8af87fb029c0658..87364bd173ecc65784e378059cd6ddb12cd90cb4 100755
--- a/cfg/IriStateMachineCompanion.cfg
+++ b/cfg/IriStateMachineCompanion.cfg
@@ -40,7 +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("threshold_max_dist_between_robot_and_person_conf", double_t, 0, "Maximum distance between robot and person to talk (meters)", 5.0, 0.0, 30.0)
-
+gen.add("threshold_max_dist_between_robot_and_person_conf", double_t, 0, "Maximum distance between robot and person to talk (meters)", 3.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)
exit(gen.generate(PACKAGE, "IriStateMachineCompanionAlgorithm", "IriStateMachineCompanion"))
diff --git a/include/iri_state_machine_companion_alg_node.h b/include/iri_state_machine_companion_alg_node.h
index d2cf55303f0cca4142de710d0e173103b13c535b..3a4e8b048d5f078f14e50d91c17f2d3ebeb39ed0 100644
--- a/include/iri_state_machine_companion_alg_node.h
+++ b/include/iri_state_machine_companion_alg_node.h
@@ -96,7 +96,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
unsigned int id_person_companion_, id_second_person_companion_;
double threshold_max_dist_between_robot_and_person_;
-
+ double threshold_less_max_vel_robot_;
// [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 896084ab64f02acb0ba99fc20953220226d9c7f7..7732ac6a2ccf8adb205729d49ef3c708054fe7a5 100644
--- a/src/iri_state_machine_companion_alg_node.cpp
+++ b/src/iri_state_machine_companion_alg_node.cpp
@@ -8,7 +8,7 @@ IriStateMachineCompanionAlgNode::IriStateMachineCompanionAlgNode(void) :
{
//init class attributes if necessary
this->loop_rate_ = 2;//in [Hz] TODO: ver a cuantos ercios ir.
-ROS_INFO("ini node");
+//ROS_INFO("ini node");
this->fixed_frame = "/map";
this->robot_frame = "/tibi/base_link";
@@ -31,7 +31,7 @@ ROS_INFO("ini node");
// [init clients]
// [init action servers]
-ROS_INFO("node created");
+//ROS_INFO("node created");
// [init action clients]
}
@@ -92,30 +92,30 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
unsigned int actual_numCompanionPeople=save_companionStatus_mesage_global_.numCompanionPeople;
- ROS_INFO("save_companionStatus_mesage_global_.numCompanionPeople=%d",save_companionStatus_mesage_global_.numCompanionPeople);
+ //ROS_INFO("save_companionStatus_mesage_global_.numCompanionPeople=%d",save_companionStatus_mesage_global_.numCompanionPeople);
if(actual_numCompanionPeople==0){
this->tibi_akp_status_UInt64_msg_.data =1; // status no people.
}
// TODO status fake person ==2 and status return other goal ==3
bool aboiding_stat_obst = save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos;
- ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos=%d",save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos);
+ //ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos=%d",save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos);
if(aboiding_stat_obst==true){
//this->tibi_akp_status_UInt64_msg_.data =4; // aboiding static obstacles
}
bool aboiding_dyn_obst = save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos;
- ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos=%d",save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos);
+ //ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos=%d",save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos);
if(aboiding_dyn_obst==true){
//this->tibi_akp_status_UInt64_msg_.data =5; // aboiding dynamic obstacles
}
if(!save_companionStatus_mesage_global_.idsCompanionPeople.empty()){
id_person_companion_=save_companionStatus_mesage_global_.idsCompanionPeople[0];
- ROS_INFO("id_person_companion_=%d",id_person_companion_);
+ //ROS_INFO("id_person_companion_=%d",id_person_companion_);
if(save_companionStatus_mesage_global_.idsCompanionPeople.size()>1){
id_second_person_companion_=save_companionStatus_mesage_global_.idsCompanionPeople[1];
- ROS_INFO("id_second_person_companion_=%d",id_second_person_companion_);
+ //ROS_INFO("id_second_person_companion_=%d",id_second_person_companion_);
}
}
@@ -130,14 +130,17 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
//std::cout<< "this->obs; id=" <<this->obs[u].id<<"; x="<<this->obs[u].x<<"; y="<<this->obs[u].y<<"; vx="<<this->obs[u].vx<<"; vy="<<this->obs[u].vy<< std::endl;
}
- ROS_INFO("save_companionStatus_mesage_global_.maximumRobotVelocity=%f; person_companion_velocity=%f",save_companionStatus_mesage_global_.maximumRobotVelocity,person_companion_velocity);
+ //ROS_INFO("save_companionStatus_mesage_global_.maximumRobotVelocity=%f; person_companion_velocity=%f",save_companionStatus_mesage_global_.maximumRobotVelocity,person_companion_velocity);
+ threshold_less_max_vel_robot_=config_.threshold_less_max_vel_robot_conf;
+
double max_robot_velocity=save_companionStatus_mesage_global_.maximumRobotVelocity;
- if(max_robot_velocity < person_companion_velocity){
+ if((max_robot_velocity - threshold_less_max_vel_robot_) < person_companion_velocity){
this->tibi_akp_status_UInt64_msg_.data =7; // status walk slowly, please. I can not follow you.
}
threshold_max_dist_between_robot_and_person_=config_.threshold_max_dist_between_robot_and_person_conf;
- ROS_INFO("threshold_max_dist_between_robot_and_person_=%f; distance_robot_person_comp=%f",threshold_max_dist_between_robot_and_person_,distance_robot_person_comp);
+
+ //ROS_INFO("threshold_max_dist_between_robot_and_person_=%f; distance_robot_person_comp=%f",threshold_max_dist_between_robot_and_person_,distance_robot_person_comp);
if(distance_robot_person_comp > threshold_max_dist_between_robot_and_person_){
this->tibi_akp_status_UInt64_msg_.data =8; //status wait for me, please.
@@ -159,7 +162,7 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
/* [subscriber callbacks] */
void IriStateMachineCompanionAlgNode::companionState_callback(const iri_nav_msgs::companionStateResults::ConstPtr& msg)
{
- ROS_INFO("IriStateMachineCompanionAlgNode::companionState_callback: New Message Received");
+ //ROS_INFO("IriStateMachineCompanionAlgNode::companionState_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
@@ -186,7 +189,7 @@ void IriStateMachineCompanionAlgNode::companionState_mutex_exit(void)
void IriStateMachineCompanionAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
- ROS_INFO("IriStateMachineCompanionAlgNode::odom_callback: New Message Received");
+ //ROS_INFO("IriStateMachineCompanionAlgNode::odom_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
@@ -276,7 +279,7 @@ void IriStateMachineCompanionAlgNode::odom_mutex_exit(void)
void IriStateMachineCompanionAlgNode::people_tracks_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg)
{
- ROS_INFO("IriStateMachineCompanionAlgNode::people_tracks_callback: New Message Received");
+ //ROS_INFO("IriStateMachineCompanionAlgNode::people_tracks_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();