diff --git a/cfg/AkpLocalPlanner.cfg b/cfg/AkpLocalPlanner.cfg
index 1f02fe3cc4f353775634c35073a8975f400cb761..f63df1c0ec9eeed6709aa90ee923470f02c67a9d 100755
--- a/cfg/AkpLocalPlanner.cfg
+++ b/cfg/AkpLocalPlanner.cfg
@@ -34,11 +34,7 @@ PACKAGE='iri_robot_aspsi'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
-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", "ana/base_link")
-gen.add("fuera_bolitas_goals_companion_markers_conf", bool_t, 0, " Fuera bolas markers", True)
-gen.add("conf_debug_all_IVO", bool_t, 0, " All mesagenes to debug IVO", True)
-gen.add("conf_debug_few_IVO", bool_t, 0, " Only few velocity messages to debug IVO", False)
+
#### Parameters to start the simulation ####
gen.add("Block_parameters_division1" , str_t, 0, "Parameters to start the simulation:","Parameters to start the simulation:")
@@ -55,21 +51,11 @@ gen.add("launch_goal" , bool_t, 0, "if True => send goal", False)
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)
-#### Parameters to visualize markers and output messages ####
-gen.add("Block_parameters_division3" , str_t, 0, "Parameters to visualize markers and output messages: ", "Parameters to visualize markers and output messages: ")
+#### Parameters to visualize markers and output messages TODO: intentar quitar estos, no se usa. EXCEPTO vis_mode. ####
+gen.add("Block_parameters_division3" , str_t, 0, "Parameters to visualize markers of ASPSI: ", "Parameters to visualize markers of ASPSI: ")
# visualization markers
gen.add("vis_mode", int_t, 0, "Visualization markers: #0 no markers, #1 min, #2 normal(2d), #3 super (3d), #4 SUPER!", 4, 0, 7)
-# minimum output screen messages.
-gen.add("output_screen_messages" , bool_t, 0, "if True => publishes the screen messages of velocity and other few things.", False)
-gen.add("conf_see_save_in_file", bool_t, 0, " bool to see save in file", False)
-gen.add("see_std_out_velocities", bool_t, 0, " true to see std_out mesages", False)
-gen.add("see_std_out_mesages", bool_t, 0, " true to see std_out mesages", False)
-#### SFM habilitate force markers ####
-gen.add("robot_goal_force_marker" , bool_t, 0, "enable visualization of robot goal force marker", True)
-gen.add("robot_person_forces_marker" , bool_t, 0, "enable visualization of robot person forces marker", True)
-gen.add("robot_obstacles_forces_marker", bool_t, 0, "enable visualization of robot obstacles forces marker", True)
-gen.add("robot_resultant_force_marker" , bool_t, 0, "enable visualization of robot resultan force marker", True)
-gen.add("robot_companion_force_marker" , bool_t, 0, "enable visualization of robot resultan force marker", True)
+
#### Parameters to adapt the robot the accompaniment, distance between people and angle of accompaniment ####
@@ -125,9 +111,9 @@ gen.add("distance_to_dynamic_goal_Vform", double_t, 0, "distance fr
gen.add("Block_parameters_division7" , str_t, 0, "Parameters to reduce the computational load due to obstacles:", "Parameters to reduce the computational load due to obstacles:")
# distance arround the robot to detect obstacles and do not saturate the node.
gen.add("detection_laser_obstacle_distances", double_t, 0, " distance to detect the laser obstacles (reduce the computaional time)", 4, 0.0, 20.0)
-# proximity distance between people and robot
-# circle arround the robot to detect dynamic obstacles (other people, not companions).
-gen.add("radi_to_detect_other_people_no_companions", double_t, 0, " distancia entre robot y la primera persona que acompana. para pararse en el sitio cuando las personas se paran", 4, 0.0, 50.0)
+
+
+
#### Parameters to adjust the robot's accelerations and velocities. Can be different for different robots. ####
@@ -144,11 +130,16 @@ gen.add("av_break", double_t, 0, "robot minimum acceleration (break
gen.add("aw_max", double_t, 0, "robot maximum angular acceleration", 0.9, 0.0, 5.0)
+gen.add("Block_parameters_divisionTest" , str_t, 0, "Parameters to test good companion and outside velocities + frames", "Parameters to test good companion and outside velocities + frames:")
+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", "ana/base_link")
+gen.add("fuera_bolitas_goals_companion_markers_conf", bool_t, 0, " Fuera bolas markers", True)
+gen.add("conf_debug_all_IVO", bool_t, 0, " All mesagenes to debug IVO", True)
+gen.add("conf_debug_few_IVO", bool_t, 0, " Only few velocity messages to debug IVO", False)
+gen.add("external_goal", bool_t, 0, "Set a external goal or not (If you want to fix the robot goal manually in r-viz)", False) # esta puede hacerles valta
-#### Other needed variables that normally we do not have to change it.
-gen.add("Block_parameters_division9" , str_t, 0, "Other parameters that are correctly set-up for all sistems, but it is better to keep here:", "Other parameters that are correctly set-up for all sistems, but it is better to keep here:")
-gen.add("move_base", bool_t, 0, "disabled filters cmd_vel and sends zeros", True) # es para parar al robot.
-gen.add("frozen_mode", bool_t, 0, "Visualization freezed", False)
+#### Other needed variables that normally we do not have to change it. (TODO: ESTOS INTENTAR QUITARLOS)
+gen.add("Block_parameters_division9" , str_t, 0, "Other parameters that are correctly set-up for all sistems, but I have not had time to include only internal ones or they are inherited and difficult to remove:", "Other parameters that are correctly set-up for all sistems, but I have not had time to include only internal ones or they are inherited and difficult to remove:")
gen.add("plan_mode", int_t, 0, "#0-F_RRT_Uniform, #1-F_RRT_Gauss_Circle, #2-F_RRT_Gauss_Circle_alpha", 1, 0, 2)
gen.add("distance_mode", int_t, 0, "#0-Euclidean, #1-cost-to-go-erf, #2-c2g-norm, #3-c2g-raw. It is recomnded not to use *erf/*norm methods and not using also *erf/*norm in the global mode, except for c2g-raw", 1, 0, 3)
gen.add("global_mode", int_t, 0, "Designed to be paired with the distance_mode, although it may use a different global mode:#0-Scaliarization, #1-Weighted-sum-erf, #2-Weighted-sum-norm, #3-MO_erf, #4-MO_norm", 1, 0, 4)
@@ -175,7 +166,7 @@ gen.add("cost_l_minima", double_t,0, "local minima scape cost paramter",
#### dificiles/largas de quitar.
-gen.add("external_goal", bool_t, 0, "Set a external goal or not", False)
+
gen.add("in_max_asos_point_to_person", int_t, 0, "detector de obstaculos maximo numero de puntos associados a persona", 100, 0, 1000)
gen.add("set_initial_v_robot_needed_for_odom", double_t, 0, " set_initial_v_robot_needed_for_odom", 0.17 , 0.0, 30.0)
# change time vindow to calculate the people velocity (propagations)
@@ -190,6 +181,10 @@ gen.add("conf_final_max_v", double_t, 0, "Is minimun velocity to change the
#gen.add("use_default_PS3_button", bool_t, 0, " False to set out PS3 Up and down buttons", True)
#gen.add("joy_watchdog_time", double_t, 0, "Maximum time (seconds) between joy msgs", 1.0, 0.1, 3.0)
+
+###PARAMETROS A ELIMINAR
+
+
# end of SFM companion params
exit(gen.generate(PACKAGE, "AkpLocalPlannerAlgorithm", "AkpLocalPlanner"))
diff --git a/config/IVO/akp_local_planner_params.yaml b/config/IVO/akp_local_planner_params.yaml
index 0ae23f78852e16c286016270dd66bc22dc239081..5034e42d5c1a0ea95040189873c96a74e0bcd001 100644
--- a/config/IVO/akp_local_planner_params.yaml
+++ b/config/IVO/akp_local_planner_params.yaml
@@ -38,7 +38,7 @@ AkpLocalPlanner:
esfm_to_obstacle_lambda: 1.0
esfm_k: 2.3
esfm_d: 0.2
- min_v_to_predict : 0.1
+ min_v_to_predict : 0.2
ppl_collision_mode : 0
pr_force_mode : 0
goal_providing_mode : 1
diff --git a/config/ana2/akp_local_planner_params.yaml b/config/ana2/akp_local_planner_params.yaml
index 758cf17abeb4c97392993ed4fd0e94bd8fc7b9c8..bb0892cca7af46e52077b9fa8cd41a038e7cc787 100644
--- a/config/ana2/akp_local_planner_params.yaml
+++ b/config/ana2/akp_local_planner_params.yaml
@@ -38,7 +38,7 @@ AkpLocalPlanner:
esfm_to_obstacle_lambda: 1.0
esfm_k: 2.3
esfm_d: 0.2
- min_v_to_predict : 0.1
+ min_v_to_predict : 0.2
ppl_collision_mode : 0
pr_force_mode : 0
goal_providing_mode : 1
diff --git a/config/dabo/akp_local_planner_params.yaml b/config/dabo/akp_local_planner_params.yaml
index 5e477a9b3a974c72b11f447b4464300e7cd908e5..52b966c67fc3881ec79a5863d9938864b288f026 100644
--- a/config/dabo/akp_local_planner_params.yaml
+++ b/config/dabo/akp_local_planner_params.yaml
@@ -38,7 +38,7 @@ AkpLocalPlanner:
esfm_to_obstacle_lambda: 1.0
esfm_k: 2.3
esfm_d: 0.2
- min_v_to_predict : 0.1
+ min_v_to_predict : 0.2
ppl_collision_mode : 0
pr_force_mode : 0
goal_providing_mode : 1
diff --git a/config/tibi/akp_local_planner_params.yaml b/config/tibi/akp_local_planner_params.yaml
index 5e477a9b3a974c72b11f447b4464300e7cd908e5..52b966c67fc3881ec79a5863d9938864b288f026 100644
--- a/config/tibi/akp_local_planner_params.yaml
+++ b/config/tibi/akp_local_planner_params.yaml
@@ -38,7 +38,7 @@ AkpLocalPlanner:
esfm_to_obstacle_lambda: 1.0
esfm_k: 2.3
esfm_d: 0.2
- min_v_to_predict : 0.1
+ min_v_to_predict : 0.2
ppl_collision_mode : 0
pr_force_mode : 0
goal_providing_mode : 1
diff --git a/include/akp_local_planner_alg_node.h b/include/akp_local_planner_alg_node.h
index a6a13a73fd29c651baaafd2cef03beac502e15af..220f1c9a7aaa824633a73431ace4af573980c34e 100644
--- a/include/akp_local_planner_alg_node.h
+++ b/include/akp_local_planner_alg_node.h
@@ -372,8 +372,7 @@ class AkpLocalPlanner : public nav_core::BaseLocalPlanner
bool test_with_2people_but_tibi_really_only_with_me_;
Cperson_abstract::companion_reactive reactive_;
- // ely visualization bool variables (see forces)
- bool robot_goal_force_marker, robot_person_forces_marker, robot_obstacles_forces_marker, robot_resultant_force_marker, robot_companion_force_marker;
+
bool debug_antes_subgoals_entre_AKP_goals_;
//tf::TransformListener* tf_listener2_;
@@ -512,7 +511,6 @@ class AkpLocalPlanner : public nav_core::BaseLocalPlanner
double v_max_due_to_people_companion_; //este limite lo hago en el nodo directamente, ya que es más restrictivo que el que hago interno de la V_max seada como valor maximo.
SpointV person_companion_position_, second_person_companion_position_;
- double radi_other_people_detection_;
double dist_betw_rob_and_comp_people_to_slow_velocity_;
double ros_max_real_speed_out_;
diff --git a/iri_robot_aspsi_how_to/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt b/iri_robot_aspsi_how_to/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt
index c1135e42355176542bafb01692e34786c73aa7b0..ca594e4c1b0afaeff321058cbc53cf8500b0fa03 100644
Binary files a/iri_robot_aspsi_how_to/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt and b/iri_robot_aspsi_how_to/instructions_for_IVO/Install_instructions_IVO_ONLY_one_person_ACCOMPANIMENT_ASPSI.odt differ
diff --git a/iri_robot_aspsi_how_to/instructions_for_IVO/Tutorial_capabilities/Tutorial_Capabilities.odt b/iri_robot_aspsi_how_to/instructions_for_IVO/Tutorial_capabilities/Tutorial_Capabilities.odt
index 01369eba69a0a6aa533c411429030569b09871bc..f6a40954b269ac6da88c48ffc0aefc08838467ae 100644
Binary files a/iri_robot_aspsi_how_to/instructions_for_IVO/Tutorial_capabilities/Tutorial_Capabilities.odt and b/iri_robot_aspsi_how_to/instructions_for_IVO/Tutorial_capabilities/Tutorial_Capabilities.odt differ
diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index db9b824e5fce498cf042ed62b7c5ac6b3bb1186c..ca695222d9cf7ff07a04b8c2b9f3be19ab0ca753 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -52,7 +52,6 @@ AkpLocalPlanner::AkpLocalPlanner(void) :
// change_ps3_config_(false),
in_set_planner_dt_(0.2),
//use_default_PS3_button_(true),
- radi_other_people_detection_(10),
dist_betw_rob_and_comp_people_to_slow_velocity_(2.0),
speed_k_(1),
stop_robot_manually_(true),
@@ -66,7 +65,9 @@ AkpLocalPlanner::AkpLocalPlanner(void) :
output_screen_messages_(false),
simulation_(false),
debug_all_IVO_(false),
- debug_few_IVO_(false)
+ debug_few_IVO_(false),
+ see_std_out_velocities_ros_(false),
+ see_std_out_mesages_ros_(false)
{
ROS_INFO("CREATE ALG NODE");
@@ -463,6 +464,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
} //conf_dist_betw_rob_and_comp_people_to_slow_velocity
//ROS_INFO("simulation_=%d; ",simulation_);
+ //ROS_INFO("plan_mode=%d; distance_mode=%d; global_mode=%d; min_v_to_predict=%f; ppl_collision_mode=%d; pr_force_mode=%d ",plan_mode,distance_mode,global_mode,min_v_to_predict,ppl_collision_mode,pr_force_mode);
@@ -651,12 +653,6 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
actual_person_comp1_pose=this->obs[u];
}
- }else if(this->robot_pose_.distance(actual_obs)<radi_other_people_detection_){ // coger a las demas personas, que esten a distancia menor que 10 m del robot.
- this->obs2.push_back(this->obs[u]);
-
- if(see_std_out_mesages_ros_){
- ROS_INFO( "TRACKS EN AKP (Other people) => obs.id=%d; this->obs[%d].vx=%f; .vy=%f; x=%f; .y=%f; ",this->obs[u].id,u,this->obs[u].vx,this->obs[u].vy, this->obs[u].x,this->obs[u].y);
- }
}
}
@@ -1648,20 +1644,11 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
//ROS_INFO(" INI AkpLocalPlanner::reconfigureCallback (if !setup_)");
this->config_=config;
- this->robot_goal_force_marker = config.robot_goal_force_marker;
- this->robot_person_forces_marker = config.robot_person_forces_marker;
- this->robot_obstacles_forces_marker = config.robot_obstacles_forces_marker;
- this->robot_resultant_force_marker = config.robot_resultant_force_marker;
- this->config_=config;
+
default_config_ = config;
setup_ = true;
// change (ely) para puentear lo de move_base a true! (que me deje dar goals desde el principio!)
- this->move_base = config.move_base;
-
- if(debug_antes_subgoals_entre_AKP_goals_){
- std::cout << " AkpLocalPlanner::reconfigureCallback this->move_base ="<< this->move_base << std::endl;
- }
this->plan_mode_ = (Cplan_local_nav::plan_mode) config.plan_mode;
this->planner_.set_planning_mode(plan_mode_);
this->planner_.set_distance_mode((Cplan_local_nav::distance_mode)config.distance_mode );
@@ -1680,7 +1667,6 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
config.cost_l_minima);
this->planner_.set_robot_params(config.v_max, config.w_max, config.av_max, config.av_break, config.aw_max, config.platform_radii, config.av_max_negativa, config.av_max_VrobotZero, config.lim_VrobotZero);
this->vis_mode_ = config.vis_mode;
- this->frozen_mode_ = config.frozen_mode;
this->planner_.set_min_v_to_predict( config.min_v_to_predict );//Cscene_bhmip
this->planner_.set_ppl_collision_mode( config.ppl_collision_mode );
this->planner_.set_pr_force_mode( config.pr_force_mode );
@@ -1691,8 +1677,6 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
{
//ROS_INFO(" INI AkpLocalPlanner::reconfigureCallback (if setup_)");
- this->move_base = config.move_base;
-
if(debug_antes_subgoals_entre_AKP_goals_){
std::cout << " AkpLocalPlanner::reconfigureCallback this->move_base ="<< this->move_base << std::endl;
}
@@ -1719,7 +1703,6 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
config.cost_l_minima);
this->planner_.set_robot_params(config.v_max, config.w_max, config.av_max, config.av_break, config.aw_max, config.platform_radii,config.av_max_negativa,config.av_max_VrobotZero, config.lim_VrobotZero);
this->vis_mode_ = config.vis_mode;
- this->frozen_mode_ = config.frozen_mode;
this->planner_.set_min_v_to_predict( config.min_v_to_predict );//Cscene_bhmip
this->planner_.set_ppl_collision_mode( config.ppl_collision_mode );
this->planner_.set_pr_force_mode( config.pr_force_mode );
@@ -1778,7 +1761,6 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
this->planner_.plan_set_initial_v_robot_needed(config.set_initial_v_robot_needed_for_odom);
- radi_other_people_detection_=config.radi_to_detect_other_people_no_companions;
@@ -1868,15 +1850,10 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
this->planner_.set_change_dynamic_of_final_dest(config.in_change_dynamically_final_dest);
- see_std_out_mesages_ros_=config.see_std_out_mesages;
- see_std_out_velocities_ros_=config.see_std_out_velocities;
- this->planner_.set_see_std_out_velocities(config.see_std_out_velocities);
- this->planner_.set_see_save_in_file(config.conf_see_save_in_file);
+
- output_screen_messages_=config.output_screen_messages;
- this->planner_.set_output_screen_messages(config.output_screen_messages);
this->planner_.set_metros_al_dynamic_goal_Vform(config.distance_to_dynamic_goal_Vform);
@@ -2641,8 +2618,7 @@ void AkpLocalPlanner::fill_forces_markers()
//ROS_INFO("(ROS) 3 after get_forces_person_companion ");
//scaled force to goal: ( blue )
- if(this->robot_goal_force_marker)
- {
+
if(debug_antes_subgoals_entre_AKP_goals_){
ROS_INFO("(ROS) robot force goal");
ROS_INFO("(ROS) ros_point_ini.x=%f",ros_point_ini.x);
@@ -2664,11 +2640,9 @@ void AkpLocalPlanner::fill_forces_markers()
force_goal_marker_.id = cont_f;
++cont_f;
MarkerArray_msg_.markers.push_back( force_goal_marker_ );
- }
+
//scaled person interaction force (green)
- if(this->robot_person_forces_marker)
- {
if(debug_antes_subgoals_entre_AKP_goals_){
ROS_INFO("(ROS) robot force person");
ROS_INFO("(ROS) force_int_person.fx=%f",force_int_person.fx);
@@ -2682,11 +2656,9 @@ void AkpLocalPlanner::fill_forces_markers()
force_int_person_marker_.id = cont_f;
++cont_f;
MarkerArray_msg_.markers.push_back( force_int_person_marker_ );
- }
+
//map obstacles interaction force (black)
- if(this->robot_obstacles_forces_marker)
- {
if(debug_antes_subgoals_entre_AKP_goals_){
ROS_INFO("(ROS) robot force obstacles");
}
@@ -2698,11 +2670,9 @@ void AkpLocalPlanner::fill_forces_markers()
force_obstacle_marker_.id = cont_f;
++cont_f;
MarkerArray_msg_.markers.push_back( force_obstacle_marker_ );
- }
+
//weighted resultant force (red)
- if(this->robot_resultant_force_marker)
- {
force_marker_.points[0] = ros_point_ini;
ros_point.x = ros_point_ini.x + 2*force_total.fx;
ros_point.y = ros_point_ini.y + 2*force_total.fy;
@@ -2710,7 +2680,7 @@ void AkpLocalPlanner::fill_forces_markers()
force_marker_.id = cont_f;
++cont_f;
MarkerArray_msg_.markers.push_back( force_marker_ );
- }
+
//scaled force to goal: ( Cian )
force_companion_marker_.points[0]= ros_point_ini;
ros_point.x = ros_point_ini.x + 2*force_companion.fx;