fixed loading of detailed model params

ctu-mrs · Nov 1, 2023 · 42970dd · 42970dd
1 parent 010e42a
commit 42970dd
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 3 deletions.
diff --git a/include/mrs_uav_managers/control_manager/common.h b/include/mrs_uav_managers/control_manager/common.h
@@ -66,7 +66,8 @@ bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_nam
 bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name);
 bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name);
 
-std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name);
+std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name, const std::string& platform_config,
+                                                                const std::string& custom_config);
 
 // translates the channel values to desired range
 double RCChannelToRange(double rc_value, double range, double deadband);

diff --git a/src/control_manager/common/common.cpp b/src/control_manager/common/common.cpp
@@ -492,10 +492,19 @@ double RCChannelToRange(double rc_value, double range, double deadband) {
 
 /* loadDetailedUavModelParams() //{ */
 
-std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name) {
+std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name, const std::string& platform_config,
+                                                                const std::string& custom_config) {
 
   mrs_lib::ParamLoader param_loader(nh, node_name);
 
+  if (custom_config != "") {
+    param_loader.addYamlFile(custom_config);
+  }
+
+  if (platform_config != "") {
+    param_loader.addYamlFile(platform_config);
+  }
+
   double arm_length;
   double body_height;
   double force_constant;

diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
@@ -1253,7 +1253,7 @@ void ControlManager::initialize(void) {
 
   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
 
-  common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager");
+  common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
 
   common_handlers_->control_output_modalities = _hw_api_inputs_;