ControlManager: fxd the err msg print regarding model params

ctu-mrs · Feb 10, 2024 · 5abdf8e · 5abdf8e
1 parent 051420f
commit 5abdf8e
Showing 1 changed file with 28 additions and 17 deletions.
diff --git a/src/control_manager/common/common.cpp b/src/control_manager/common/common.cpp
@@ -505,31 +505,40 @@ std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle&
     param_loader.addYamlFile(platform_config);
   }
 
+  double mass;
   double arm_length;
   double body_height;
   double force_constant;
   double torque_constant;
   double prop_radius;
   double rpm_min;
   double rpm_max;
-  double mass;
 
-  param_loader.loadParam("uav_mass", mass);
+  bool detailed_loaded = true;
+
+  detailed_loaded *= param_loader.loadParam("uav_mass", mass, 0.0);
+
+  detailed_loaded *= param_loader.loadParam("model_params/arm_length", arm_length, 0.0);
+  detailed_loaded *= param_loader.loadParam("model_params/body_height", body_height, 0.0);
 
-  param_loader.loadParam("model_params/arm_length", arm_length);
-  param_loader.loadParam("model_params/body_height", body_height);
+  detailed_loaded *= param_loader.loadParam("model_params/propulsion/force_constant", force_constant, 0.0);
+  detailed_loaded *= param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant, 0.0);
+  detailed_loaded *= param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius, 0.0);
+  detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min, 0.0);
+  detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max, 0.0);
 
-  param_loader.loadParam("model_params/propulsion/force_constant", force_constant);
-  param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant);
-  param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius);
-  param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min);
-  param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max);
+  Eigen::MatrixXd allocation_matrix;
 
-  Eigen::MatrixXd allocation_matrix = param_loader.loadMatrixDynamic2("model_params/propulsion/allocation_matrix", 4, -1);
+  detailed_loaded *= param_loader.loadMatrixDynamic("model_params/propulsion/allocation_matrix", allocation_matrix, Eigen::Matrix4d::Identity(), 4, -1);
 
-  if (!param_loader.loadedSuccessfully()) {
-    ROS_INFO("[%s]: detailed model params not loaded, missing some info", node_name.c_str());
+  if (!detailed_loaded) {
+    ROS_WARN(
+        "[%s]: detailed UAV model params not loaded, missing some parameters. This will not permit operations when ACTUATORS or CONTROL_GROUP control "
+        "outputs would be possible.",
+        node_name.c_str());
     return {};
+  } else {
+    ROS_INFO("[%s]: detailed UAV model params successfully loaded.", node_name.c_str());
   }
 
   int n_motors = allocation_matrix.cols();
@@ -540,16 +549,20 @@ std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle&
   model_params.body_height = body_height;
   model_params.prop_radius = prop_radius;
 
-  Eigen::MatrixXd inertia_matrix = param_loader.loadMatrixDynamic2("model_params/inertia_matrix", 3, 3);
+  Eigen::Matrix3d inertia_matrix;
+
+  bool inertia_loaded = param_loader.loadMatrixStatic("model_params/inertia_matrix", inertia_matrix, Eigen::Matrix3d::Identity());
 
-  if (param_loader.loadedSuccessfully()) {
+  if (inertia_loaded) {
 
     model_params.inertia = inertia_matrix;
-    ROS_INFO("[%s]: inertia loaded from config file:", node_name.c_str());
+    ROS_INFO("[%s]: inertia matrix loaded from config file:", node_name.c_str());
     ROS_INFO_STREAM(model_params.inertia);
 
   } else {
 
+    ROS_INFO("[%s]: inertia matrix missing in the config file, computing it from the other params.", node_name.c_str());
+
     // create the inertia matrix
     model_params.inertia       = Eigen::Matrix3d::Zero();
     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
@@ -597,8 +610,6 @@ std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle&
     alloc_tmp(i, 3) = 1.0;
   }
 
-  std::cout << "Control group mixer: " << std::endl << alloc_tmp << std::endl;
-
   model_params.control_group_mixer = alloc_tmp;
 
   return model_params;