config - added params for UnsaturatedTranslation cost function [#108

]

cfg/HuberoPlanner.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")

include/hubero_local_planner/hubero_config.h

@@ -216,6 +216,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 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 hubero_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 {

src/hubero_config_ros.cpp

@@ -200,6 +200,7 @@ void HuberoConfigROS::reconfigure(HuberoPlannerConfig& 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 HuberoConfigROS::reconfigure(HuberoPlannerConfig& 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;