config - added scale param for SlowTranslation cost function [#108]

rayvburn · Aug 30, 2023 · 9f5c2d8 · 9f5c2d8
1 parent d91f1ab
commit 9f5c2d8
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 0 deletions.
diff --git a/cfg/HuberoPlanner.cfg b/cfg/HuberoPlanner.cfg
@@ -148,6 +148,7 @@ group_cost.add("occdist_scale", double_t, 0, "The weight for the obstacle distan
 group_cost.add("occdist_sum_scores", bool_t, 0, "Whether to sum up the scores along the path or use the maximum one", False)
 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.4, 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`", 0.0, 0.0)
+group_cost.add("slow_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("ttc_scale", double_t, 0, "The weight for the time to collision cost function (scores whole trajectory instead of a single cell)", 3.0, 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)

diff --git a/include/hubero_local_planner/hubero_config.h b/include/hubero_local_planner/hubero_config.h
@@ -206,6 +206,8 @@ namespace hubero_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 slow_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

diff --git a/src/hubero_config_ros.cpp b/src/hubero_config_ros.cpp
@@ -195,6 +195,7 @@ void HuberoConfigROS::reconfigure(HuberoPlannerConfig& cfg) {
 	costs_->occdist_sum_scores = cfg.occdist_sum_scores;
 	costs_->alignment_scale = cfg.alignment_scale;
 	costs_->goal_front_scale = cfg.goal_front_scale;
+	costs_->slow_translation_scale = cfg.slow_translation_scale;
 	costs_->backward_scale = cfg.backward_scale;
 	costs_->ttc_scale = cfg.ttc_scale;
 	costs_->heading_change_smoothness_scale = cfg.heading_change_smoothness_scale;