hotfix in solver experiments script

ws/src/solver_experiments/solver_experiments/solver_experiments.py

@@ -7,6 +7,7 @@
 from math import radians
 import platform
 import rclpy
+import rclpy.parameter
 import rosbag2_py
 from ament_index_python import get_package_share_directory
 from interfaces.msg import QuadControlTarget
@@ -16,6 +17,7 @@
 from rcl_interfaces.srv import SetParameters
 from rclpy.node import Node
 from std_srvs.srv import Trigger
+import argparse
 
 
 class ExperimentPlayer(Node):
@@ -476,10 +478,13 @@ def run_experiment(
 
 
 def main(args=None):
-    # Main script
     rclpy.init(args=args)
+
     solver_experiments_node = SolverExperiments("src/solver_experiments/results")
-
+    solver_experiments_node.declare_parameter("mpc_prediction_horizon", 20)
+    solver_experiments_node.declare_parameter("test_preset", "")
+
+    # Main script
     def run_func():
         executor = rclpy.executors.MultiThreadedExecutor(2)
         executor.add_node(solver_experiments_node)
@@ -627,50 +632,46 @@ def run_func():
     mpc_osqp_linsys_modes = ["qdldl", "mkl pardiso"]
     if platform.processor() == "aarch64":
         mpc_osqp_linsys_modes = ["qdldl"]
+    mpc_condensed_sizes = range(solver_experiments_node.get_parameter("mpc_prediction_horizon").get_parameter_value().integer_value, 0, -1)
     wbc_solvers = ["EiquadprogSolver", "ProxQPSolver", "QPOasesSolver", "HPIPMSolver"]
     wbc_scenes = ["AccelerationSceneReducedTSID", "AccelerationSceneTSID"]
-    mpc_condensed_sizes = [
-        20,
-        19,
-        18,
-        17,
-        16,
-        15,
-        14,
-        13,
-        12,
-        11,
-        10,
-        9,
-        8,
-        7,
-        6,
-        5,
-        4,
-        3,
-        2,
-        1,
-    ]
     solver_tolerances = [
         0.001,
         1e-06,
         1e-09
     ]
     max_trials = 3
 
+    preset = solver_experiments_node.get_parameter("test_preset").get_parameter_value().string_value
+
+    if preset == 'mpc':
+        wbc_solvers = [wbc_solvers[0]]
+        wbc_scenes = [wbc_scenes[0]]
+    elif preset == 'wbc':
+        mpc_solvers = [mpc_solvers[0]]
+        mpc_hpipm_modes = [mpc_hpipm_modes[0]]
+        mpc_osqp_linsys_modes = [mpc_osqp_linsys_modes[0]]
+        mpc_condensed_sizes = [mpc_condensed_sizes[0]]
+
     for mpc_solver in mpc_solvers:
         if not mpc_solver.__contains__("PARTIAL"):
-            mpc_condensed_sizes = [1]
+            mpc_condensed_sizes_f = [1]
+        else:
+            mpc_condensed_sizes_f = mpc_condensed_sizes
         if not mpc_solver.__contains__("HPIPM"):
-            mpc_hpipm_modes = ["xxxxx"]
+            mpc_hpipm_modes_f = ["xxxxx"]
+        else:
+            mpc_hpipm_modes_f = mpc_hpipm_modes
         if not mpc_solver.__contains__("OSQP"):
-            mpc_osqp_linsys_modes = ["xxxxx"]
+            mpc_osqp_linsys_modes_f = ["xxxxx"]
+        else:
+            mpc_osqp_linsys_modes_f = mpc_osqp_linsys_modes
 
 
         for solver_tolerance in solver_tolerances:    
-            for mpc_osqp_linsys_mode in mpc_osqp_linsys_modes:
-                for mpc_hpipm_mode in mpc_hpipm_modes:
-                    for mpc_condensed_size in mpc_condensed_sizes:
+            for mpc_osqp_linsys_mode in mpc_osqp_linsys_modes_f:
+                for mpc_hpipm_mode in mpc_hpipm_modes_f:
+                    for mpc_condensed_size in mpc_condensed_sizes_f:
                         for wbc_scene in wbc_scenes:
                             for wbc_solver in wbc_solvers:
                                 for experiment, experiment_name, experiment_gait in zip(
@@ -698,6 +699,8 @@ def run_func():
                                             + "-"
                                             + str(solver_tolerance)
                                         )
+                                        print(target_bag)
+                                        continue
                                         success = solver_experiments_node.run_experiment(
                                             mpc_solver,
                                             mpc_hpipm_mode,