config - added path_plan_update_violation_multiplier parameter [#122

]

cfg/HumapPlanner.cfg

@@ -35,6 +35,7 @@ group_behaviours = gen.add_group("PlannerBehaviours", type="tab")
 group_behaviours.add("oscillation_buffer_length", double_t, 0, "Length of a buffer to detect oscillations. Expressed as a number of seconds to keep the measurements.", 5.0, 1.0, 20.0)
 group_behaviours.add("path_crossing_confidence_threshold", double_t, 0, "Confidence threshold to consider geometrical arrangement between the robot and a person as path crossing", 0.6, 0.0, 1.0)
 group_behaviours.add("yield_way_max_travel_distance", double_t, 0, "Maximum distance that the robot can travel while in the YIELD_WAY_CROSSING state", 0.75, 0.2, 3.0)
+group_behaviours.add("path_plan_update_violation_multiplier", double_t, 0, "The multiplier of expected path planning frequency; once the global path plan is older than 'expected_freq * multiplier', it is considered a violation", 3.0, 1.0, 30.0)
 
 # SFM group parameters
 group_sfm = gen.add_group("SFM", type="tab")

include/humap_local_planner/humap_config.h

@@ -61,6 +61,11 @@ namespace humap_local_planner {
 		double path_crossing_confidence_threshold = 0.6;
 		/// Maximum distance that the robot can travel while in the YIELD_WAY_CROSSING state
 		double yield_way_max_travel_distance = 0.75;
+		/**
+		 * The multiplier of expected path planning frequency; once the global path plan is older than
+		 * 'expected_freq * multiplier', it is considered a violation
+		 */
+		double path_plan_update_violation_multiplier = 5.0;
 	};
 
 	/// \brief Declaration of an SfmParams typedef'ed struct;

src/humap_config_ros.cpp

@@ -105,6 +105,7 @@ void HumapConfigROS::reconfigure(HumapPlannerConfig& cfg) {
 	planner_behaviors_->oscillation_buffer_length = cfg.oscillation_buffer_length;
 	planner_behaviors_->path_crossing_confidence_threshold = cfg.path_crossing_confidence_threshold;
 	planner_behaviors_->yield_way_max_travel_distance = cfg.yield_way_max_travel_distance;
+	planner_behaviors_->path_plan_update_violation_multiplier = cfg.path_plan_update_violation_multiplier;
 
 	sfm_->fov = cfg.fov;
 	sfm_->fov_factor_method = cfg.sfm_fov_factor_method;