From 4b3c9afcb7d66cb722d3b5c2e420bd15f1410b1c Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Wed, 30 Aug 2023 14:32:34 +0200 Subject: [PATCH] `config` - added params for `UnsaturatedTranslation` cost function [#108] --- cfg/HumapPlanner.cfg | 2 ++ include/humap_local_planner/humap_config.h | 4 ++++ src/humap_config_ros.cpp | 2 ++ 3 files changed, 8 insertions(+) diff --git a/cfg/HumapPlanner.cfg b/cfg/HumapPlanner.cfg index 2df6890e..90bf585b 100755 --- a/cfg/HumapPlanner.cfg +++ b/cfg/HumapPlanner.cfg @@ -194,6 +194,7 @@ group_cost.add("occdist_separation_kernel", int_t, 0, "Type of kernel that the r group_cost.add("ttc_scale", double_t, 0, "The weight for the time to collision cost function (scores a whole trajectory instead of a single cell)", 3.0, 0.0) group_cost.add("alignment_scale", double_t, 0, "The weight for the alignment with the global plan. In a classical DWA implementation equal to `path_distance_scale`", 1.5, 0.0) group_cost.add("goal_front_scale", double_t, 0, "The weight for the achievement of the additional goal point placed in front of the mobile base. In a classical DWA implementation equal to `goal_distance_scale`", 8.0, 0.0) +group_cost.add("unsaturated_translation_scale", double_t, 0, "The weight for deviating from the maximum allowable translational velocities", 0.0, 0.0) group_cost.add("backward_scale", double_t, 0, "The weight for the backward motion penalisation", 0.08, 0.0) group_cost.add("heading_change_smoothness_scale", double_t, 0, "The weight for the cost function that penalizes robot rotational velocity changes", 11.0, 0.0) @@ -223,6 +224,7 @@ group_cost.add("heading_dir_compute_whole_horizon", bool_t, 0, ONLY_FIRST_STEP_D group_cost.add("personal_space_compute_whole_horizon", bool_t, 0, ONLY_FIRST_STEP_DESCRIPTION, True) group_cost.add("fformation_space_compute_whole_horizon", bool_t, 0, ONLY_FIRST_STEP_DESCRIPTION, True) group_cost.add("passing_speed_compute_whole_horizon", bool_t, 0, ONLY_FIRST_STEP_DESCRIPTION, True) +group_cost.add("unsaturated_translation_compute_whole_horizon", bool_t, 0, ONLY_FIRST_STEP_DESCRIPTION, False) # Diagnostics group_diagnostics = gen.add_group("Diagnostics", type="tab") diff --git a/include/humap_local_planner/humap_config.h b/include/humap_local_planner/humap_config.h index 7f77fbea..ab9ec8d1 100644 --- a/include/humap_local_planner/humap_config.h +++ b/include/humap_local_planner/humap_config.h @@ -216,6 +216,8 @@ namespace humap_local_planner { double alignment_scale = 0.6; /// The weight for the achievement of the additional goal point placed in front of the mobile base double goal_front_scale = 0.8; + /// The weight for deviating from the maximum allowable translational velocities + double unsaturated_translation_scale = 0.0; /// The weight for the backward motion penalisation double backward_scale = 0.5; /// The weight for the time to collision (TTC) cost function @@ -256,6 +258,8 @@ namespace humap_local_planner { bool fformation_space_compute_whole_horizon = true; /// Whether to compute the cost fun. for the whole horizon or only for the first step of each traj. candidate bool passing_speed_compute_whole_horizon = true; + /// Whether to compute the cost fun. for the whole horizon or only for the first step of each traj. candidate + bool unsaturated_translation_compute_whole_horizon = true; }; struct DiagnosticsParams { diff --git a/src/humap_config_ros.cpp b/src/humap_config_ros.cpp index 4065b6bb..eee81970 100644 --- a/src/humap_config_ros.cpp +++ b/src/humap_config_ros.cpp @@ -200,6 +200,7 @@ void HumapConfigROS::reconfigure(HumapPlannerConfig& cfg) { costs_->occdist_separation_kernel = cfg.occdist_separation_kernel; costs_->alignment_scale = cfg.alignment_scale; costs_->goal_front_scale = cfg.goal_front_scale; + costs_->unsaturated_translation_scale = cfg.unsaturated_translation_scale; costs_->backward_scale = cfg.backward_scale; costs_->ttc_scale = cfg.ttc_scale; costs_->heading_change_smoothness_scale = cfg.heading_change_smoothness_scale; @@ -220,6 +221,7 @@ void HumapConfigROS::reconfigure(HumapPlannerConfig& cfg) { costs_->personal_space_compute_whole_horizon = cfg.personal_space_compute_whole_horizon; costs_->fformation_space_compute_whole_horizon = cfg.fformation_space_compute_whole_horizon; costs_->passing_speed_compute_whole_horizon = cfg.passing_speed_compute_whole_horizon; + costs_->unsaturated_translation_compute_whole_horizon = cfg.unsaturated_translation_compute_whole_horizon; diagnostics_->log_trajectory_generation_samples = cfg.log_trajectory_generation_samples; diagnostics_->log_trajectory_generation_details = cfg.log_trajectory_generation_details;