From 4793a9b7c1672e573e01f47d4d4554a72fce60d0 Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Tue, 2 Apr 2019 17:25:51 +0000
Subject: [PATCH] mejorado evitar obstaculos estaticos y dinamicos + add un
nuevo estado en el state machine + que diga iterativamente que no tiene
gente. Ese ultimo a adaptar el rato que tarda en volver a decirlo
---
cfg/IriStateMachineCompanion.cfg | 4 +++-
.../iri_state_machine_companion_alg_node.h | 3 +--
src/iri_state_machine_companion_alg_node.cpp | 21 +++++++++++++++++++
3 files changed, 25 insertions(+), 3 deletions(-)
diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg
index 87364bd..02bdaf6 100755
--- a/cfg/IriStateMachineCompanion.cfg
+++ b/cfg/IriStateMachineCompanion.cfg
@@ -41,6 +41,8 @@ gen = ParameterGenerator()
#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)", 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)
+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.0, 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)
+
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 3a4e8b0..7f6eae1 100644
--- a/include/iri_state_machine_companion_alg_node.h
+++ b/include/iri_state_machine_companion_alg_node.h
@@ -95,8 +95,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
void people_tracks_mutex_exit(void);
unsigned int id_person_companion_, id_second_person_companion_;
- double threshold_max_dist_between_robot_and_person_;
- double threshold_less_max_vel_robot_;
+ double threshold_max_dist_between_robot_and_person_,threshold_less_max_vel_robot_,threshold_minimun_distance_not_collide_;
// [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 7732ac6..f1499b3 100644
--- a/src/iri_state_machine_companion_alg_node.cpp
+++ b/src/iri_state_machine_companion_alg_node.cpp
@@ -122,12 +122,22 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
// TODO: status side-by-side. ==6
double person_companion_velocity=0.0;
double distance_robot_person_comp;
+
+ double minimum_distance=10;
for(unsigned int u=0;u<this->obs.size();u++){
+ if((u==0)&&(this->obs[u].id!=id_person_companion_)){
+ minimum_distance=sqrt((this->obs[u].x - this->robot_pose_.x)*(this->obs[u].x -this->robot_pose_.x)+(this->obs[u].y - this->robot_pose_.y)*(this->obs[u].y -this->robot_pose_.y));
+ }
if(this->obs[u].id==id_person_companion_){
person_companion_velocity=sqrt(this->obs[u].vx*this->obs[u].vx+this->obs[u].vy*this->obs[u].vy);
distance_robot_person_comp=sqrt((this->obs[u].x - this->robot_pose_.x)*(this->obs[u].x -this->robot_pose_.x)+(this->obs[u].y - this->robot_pose_.y)*(this->obs[u].y -this->robot_pose_.y));
}
//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;
+
+ double actual_per_rob_dist=sqrt((this->obs[u].x - this->robot_pose_.x)*(this->obs[u].x -this->robot_pose_.x)+(this->obs[u].y - this->robot_pose_.y)*(this->obs[u].y -this->robot_pose_.y));
+ if((actual_per_rob_dist<minimum_distance)&&(this->obs[u].id!=id_person_companion_)){ // la person companion no cuenta. Me faltará caso obst staticos...
+ minimum_distance=actual_per_rob_dist;
+ }
}
//ROS_INFO("save_companionStatus_mesage_global_.maximumRobotVelocity=%f; person_companion_velocity=%f",save_companionStatus_mesage_global_.maximumRobotVelocity,person_companion_velocity);
@@ -145,6 +155,17 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
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.
}
+
+ threshold_minimun_distance_not_collide_=config_.threshold_minimun_distance_not_collide_conf;
+ // new status: I can not move!, Sorry.
+ /*if((minimum_distance < threshold_minimun_distance_not_collide_)&&(this->robot_pose_.v<0.2)){
+ this->tibi_akp_status_UInt64_msg_.data =9; //obstacle very close.
+ }*/
+
+ if(save_companionStatus_mesage_global_.estateNoPathObstacleClose){
+ ROS_INFO("NO PATH STATUS save_companionStatus_mesage_global_.estateNoPathObstacleClose=%d",save_companionStatus_mesage_global_.estateNoPathObstacleClose);
+ this->tibi_akp_status_UInt64_msg_.data =9; //obstacle very close.
+ }
//this->tibi_akp_status_UInt64_msg_.data = my_var;
--
GitLab