diff --git a/cfg/AkpLocalPlanner.cfg b/cfg/AkpLocalPlanner.cfg
index 2c5216b5137de6e056ec94e846eaa54d5544473b..da7269ab7765d4c58a2624323d1cbbec81b4a25f 100755
--- a/cfg/AkpLocalPlanner.cfg
+++ b/cfg/AkpLocalPlanner.cfg
@@ -43,7 +43,7 @@ gen.add("conf_debug_few_IVO", bool_t, 0, " Only few velocity messages
gen.add("Block_parameters_division2" , str_t, 0, "To Stop the robot manually: ", "To Stop the robot manually:")
gen.add("stop_robot_manually_conf", bool_t, 0, " To stop the robot manually.", False)
-
+gen.add("conf_max_obs_to_consider_narrow_passage", int_t, 0, "number static obstacles near the robot to consider that we are in a narrow pasage case", 10, 1, 10000)
group1 = gen.add_group("a_type_of_accompaniment", state = True, type="tab")
group1.add("Block_parameters_division1" , str_t, 0, "Parameters to start the simulation:","Parameters to start the simulation:")
diff --git a/include/akp_local_planner_alg_node.h b/include/akp_local_planner_alg_node.h
index 220f1c9a7aaa824633a73431ace4af573980c34e..3bb1185a17410b06f66fecf7d87903bb69383dcf 100644
--- a/include/akp_local_planner_alg_node.h
+++ b/include/akp_local_planner_alg_node.h
@@ -526,6 +526,7 @@ class AkpLocalPlanner : public nav_core::BaseLocalPlanner
bool debug_all_IVO_;
bool debug_few_IVO_;
+ unsigned int max_obs_to_consider_narrow_passage_;
public:
/**
diff --git a/local_lib_people_prediction/src/nav/plan_local_nav.cpp b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
index 42dfbbe79f9c5bbcb2b001073804100dbcb6f496..c77da9a0e22354efc32bf5dd0545761a30fbab11 100644
--- a/local_lib_people_prediction/src/nav/plan_local_nav.cpp
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
@@ -2139,6 +2139,7 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
obstacles_start=clock();
int number_of_obstacles(0);
int number_of_obstacles2(0);
+ unsigned int number_of_obstacles_state_machine(0);
double dist_between_obstacles;
Spoint before_obstacle;
bool first_case=true;
@@ -2219,6 +2220,7 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
Spoint robot_ac=robot_->get_current_pointV();
if(((iit.distance( before_obstacle )>dist_between_obstacles)&&(!first_case))&&(iit.distance(robot_ac)<max_distance_to_obstacles_detected_)){
number_of_obstacles2++;
+ number_of_obstacles_state_machine=number_of_obstacles_state_machine+1;
}
before_obstacle=iit;
first_case=false;
@@ -2234,6 +2236,22 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
}
number_of_obstacles_=number_of_obstacles;
+ num_obs_to_state_machine_=number_of_obstacles_state_machine;
+ if(num_obs_to_state_machine_>0){
+ evitando_obst_estaticos_=true;
+ }else{
+ evitando_obst_estaticos_=false;
+ }
+ /*if(num_obs_to_state_machine_>max_num_obst_to_conider_passage_for_state_machine_){
+ evitando_obst_estaticos_=true;
+ }else{
+ evitando_obst_estaticos_=false;
+ }*/
+ std::cout << " max_distance_to_obstacles_detected_= "<< max_distance_to_obstacles_detected_ <<"laser_obstacle_list_.empty()="<<laser_obstacle_list_.empty()<< std::endl;
+ std::cout << " num_obs_to_state_machine_="<<num_obs_to_state_machine_<<"number_of_obstacles_state_machine"<<number_of_obstacles_state_machine<<"evitando_obst_estaticos_="<<evitando_obst_estaticos_<< std::endl;
+
+
+
obstacles_end=clock();
// std::cout << " (ROBOT COMPANION) number_of_obstacles= "<< number_of_obstacles_ <<"; dist_between_obstacles="<<dist_between_obstacles<<"; nearby_obstacle_list_.size()="<<nearby_obstacle_list_.size()<< std::endl;
if(check_execution_times_){
@@ -2554,15 +2572,29 @@ bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
/**** Parte de gonzalo: depending on the number of persons considered, set the std of the std_goal_workspace [rad] ***/
int obstacles=number_of_obstacles2;
+ unsigned int num_nearest_people=0;
for( auto iit: nearby_person_list_ )
{
Spoint robot_point=robot_->get_current_pointV();
Spoint act_person=iit->get_current_pointV();
- //std::cout <<"; id_person="<<iit->get_id()<< " robot_point.distance(act_person)="<<robot_point.distance(act_person)<<"; max_distance_to_obstacles_detected_="<<max_distance_to_obstacles_detected_<< std::endl;
+ std::cout <<"; id_person="<<iit->get_id()<< " robot_point.distance(act_person)="<<robot_point.distance(act_person)<<"; max_distance_to_obstacles_detected_="<<max_distance_to_obstacles_detected_<< std::endl;
if(robot_point.distance(act_person)<max_distance_to_obstacles_detected_){
obstacles=obstacles+1;
+ num_nearest_people=num_nearest_people+1;
}
}
+ num_nearest_people=num_nearest_people-1; //rest the accompanied person.
+ num_avoid_people_to_state_machine_=num_nearest_people;
+
+ if(num_avoid_people_to_state_machine_>0){
+ evitando_obst_dynamicos_=true;
+ }else{
+ evitando_obst_dynamicos_=false;
+ }
+ std::cout << " nearby_person_list_.empty()="<<nearby_person_list_.empty()<<"nearby_person_list_.size()="<<nearby_person_list_.size()<<"person_list_.size()="<<person_list_.size() << std::endl;
+
+ std::cout << " num_avoid_people_to_state_machine_="<<num_avoid_people_to_state_machine_<<"num_nearest_people"<<num_nearest_people<<"evitando_obst_dynamicos_="<<evitando_obst_dynamicos_<< std::endl;
+
// remove the accompanied people from the conter of obstacles, for the std of the random goals of the local window, because the std-enlarges-with-the-number-of-obstacles.
if(we_have_pointer_to_first_person_){ // the robot do not take into account the accompanied people to the std_value for aboid obstacles.
diff --git a/local_lib_people_prediction/src/nav/plan_local_nav.h b/local_lib_people_prediction/src/nav/plan_local_nav.h
index b9480ab2a69d80f9830d68166f56d209286fc0e1..32c09d67eb61c21c9af6d7c477cad4249c071254 100644
--- a/local_lib_people_prediction/src/nav/plan_local_nav.h
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.h
@@ -256,7 +256,7 @@ class Cplan_local_nav : public Cprediction_behavior
void set_max_d_to_detect_laser_obs(double in_max_d_to_detect_laser_obs){
Cprediction_behaviour_set_max_d_to_detect_laser_obs(in_max_d_to_detect_laser_obs);
max_distance_to_obstacles_detected_=in_max_d_to_detect_laser_obs;
- std::cout << "set_max_d_to_detect_laser_obs-> in_max_d_to_detect_laser_obs="<<in_max_d_to_detect_laser_obs<< std::endl;
+ std::cout << "set_max_d_to_detect_laser_obs-> in_max_d_to_detect_laser_obs="<<in_max_d_to_detect_laser_obs<<"max_distance_to_obstacles_detected_="<<max_distance_to_obstacles_detected_<< std::endl;
}
void set_save_results_in_file(bool in_save_results_in_file){
@@ -580,6 +580,29 @@ class Cplan_local_nav : public Cprediction_behavior
return obstacle_very_close_no_path_;
}
+ bool evitando_obst_estaticos_;
+ bool get_bool_evitando_obst_estaticos(){
+ return evitando_obst_estaticos_;
+ }
+ bool evitando_obst_dynamicos_;
+ bool get_bool_evitando_obst_dynamicos(){
+ return evitando_obst_dynamicos_;
+ }
+ unsigned int num_obs_to_state_machine_;
+ unsigned int get_save_num_obs_to_state_machine(){
+ return num_obs_to_state_machine_;
+ }
+ unsigned int num_avoid_people_to_state_machine_;
+ unsigned int get_save_num_avoid_people_to_state_machine(){
+ // used max_distance_to_obstacles_detected_ to take into account the nearest people to be avoided. By default I think that is set up to 3 meters.
+ // If you need to differentiate between the nearest obstacles detected and the nearest people to be avoided (state machine state),
+ //please, create a new variable for the state-machine people avoided
+ return num_avoid_people_to_state_machine_;
+ }
+ unsigned int max_num_obst_to_conider_passage_for_state_machine_;
+ void set_max_num_obst_to_conider_passage_for_state_machine(unsigned int in_max_num_obst_to_conider_passage_for_state_machine){
+ max_num_obst_to_conider_passage_for_state_machine_=in_max_num_obst_to_conider_passage_for_state_machine;
+ }
void set_threshold_collision_dist_stop_pers_goal(double in_threshold_collision_dist_stop_pers_goal){
threshold_collision_dist_stop_pers_goal_=in_threshold_collision_dist_stop_pers_goal;
diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index dd0aecbe42c85fc7f760b90cc7a6e4617b33487d..64854e971100962fc37bfc7f8e5a74ee6b579c56 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -66,6 +66,7 @@ AkpLocalPlanner::AkpLocalPlanner(void) :
simulation_(false),
debug_all_IVO_(false),
debug_few_IVO_(false),
+ max_obs_to_consider_narrow_passage_(10),
see_std_out_velocities_ros_(false),
see_std_out_mesages_ros_(false)
{
@@ -653,6 +654,9 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
actual_person_comp1_pose=this->obs[u];
}
+ }else{ //include also, other people no companions:
+ this->obs2.push_back(this->obs[u]);
+
}
}
@@ -1169,10 +1173,26 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
actual_dest_pers_robot.position.z=0.0;
companionState_companionStateResults_msg_.destinations[0]=actual_dest_pers_robot;
// estas dos siguientes era mejor no usarlas y no decir nada. Hubiera sido un incordio y ya se veia que estaba evitando obstaculos.
- companionState_companionStateResults_msg_.estoyEvitandoObstaculosEstaticos=false; //this->planner_.get_bool_evitando_obst_estaticos();
- companionState_companionStateResults_msg_.estoyEvitandoObstaculosDynamicos=false;//this->planner_.get_bool_evitando_obst_dynamicos();
+
companionState_companionStateResults_msg_.estateNoPathObstacleClose=this->planner_.get_obstacle_very_close_no_path();
+ companionState_companionStateResults_msg_.estoyEvitandoObstaculosEstaticos=this->planner_.get_bool_evitando_obst_estaticos(); //=false;
+ companionState_companionStateResults_msg_.estoyEvitandoObstaculosDynamicos=this->planner_.get_bool_evitando_obst_dynamicos();
+
+ // num obstaculos a menos de 3 m!??? (incluir threshold_distance_to_consider_avoid_obstacles_
+ companionState_companionStateResults_msg_.numAvoidedObstacles=this->planner_.get_save_num_obs_to_state_machine();
+ companionState_companionStateResults_msg_.numAvoidedPeople=this->planner_.get_save_num_avoid_people_to_state_machine();
+
+ unsigned int SaveNumObsToStateMachine=this->planner_.get_save_num_obs_to_state_machine();
+ if(SaveNumObsToStateMachine>max_obs_to_consider_narrow_passage_)
+ {
+ companionState_companionStateResults_msg_.MaybeNarrowPassage=true;
+ }else{
+ companionState_companionStateResults_msg_.MaybeNarrowPassage=false;
+ }
+
+
+
bool robot_in_lat=true;
robot_in_lat=true;
@@ -1535,7 +1555,9 @@ void AkpLocalPlanner::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
void AkpLocalPlanner::tracks_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg)
{
- //ROS_INFO("AkpLocalPlanner::tracks_callback: New Message Received");
+ ROS_INFO("AkpLocalPlanner::tracks_callback: New Message Received");
+ ROS_ERROR("AkpLocalPlanner::tracks_callback: tracks are in %s frame instead of %s", msg->header.frame_id.c_str(), this->fixed_frame.c_str());
+
if(msg->header.frame_id == this->fixed_frame)
{
@@ -1870,6 +1892,9 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
this->planner_.set_evaluate_costs_filename(config.evaluate_costs_filename);
this->planner_.set_evaluate_change_distance_and_angle_companion_filename(config.evaluate_change_distance_and_angle_companion_filename);
+ max_obs_to_consider_narrow_passage_=config_.conf_max_obs_to_consider_narrow_passage;
+
+ this->planner_.set_max_d_to_detect_laser_obs(config.detection_laser_obstacle_distances);
if(config.see_config_set_up){
std::cout << std::endl<< std::endl<< std::endl;