HuberoConfig & SocialConductor - added As param configurable in…

… real-time
rayvburn · May 27, 2022 · d4c0a9a · d4c0a9a
1 parent a0bec6b
commit d4c0a9a
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 1 deletion.
diff --git a/cfg/HuberoPlanner.cfg b/cfg/HuberoPlanner.cfg
@@ -50,6 +50,7 @@ group_fis = gen.add_group("FIS", type="tab")
 
 group_fis.add("force_factor", double_t, 0, "Factor to multiply resultant human action force with", 30.0, 0.0, 100.0)
 group_fis.add("human_action_range", double_t, 0, "Distance at which any human action force fade completely", 8.0, 0.0, 10.0)
+group_fis.add("as", double_t, 0, "Levelling factor that also directly affects amplitude of the resultant force", 1.1865, 0.0, 10.0)
 
 # Trajectory generation group parameters
 group_traj = gen.add_group("Trajectory", type="tab")

diff --git a/include/hubero_local_planner/fuzz/social_conductor.h b/include/hubero_local_planner/fuzz/social_conductor.h
@@ -39,6 +39,9 @@ class SocialConductor {
 	/// \brief Updates internal state according to a given structure's content
 	void initialize(std::shared_ptr<const hubero_local_planner::FisParams> cfg);
 
+	/// \brief Updates parameter(s)
+	void setEquationParameters(double as);
+
 	/// \brief Evaluates force factor (from given `cfg`) to check if FIS calculations make sense
 	bool areFuzzyBehavioursDisabled() const;
 
@@ -70,6 +73,11 @@ class SocialConductor {
 	/// \return
 	std::string getBehaviourActive() const;
 
+	/// \brief Retrieves As parameter
+	double getParameterAs() const {
+		return As_;
+	}
+
 	/// \brief Destructor
 	virtual ~SocialConductor() = default;
 
@@ -95,6 +103,9 @@ class SocialConductor {
 		const double& speed_agent,
 		const double& speed_obstacle
 	);
+
+	/// \brief Levelling factor
+	double As_;
 };
 
 } /* namespace fuzz */

diff --git a/include/hubero_local_planner/hubero_config.h b/include/hubero_local_planner/hubero_config.h
@@ -71,6 +71,8 @@ namespace hubero_local_planner {
 		double force_factor = 1.0;
 		/// Distance at which any created human action force fade completely
 		double human_action_range = 4.0;
+		/// Levelling factor that also directly affects amplitude of the resultant force
+		double as = 1.1865;
 	};
 
 	/**

diff --git a/src/fuzz/social_conductor.cpp b/src/fuzz/social_conductor.cpp
@@ -15,6 +15,10 @@ void SocialConductor::initialize(std::shared_ptr<const hubero_local_planner::Fis
 	cfg_ = cfg;
 }
 
+void SocialConductor::setEquationParameters(double as) {
+	As_ = as;
+}
+
 bool SocialConductor::areFuzzyBehavioursDisabled() const {
 	if (cfg_ == nullptr) {
 		return true;
@@ -51,7 +55,7 @@ bool SocialConductor::computeBehaviourForce(
 		// scale force vector with relevant factors
 		double membership_factor = fis_outputs_v.at(i).membership;
 		double geom_factor = computeBehaviourStrength(dist_v.at(i), speed_agent, speeds_v.at(i));
-		behaviour_force_ += v_temp * membership_factor * geom_factor * INTERACTION_STRENGTH_LEVELLING_FACTOR;
+		behaviour_force_ += v_temp * As_ * membership_factor * geom_factor * INTERACTION_STRENGTH_LEVELLING_FACTOR;
 
 		updateActiveBehaviour(fis_outputs_v.at(i).term_name);
 	}

diff --git a/src/hubero_config_ros.cpp b/src/hubero_config_ros.cpp
@@ -101,6 +101,7 @@ void HuberoConfigROS::reconfigure(HuberoPlannerConfig& cfg) {
 
 	fis_->force_factor = cfg.force_factor;
 	fis_->human_action_range = cfg.human_action_range;
+	fis_->as = cfg.as;
 
 	limits_->acc_lim_theta = cfg.acc_lim_theta;
 	limits_->acc_lim_x = cfg.acc_lim_x;