From 2c163a317bb66f407ad9f96b6ac3d38c84cdbf37 Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Tue, 13 Jul 2021 10:05:53 +0000
Subject: [PATCH] =?UTF-8?q?arreglado=20dynamic=20reconfigure=20con=20pesta?=
=?UTF-8?q?=C3=B1as=3Dsecciones?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
cfg/AkpLocalPlanner.cfg | 221 +++++++++++-------
.../src/nav/plan_local_nav.h | 22 ++
src/akp_local_planner_alg_node.cpp | 127 ++++++++++
3 files changed, 282 insertions(+), 88 deletions(-)
diff --git a/cfg/AkpLocalPlanner.cfg b/cfg/AkpLocalPlanner.cfg
index 866c182..2c5216b 100755
--- a/cfg/AkpLocalPlanner.cfg
+++ b/cfg/AkpLocalPlanner.cfg
@@ -37,148 +37,193 @@ gen = ParameterGenerator()
#### Parameters to start the simulation ####
-gen.add("Block_parameters_division1" , str_t, 0, "Parameters to start the simulation:","Parameters to start the simulation:")
-# select the nearest person like accompanied person
-gen.add("select_near_pers_as_companion_one_person", bool_t, 0, "select the nearest person like accompanied person", False)
-# id's people companions and number of these
-gen.add("id_person_companion", int_t, 0, "Id of the person which is accompanied by the robot", 1, 1, 10000000)
-# num people in group, one person.
-gen.add("number_of_people_in_group", int_t, 0, "number of people inside the group that accompanies the robot", 1, 1, 10000000)
-# to launch a goal from the dynamic-reconfigure.
-gen.add("launch_goal" , bool_t, 0, "if True => send goal", False)
-
##### stop manually the robot. ####
+gen.add("see_config_set_up", bool_t, 0, " To see all the mesages configured with the dynamic reconfigure to view that all is well configured", False)
+gen.add("conf_debug_few_IVO", bool_t, 0, " Only few velocity messages to debug IVO", True)
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)
+
+
+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:")
+# select the nearest person like accompanied person
+group1.add("select_near_pers_as_companion_one_person", bool_t, 0, "select the nearest person like accompanied person", False)
+# id's people companions and number of these
+group1.add("id_person_companion", int_t, 0, "Id of the person which is accompanied by the robot", 1, 1, 10000000)
+# num people in group, one person.
+group1.add("number_of_people_in_group", int_t, 0, "number of people inside the group that accompanies the robot", 1, 1, 10000000)
#### 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: ")
+group1.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)
-
-gen.add("Block_parameters_division50" , str_t, 0, " Iterative Simulation ", " Iterative Simulation ")
-gen.add("change_sim" , bool_t, 0, "FALSE FOR REAL; True for simulation", True)
-gen.add("change_sim_target_per" , bool_t, 0, "FALSE FOR REAL; True for simulation", True)
+group1.add("vis_mode", int_t, 0, "Visualization markers: #0 no markers, #1 min, #2 normal(2d), #3 super (3d), #4 SUPER!", 4, 0, 7)
#### Parameters to adapt the robot the accompaniment, distance between people and angle of accompaniment ####
-gen.add("Block_parameters_division4" , str_t, 0, "Parameters to adapt the accompaniment to the robot-platform (set-up for Tibi): ","Parameters to adapt the accompaniment to the robot-platform (already set-up for Tibi): ")
-# set-up the robot radius
-gen.add("platform_radii", double_t, 0, "robot radii", 0.5, 0.0, 5.0)
+group2 = gen.add_group("b_accompaniment_distance_and_angle", state = True, type="tab")
# set-up the best accompaniment distance
-gen.add("proximity_distance_between_robot_and_person", double_t, 0, "Is the distance between robot and person", 1.5, 0.0, 5.0)
+group2.add("proximity_distance_between_robot_and_person", double_t, 0, "Is the distance between robot and person", 1.5, 0.0, 5.0)
# set-up the best accompaniment angle
-gen.add("real_companion_angle_SideBySide", double_t, 0, "Is the companion angle set for the case without obstacles", 90, 0.0, 180.0)
+group2.add("real_companion_angle_SideBySide", double_t, 0, "Is the companion angle set for the case without obstacles", 90, 0.0, 180.0)
+
+
+#### Parameters to reduce the computational load of the planner of config of the planner limits. (may change in other systems. ####
+group3 = gen.add_group("c_horizon_time_and_vertex", state = True, type="tab")
+group3.add("Block_parameters_division5" , str_t, 0, "Local-planner parameters:", "Local-planner parameters:")
+# horitzon time of the local planner.
+group3.add("horizon_time", double_t, 0, "Horizon time in which people and obstacles are considered for planning purposes", 4.0, 0.1, 40.0)
+# number of vertex for the planned paths
+group3.add("number_vertex", int_t, 0, "Number of total vertexes for planning purposes", 200, 1, 10000)
+# the dt between iterations of the planner. => set up due to the iteration time of the controller.
+group3.add("set_planner_dt", double_t, 0, " planner dt_", 0.2 , 0.0, 30.0)
+
+
+group4 = gen.add_group("d_detection_obstacles", state = True, type="tab")
+#### Parameters to reduce the computational load due to static and dynamic obstacles ####
+group4.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.
+group4.add("detection_laser_obstacle_distances", double_t, 0, " distance to detect the laser obstacles (reduce the computaional time)", 4, 0.0, 20.0)
+
+
+#### Parameters to adjust the robot's accelerations and velocities. Can be different for different robots. ####
+group5 = gen.add_group("e_accelerations_and_velocities", state = True, type="tab")
# set-up the maximum real robot speed
-gen.add("max_real_speed_out", double_t, 0, "maximum robot real velocity", 0.6 , 0.0, 2.0)
+group5.add("max_real_speed_out", double_t, 0, "maximum robot real velocity", 0.6 , 0.0, 2.0)
# set-up the constant of the controller (if your robot velocity controller divides the velocity by 2 or something similar) => Normally, only used in the real robot.
-gen.add("max_real_angular_speed_out", double_t, 0, " limite velocidad maxima de salida del robot", 0.4 , 0.0, 2.0)
-gen.add("speed_k", double_t, 0, " constant multiplying output linear speed", 1.5 , 1.0, 2.0) # For our real robot is needed a speed_k=1.5
-gen.add("speed_k_angular", double_t, 0, " constant multiplying output linear speed", 1.2 , 1.0, 2.0)
+group5.add("max_real_angular_speed_out", double_t, 0, " limite velocidad maxima de salida del robot", 0.4 , 0.0, 2.0)
+group5.add("speed_k", double_t, 0, " constant multiplying output linear speed", 1.5 , 1.0, 2.0) # For our real robot is needed a speed_k=1.5
+group5.add("speed_k_angular", double_t, 0, " constant multiplying output linear speed", 1.2 , 1.0, 2.0)
+
+group5.add("Block_parameters_division8" , str_t, 0, "Parameters to adjust the robot's accelerations and velocities:", "Parameters to adjust the robot's accelerations and velocities:")
+# set max velocities and accelerations for propagate the robot position
+group5.add("v_max", double_t, 0, "robot maximum velocity", 1.2, 0.0, 5.0)
+group5.add("w_max", double_t, 0, "robot maximum angular velocity", 1.0, 0.0, 5.0)
+group5.add("av_max", double_t, 0, "robot maximum acceleration", 0.4, 0.0, 5.0)
+group5.add("av_max_VrobotZero", double_t, 0, "robot maximum acceleration VrobotZero", 0.6, 0.0, 5.0)
+group5.add("lim_VrobotZero", double_t, 0, "robot lim acceleration VrobotZero", 0.1, 0.0, 5.0)
+group5.add("av_max_negativa", double_t, 0, "robot maximum negative acceleration", 0.2, 0.0, 5.0)
+group5.add("av_break", double_t, 0, "robot minimum acceleration (breaking)", 0.4, 0.0, 5.0)
+group5.add("aw_max", double_t, 0, "robot maximum angular acceleration", 0.9, 0.0, 5.0)
+
+
+
+
+
+
+group7 = gen.add_group("g_stop_slowly_the_robot_when_person_stops", state = True, type="tab")
#### Parameters to do a slow stop in the real robot, when accompanies a person and this person stops ####
-gen.add("Block_parameters_division42" , str_t, 0, "Parameters to adapt the robot stop when person stops: ","Parameters to adapt the robot stop when person stops: ")
+group7.add("Block_parameters_division42" , str_t, 0, "Parameters to adapt the robot stop when person stops: ","Parameters to adapt the robot stop when person stops: ")
# for our tibi robot are good these value of parameters: step_decreace_velocity_stop_robot_ (with real robot)=0.05 limit_velocity_stop_robot_ (with real robot)=0.1 => is the limit to let the robot decrease the velocity as much as want, without the slow reduction.
# step_decreace_velocity_stop_robot_conf is the step to reduce slowly the velocity in each iteration when the velocity is below the limit_velocity_stop_robot_conf.
-gen.add("step_decreace_velocity_stop_robot_conf", double_t, 0, " Step to decrease velocity to stop the robot slowly", 0.1 , 0.0, 2.0)
+group7.add("step_decreace_velocity_stop_robot_conf", double_t, 0, " Step to decrease velocity to stop the robot slowly", 0.1 , 0.0, 2.0)
#limit_velocity_stop_robot_conf is the limit to let the robot decrease the velocity as much as want, without the slow reduction.
-gen.add("limit_velocity_stop_robot_conf", double_t, 0, " limit of the velocity to decrease velocity to stop the robot slowly", 0.1 , 0.0, 2.0)
+group7.add("limit_velocity_stop_robot_conf", double_t, 0, " limit of the velocity to decrease velocity to stop the robot slowly", 0.1 , 0.0, 2.0)
-#### Parameters to reduce the computational load of the planner of config of the planner limits. (may change in other systems. ####
-gen.add("Block_parameters_division5" , str_t, 0, "Local-planner parameters:", "Local-planner parameters:")
-# horitzon time of the local planner.
-gen.add("horizon_time", double_t, 0, "Horizon time in which people and obstacles are considered for planning purposes", 4.0, 0.1, 40.0)
-# number of vertex for the planned paths
-gen.add("number_vertex", int_t, 0, "Number of total vertexes for planning purposes", 200, 1, 10000)
-# the dt between iterations of the planner. => set up due to the iteration time of the controller.
-gen.add("set_planner_dt", double_t, 0, " planner dt_", 0.2 , 0.0, 30.0)
+group8 = gen.add_group("h_dynamic_destination", state = True, type="tab")
#### Parameters to adapt the dynamic-final-goals to the environments####
-gen.add("Block_parameters_division6" , str_t, 0, "Parameters to adapt the dynamic-final-goals to the environment:", "Parameters to adapt the dynamic-final-goals to the environment:")
+group8.add("Block_parameters_division6" , str_t, 0, "Parameters to adapt the dynamic-final-goals to the environment:", "Parameters to adapt the dynamic-final-goals to the environment:")
# to change the final goal dynamically using the group orientation of movement.
-gen.add("in_change_dynamically_final_dest" , bool_t, 0, "if true, we change the final dest tacking into account the person orientation", True)
+group8.add("in_change_dynamically_final_dest" , bool_t, 0, "if true, we change the final dest tacking into account the person orientation", True)
# limit of distance from the static goal to change the final goal dynamically using the group orientation of movement.
-gen.add("distance_to_dynamic_goal_Vform", double_t, 0, "distance from the person's actual goal to the dynamic goal. So that it does not crash into walls, but follow orientation people", 5.0, 0.0, 20.0)
+group8.add("distance_to_dynamic_goal_Vform", double_t, 0, "distance from the person's actual goal to the dynamic goal. So that it does not crash into walls, but follow orientation people", 5.0, 0.0, 20.0)
# Depends on the environment:
# (brl =>distance_to_dynamic_goal_Vform=6.0m) (fme=>distance_to_dynamic_goal_Vform=4.0m)
-#### Parameters to reduce the computational load due to static and dynamic obstacles ####
-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)
-#### Parameters to adjust the robot's accelerations and velocities. Can be different for different robots. ####
-gen.add("Block_parameters_division8" , str_t, 0, "Parameters to adjust the robot's accelerations and velocities:", "Parameters to adjust the robot's accelerations and velocities:")
+group9 = gen.add_group("i_frames_and_debugs", state = True, type="tab")
+
+group9.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:")
+group9.add("frame_map", str_t, 0, "frame to transform poses to", "map")
+group9.add("frame_robot_footprint", str_t, 0, "frame to transform poses to", "ana/base_link")
+group9.add("fuera_bolitas_goals_companion_markers_conf", bool_t, 0, " Fuera bolas markers", True)
+group9.add("conf_debug_all_IVO", bool_t, 0, " All mesagenes to debug IVO", False)
+group9.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
+
+
+group10 = gen.add_group("j_Customize_robot_collisions", state = True, type="tab")
+group10.add("Block_parameters_division4" , str_t, 0, "Parameters to adapt the accompaniment to the robot-platform (set-up for Tibi): ","Parameters to adapt the accompaniment to the robot-platform (already set-up for Tibi): ")
+# set-up the robot radius
+group10.add("platform_radii", double_t, 0, "robot radii", 0.5, 0.0, 5.0)
+group10.add("obstacle_radi_amp", double_t, 0, "To take into account radi of obstacle force", 0.31, 0.0, 15.0) # antes era 0.3 para companion. in real is 0.31 ==security distance cmd_vel
+group10.add("person_radi_amp", double_t, 0, "To take into account radi of person force", 0.4, 0.0, 15.0) # antes era 0.5 para companion. in real is 0.31 ==security distance cmd_vel
+# little_augmented_collision_margin_conf==0.75 normal value.
+group10.add("little_augmented_collision_margin_conf", double_t, 0, "we aument a little the colision margin", 0.75, 0.0, 5.0)
+
-# set max velocities and accelerations for propagate the robot position
-gen.add("v_max", double_t, 0, "robot maximum velocity", 1.2, 0.0, 5.0)
-gen.add("w_max", double_t, 0, "robot maximum angular velocity", 1.0, 0.0, 5.0)
-gen.add("av_max", double_t, 0, "robot maximum acceleration", 0.4, 0.0, 5.0)
-gen.add("av_max_VrobotZero", double_t, 0, "robot maximum acceleration VrobotZero", 0.6, 0.0, 5.0)
-gen.add("lim_VrobotZero", double_t, 0, "robot lim acceleration VrobotZero", 0.1, 0.0, 5.0)
-gen.add("av_max_negativa", double_t, 0, "robot maximum negative acceleration", 0.2, 0.0, 5.0)
-gen.add("av_break", double_t, 0, "robot minimum acceleration (breaking)", 0.4, 0.0, 5.0)
-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
+group12 = gen.add_group("l_iterative_simulation", state = True, type="tab")
+group12.add("Block_parameters_division50" , str_t, 0, " Iterative Simulation ", " Iterative Simulation ")
+group12.add("change_sim" , bool_t, 0, "FALSE FOR REAL; True for simulation", True)
+group12.add("change_sim_target_per" , bool_t, 0, "FALSE FOR REAL; True for simulation", True)
+
+
+group13 = gen.add_group("m_save_results_in_files", state = True, type="tab")
+group13.add("restart_real" , bool_t, 0, "if True => restart exp_case.txt save results in real robot.", False)
+group13.add("restart_real_data_txt" , bool_t, 0, "if True => restart exp=1 iter=1.", False)
+group13.add("results_filename" , str_t, 0, "File name for the results Side_by_side.", "/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/local_lib_ATR_7_junio/1_data_results_side_by_side/results_person_companion_side_by_side_real.txt")
+group13.add("save_results_in_file", bool_t, 0, "Set if we want to save_results_in_file", True)
+group13.add("see_std_out_velocities", bool_t, 0, " true to see std_out mesages", False)
+group13.add("evaluate_costs_filename" , str_t, 0, "File name for the results.", "/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_costs_robot.txt")
+group13.add("evaluate_change_distance_and_angle_companion_filename" , str_t, 0, "File name for the results.", "/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_companion_robot.txt")
+
+
+
+# j_group_all_others
+group14 = gen.add_group("n_group_all_others", state = True, type="tab")
#### 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)
+group14.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:")
+group14.add("plan_mode", int_t, 0, "#0-F_RRT_Uniform, #1-F_RRT_Gauss_Circle, #2-F_RRT_Gauss_Circle_alpha", 1, 0, 2)
+group14.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)
+group14.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)
# end of SFM params
-gen.add("min_v_to_predict", double_t, 0, "Minimum estimaated velocity that the BHMIP requires in order to make aprediction. If velocity is lower than this number, then no prediction is done and the targets remains in place", 0.2, 0.0, 5.0)
-gen.add("ppl_collision_mode", int_t, 0, "mode to calculate ppl collisions #0 deterministic, #1 mahalanobis to std, #2 mahalanobis to std 0.5 and Euclidean distance near, #3 Mahalanobis to std 0.3+Eucl", 0, 0, 3)
-gen.add("pr_force_mode", int_t, 0, "probabilistic interaction robot-ppl #0 deterministic, #1 sampling, #2 mahalanobis, #3 worst distance to ellipsoid", 0, 0, 3)
+group14.add("min_v_to_predict", double_t, 0, "Minimum estimaated velocity that the BHMIP requires in order to make aprediction. If velocity is lower than this number, then no prediction is done and the targets remains in place", 0.2, 0.0, 5.0)
+group14.add("ppl_collision_mode", int_t, 0, "mode to calculate ppl collisions #0 deterministic, #1 mahalanobis to std, #2 mahalanobis to std 0.5 and Euclidean distance near, #3 Mahalanobis to std 0.3+Eucl", 0, 0, 3)
+group14.add("pr_force_mode", int_t, 0, "probabilistic interaction robot-ppl #0 deterministic, #1 sampling, #2 mahalanobis, #3 worst distance to ellipsoid", 0, 0, 3)
-gen.add("goal_providing_mode", int_t, 0, "mode to provide goals to the local planner: #0 cropping or intersecting the plan with the boundary of local window; #1 Slicing the plan into a set of subgoals at salient points", 0, 0, 1)
-gen.add("slicing_diff_orientation", double_t, 0, "Slicing path changes in orientation to select a new subgoal. Only makes sense if the goal_providing_mode is set to #1", 20.0, 10.0, 90.0)
+group14.add("goal_providing_mode", int_t, 0, "mode to provide goals to the local planner: #0 cropping or intersecting the plan with the boundary of local window; #1 Slicing the plan into a set of subgoals at salient points", 0, 0, 1)
+group14.add("slicing_diff_orientation", double_t, 0, "Slicing path changes in orientation to select a new subgoal. Only makes sense if the goal_providing_mode is set to #1", 20.0, 10.0, 90.0)
### weights for the costo of the paths. It is missed here the companion cost. (Dificiles de quitar) ###
-gen.add("cost_distance", double_t,0, "goal cost parameter, distance to goal", 1.0, 0.0, 100.0)
-gen.add("cost_orientation", double_t,0, "orientation cost parameter, distance to goal", 1.0, 0.0, 100.0)
-gen.add("cost_w_robot", double_t,0, "robot cost paramters, work of the path", 1.0, 0.0, 100.0)
-gen.add("cost_w_people", double_t,0, "interacing people cost paramters, work of the pertubations generated",1.0, 0.0, 100.0)
-gen.add("cost_time", double_t,0, "potentia time cost paramter, up to get t_horizon", 0.25, 0.0, 100.0)
-gen.add("cost_obs", double_t,0, "obstacle cost paramter", 1.0, 0.0, 100.0)
-gen.add("cost_old_path", double_t,0, "old path cost paramter", 0.0, 0.0, 100.0)
-gen.add("cost_l_minima", double_t,0, "local minima scape cost paramter", 0.0, 0.0, 100.0)
+group14.add("cost_distance", double_t,0, "goal cost parameter, distance to goal", 1.0, 0.0, 100.0)
+group14.add("cost_orientation", double_t,0, "orientation cost parameter, distance to goal", 1.0, 0.0, 100.0)
+group14.add("cost_w_robot", double_t,0, "robot cost paramters, work of the path", 1.0, 0.0, 100.0)
+group14.add("cost_w_people", double_t,0, "interacing people cost paramters, work of the pertubations generated",1.0, 0.0, 100.0)
+group14.add("cost_time", double_t,0, "potentia time cost paramter, up to get t_horizon", 0.25, 0.0, 100.0)
+group14.add("cost_obs", double_t,0, "obstacle cost paramter", 1.0, 0.0, 100.0)
+group14.add("cost_old_path", double_t,0, "old path cost paramter", 0.0, 0.0, 100.0)
+group14.add("cost_l_minima", double_t,0, "local minima scape cost paramter", 0.0, 0.0, 100.0)
#### dificiles/largas de quitar.
-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)
+group14.add("in_max_asos_point_to_person", int_t, 0, "detector de obstaculos maximo numero de puntos associados a persona", 100, 0, 1000)
+group14.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)
-gen.add("change_time_window_filter_vel_people", double_t, 0, "change the value of the time window for filter the medium velocity of the prediction of people", 4.0, 0.1, 40.0)
-gen.add("conf_normal_vel_dampening_parameter", double_t, 0, "to change the normal velocity dampening parameter of the robot, aboid S behaviour", 1.6, -15.0, 15.0)
-gen.add("conf_limit_linear_vel_for_dampening_parameter", double_t, 0, "to change the limit_threshold for the linear velocity to change the dampening parameter of the robot, aboid S behaviour(not used at this moment)", 0.15, -15.0, 15.0)
-gen.add("conf_limit_angular_vel_for_dampening_parameter", double_t, 0, "to change the limit_threshold for the angular velocity to change the dampening parameter of the robot, aboid S behaviour(not used at this moment)", 0.8, -15.0, 15.0)
-gen.add("conf_final_max_v", double_t, 0, "Is minimun velocity to change the goal of the robot to do not it first position", 1.5, 0.0, 3.0)
-
+group14.add("change_time_window_filter_vel_people", double_t, 0, "change the value of the time window for filter the medium velocity of the prediction of people", 4.0, 0.1, 40.0)
+group14.add("conf_normal_vel_dampening_parameter", double_t, 0, "to change the normal velocity dampening parameter of the robot, aboid S behaviour", 1.6, -15.0, 15.0)
+group14.add("conf_limit_linear_vel_for_dampening_parameter", double_t, 0, "to change the limit_threshold for the linear velocity to change the dampening parameter of the robot, aboid S behaviour(not used at this moment)", 0.15, -15.0, 15.0)
+group14.add("conf_limit_angular_vel_for_dampening_parameter", double_t, 0, "to change the limit_threshold for the angular velocity to change the dampening parameter of the robot, aboid S behaviour(not used at this moment)", 0.8, -15.0, 15.0)
+group14.add("conf_final_max_v", double_t, 0, "Is minimun velocity to change the goal of the robot to do not it first position", 1.5, 0.0, 3.0)
+
+# to launch a goal from the dynamic-reconfigure. DEPRECATED
+group14.add("launch_goal" , bool_t, 0, "if True => send goal", False)
# wii and Ps3 comandaments parameters. // seguramente se cambiaran por los que necesite yo.
#gen.add("use_default_wii_button", bool_t, 0, " False to set out wii Up and down buttons", True)
#gen.add("use_default_PS3_button", bool_t, 0, " False to set out PS3 Up and down buttons", True)
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 cb90140..b9480ab 100644
--- a/local_lib_people_prediction/src/nav/plan_local_nav.h
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.h
@@ -281,6 +281,20 @@ class Cplan_local_nav : public Cprediction_behavior
only_comp_people_vel_and_robot_poses_=in_only_comp_people_vel_and_robot_poses;
}
+ void set_person_radi_amp(double in_person_radi_amp){
+ person_radi_amp_=in_person_radi_amp;
+ std::cout << " IMPORTANT!!!!!!!!!!! (changed) person_radi_amp_="<<person_radi_amp_<< std::endl;
+ person_radi_=in_person_radi_amp;// +0.2;
+ person_radi2_=in_person_radi_amp-0.1;
+ //person_radi_per_comp_=in_person_radi_amp;
+ }
+ void set_obstacle_radi_amp(double in_obstacle_radi_amp){
+ obstacle_radi_amp_=in_obstacle_radi_amp;
+ obstacle_radi_=in_obstacle_radi_amp;
+ obstacle_radi2_=in_obstacle_radi_amp;
+ std::cout << " IMPORTANT!!!!!!!!!!! (changed) obstacle_radi_amp_="<<obstacle_radi_amp_<< std::endl;
+ }
+
double get_horizon_time(){return horizon_time_;}
void set_person_list(std::list<Cperson_abstract *> in_person_list_){
@@ -571,9 +585,17 @@ class Cplan_local_nav : public Cprediction_behavior
threshold_collision_dist_stop_pers_goal_=in_threshold_collision_dist_stop_pers_goal;
}
+ void set_little_augmented_collision_margin(double in_little_augmented_collision_margin){
+ little_augmented_collision_margin_=in_little_augmented_collision_margin;
+ }
bool see_std_out_velocities_;
+ void set_see_std_out_velocities(bool in_see_std_out_velocities){
+ see_std_out_velocities_=in_see_std_out_velocities;
+ }
+
+
void set_dist_to_approach_static_people(double in_dist_to_approach_static_people){
dist_to_approach_static_people_=in_dist_to_approach_static_people;
std::cout <<"; dist_to_approach_static_people_="<<dist_to_approach_static_people_<< std::endl;
diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index cdfe6c2..dd0aecb 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -1857,6 +1857,133 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
this->planner_.set_metros_al_dynamic_goal_Vform(config.distance_to_dynamic_goal_Vform);
+ this->planner_.set_person_radi_amp(config.person_radi_amp);
+ this->planner_.set_obstacle_radi_amp(config.obstacle_radi_amp);
+ this->planner_.set_little_augmented_collision_margin(config.little_augmented_collision_margin_conf);
+ this->planner_.set_restart_real(config.restart_real);
+ if(config.restart_real_data_txt){
+ ROS_INFO("restart_real_data ROS");
+ this->planner_.set_restart_real_data_txt();
+ }
+ this->planner_.set_results_filename(config_.results_filename);
+ this->planner_.set_save_results_in_file(config.save_results_in_file);
+ 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);
+
+
+ if(config.see_config_set_up){
+ std::cout << std::endl<< std::endl<< std::endl;
+
+ std::cout <<"INIT, see all reconfigure changes that work ok:"<< std::endl;
+ std::cout <<"see_config_set_up="<<config.see_config_set_up<< std::endl;
+ std::cout <<"******* h_stop_the_robot_manually *******"<< std::endl;
+ std::cout <<"stop_robot_manually_conf="<<config.stop_robot_manually_conf<< std::endl;
+
+ // a_type_of_accompaniment:
+ std::cout <<"******* a_type_of_accompaniment *******"<< std::endl;
+ std::cout <<"select_near_pers_as_companion_one_person="<<config.select_near_pers_as_companion_one_person<< std::endl;
+ std::cout <<"id_person_companion="<<config.id_person_companion<< std::endl;
+ std::cout <<"number_of_people_in_group="<<config.number_of_people_in_group<< std::endl;
+ std::cout <<"vis_mode="<<config.vis_mode<< std::endl;
+
+ //b_accompaniment_distance_and_angle:
+ std::cout <<"******* b_accompaniment_distance_and_angle *******"<< std::endl;
+ std::cout <<"proximity_distance_between_robot_and_person="<<config.proximity_distance_between_robot_and_person<< std::endl;
+ std::cout <<"real_companion_angle_SideBySide="<<config.real_companion_angle_SideBySide<< std::endl;
+
+ //c_horizon_time_and_vertex
+ std::cout <<"******* c_horizon_time_and_vertex *******"<< std::endl;
+ std::cout <<"horizon_time="<<config.horizon_time<< std::endl;
+ std::cout <<"number_vertex="<<config.number_vertex<< std::endl;
+ std::cout <<"set_planner_dt="<<config.set_planner_dt<< std::endl;
+
+ //d_detection_obstacles:
+ std::cout <<"******* d_detection_obstacles *******"<< std::endl;
+ std::cout <<"detection_laser_obstacle_distances="<<config.detection_laser_obstacle_distances<< std::endl;
+
+ //e_accelerations_and_velocities
+ std::cout <<"******* e_accelerations_and_velocities *******"<< std::endl;
+ std::cout <<"max_real_speed_out="<<config.max_real_speed_out<< std::endl;
+ std::cout <<"max_real_angular_speed_out="<<config.max_real_angular_speed_out<< std::endl;
+
+ std::cout <<"speed_k="<<config.speed_k<< std::endl;
+ std::cout <<"speed_k_angular="<<config.speed_k_angular<< std::endl;
+ std::cout <<"v_max="<<config.v_max<< std::endl;
+ std::cout <<"w_max="<<config.w_max<< std::endl;
+ std::cout <<"av_max="<<config.av_max<< std::endl;
+ std::cout <<"av_max_VrobotZero="<<config.av_max_VrobotZero<< std::endl;
+ std::cout <<"lim_VrobotZero="<<config.lim_VrobotZero<< std::endl;
+ std::cout <<"av_max_negativa="<<config.av_max_negativa<< std::endl;
+ std::cout <<"av_break="<<config.av_break<< std::endl;
+ std::cout <<"aw_max="<<config.aw_max<< std::endl;
+
+ //g_stop_slowly_the_robot_when_person_stops:
+ std::cout <<"******* g_stop_slowly_the_robot_when_person_stops *******"<< std::endl;
+ std::cout <<"step_decreace_velocity_stop_robot_conf="<<config.step_decreace_velocity_stop_robot_conf<< std::endl;
+ std::cout <<"limit_velocity_stop_robot_conf="<<config.limit_velocity_stop_robot_conf<< std::endl;
+
+ std::cout <<"******* Parameters to adapt the dynamic-final-goals to the environments *******"<< std::endl;
+ std::cout <<"in_change_dynamically_final_dest="<<config.in_change_dynamically_final_dest<< std::endl;
+ std::cout <<"distance_to_dynamic_goal_Vform="<<config.distance_to_dynamic_goal_Vform<< std::endl;
+
+ //i_frames_and_debugs
+ std::cout <<"******* i_frames_and_debugs *******"<< std::endl;
+ std::cout <<"frame_map="<<config.frame_map<< std::endl;
+ std::cout <<"frame_robot_footprint="<<config.frame_robot_footprint<< std::endl;
+ std::cout <<"fuera_bolitas_goals_companion_markers_conf="<<config.fuera_bolitas_goals_companion_markers_conf<< std::endl;
+ std::cout <<"conf_debug_all_IVO="<<config.conf_debug_all_IVO<< std::endl;
+ std::cout <<"conf_debug_few_IVO="<<config.conf_debug_few_IVO<< std::endl;
+ std::cout <<"external_goal="<<config.external_goal<< std::endl;
+
+ //### #### ### BLOCK12: Robots collisions ### #### ###
+ std::cout <<"******* Robots collisions *******"<< std::endl;
+ std::cout <<"platform_radii="<<config.platform_radii<< std::endl;
+ //std::cout <<"obstacle_radi_amp="<<config.obstacle_radi_amp<< std::endl;
+ //std::cout <<"person_radi_amp="<<config.person_radi_amp<< std::endl;
+ //std::cout <<"little_augmented_collision_margin_conf="<<config.little_augmented_collision_margin_conf<< std::endl;
+
+ // (Parametros anteriores o para simulacion y tal) ########### Change between Real-Robot or Gazebo, and Iterative simulation ###########
+ std::cout <<"******* Iterative simulation *******"<< std::endl;
+ std::cout <<"change_sim="<< config.change_sim<< std::endl;
+ std::cout <<"change_sim_target_per="<< config.change_sim_target_per<< std::endl;
+
+ //### #### ### BLOCK16: Visualize markers, output messages, and data in files ### #### ###
+ std::cout <<"******* Visualize markers, output messages, and data in files *******"<< std::endl;
+ std::cout <<"restart_real="<<config.restart_real<< std::endl;
+ std::cout <<"restart_real_data_txt="<<config.restart_real_data_txt<< std::endl;
+ std::cout <<"results_filename="<<config.results_filename<< std::endl;
+ std::cout <<"save_results_in_file="<<config.save_results_in_file<< std::endl;
+ std::cout <<"see_std_out_velocities="<<config.see_std_out_velocities<< std::endl;
+ std::cout <<"evaluate_costs_filename="<<config.evaluate_costs_filename<< std::endl;
+ std::cout <<"evaluate_change_distance_and_angle_companion_filename="<<config.evaluate_change_distance_and_angle_companion_filename<< std::endl;
+
+ std::cout <<"******* n_group_all_others *******"<< std::endl;
+ std::cout <<"plan_mode="<< config.plan_mode<< std::endl;
+ std::cout <<"distance_mode="<< config.distance_mode<< std::endl;
+ std::cout <<"global_mode="<< config.global_mode<< std::endl;
+ std::cout <<"min_v_to_predict="<< config.min_v_to_predict<< std::endl;
+ std::cout <<"ppl_collision_mode="<< config.ppl_collision_mode<< std::endl;
+ std::cout <<"pr_force_mode="<< config.pr_force_mode<< std::endl;
+ std::cout <<"goal_providing_mode="<< config.goal_providing_mode<< std::endl;
+ std::cout <<"slicing_diff_orientation="<< config.slicing_diff_orientation<< std::endl;
+ std::cout <<"cost_distance="<< config.cost_distance<< std::endl;
+ std::cout <<"cost_orientation="<< config.cost_orientation<< std::endl;
+ std::cout <<"cost_w_robot="<< config.cost_w_robot<< std::endl;
+ std::cout <<"cost_w_people="<< config.cost_w_people<< std::endl;
+ std::cout <<"cost_time="<< config.cost_time<< std::endl;
+ std::cout <<"cost_obs="<< config.cost_obs<< std::endl;
+ std::cout <<"cost_old_path="<< config.cost_old_path<< std::endl;
+ std::cout <<"cost_l_minima="<< config.cost_l_minima<< std::endl;
+ std::cout <<"in_max_asos_point_to_person="<< config.in_max_asos_point_to_person<< std::endl;
+ std::cout <<"set_initial_v_robot_needed_for_odom="<< config.set_initial_v_robot_needed_for_odom<< std::endl;
+ std::cout <<"change_time_window_filter_vel_people="<< config.change_time_window_filter_vel_people<< std::endl;
+ std::cout <<"conf_normal_vel_dampening_parameter="<< config.conf_normal_vel_dampening_parameter<< std::endl;
+ std::cout <<"conf_limit_linear_vel_for_dampening_parameter="<< config.conf_limit_linear_vel_for_dampening_parameter<< std::endl;
+ std::cout <<"conf_limit_angular_vel_for_dampening_parameter="<< config.conf_limit_angular_vel_for_dampening_parameter<< std::endl;
+ std::cout <<"conf_final_max_v="<<config.conf_final_max_v<< std::endl;
+ std::cout <<"launch_goal="<<config.launch_goal<< std::endl;
+ }
+
//std::cout << " OUT!!!! AkpLocalPlanner::reconfigureCallback this->move_base ="<< this->move_base << std::endl;
this->planner_mutex_exit();
}
--
GitLab