config - added parameters to compute human-awareness cost functions…

… for the whole trajectory vs only the first step [#119]

cfg/HumapPlanner.cfg

@@ -217,6 +217,13 @@ group_cost.add("ttc_collision_distance", double_t, 0, "The distance threshold fo
 
 group_cost.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters. Set to zero to discard alignment.", 0.325, 0.1, 2.0)
 
+# selectors to compute some cost functions only for the first iteration instead of the whole horizon
+ONLY_FIRST_STEP_DESCRIPTION = "Whether to compute the cost fun. for the whole horizon or only for the first step of each traj. candidate"
+group_cost.add("heading_dir_compute_whole_horizon", bool_t, 0, ONLY_FIRST_STEP_DESCRIPTION, True)
+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)
+
 # Diagnostics
 group_diagnostics = gen.add_group("Diagnostics", type="tab")
 group_diagnostics.add("log_trajectory_generation_samples", bool_t, 0, "Whether to print info on samples used for trajectory generation", False)

include/humap_local_planner/humap_config.h

@@ -248,6 +248,14 @@ namespace humap_local_planner {
 		double ttc_collision_distance = 0.05;
 		/// The distance from the center point of the robot to place an additional scoring point, in meters
 		double forward_point_distance = 0.325;
+		/// Whether to compute the cost fun. for the whole horizon or only for the first step of each traj. candidate
+		bool heading_dir_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 personal_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 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;
 	};
 
 	struct DiagnosticsParams {

src/humap_config_ros.cpp

@@ -216,6 +216,10 @@ void HumapConfigROS::reconfigure(HumapPlannerConfig& cfg) {
 	costs_->ttc_rollout_time = cfg.ttc_rollout_time;
 	costs_->ttc_collision_distance = cfg.ttc_collision_distance;
 	costs_->forward_point_distance = cfg.forward_point_distance;
+	costs_->heading_dir_compute_whole_horizon = cfg.heading_dir_compute_whole_horizon;
+	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;
 
 	diagnostics_->log_trajectory_generation_samples = cfg.log_trajectory_generation_samples;
 	diagnostics_->log_trajectory_generation_details = cfg.log_trajectory_generation_details;