From 9db80d3d77b416d542e9222ac5f317346391fe04 Mon Sep 17 00:00:00 2001
From: Somrita Banerjee <somritabanerjee@gmail.com>
Date: Sat, 14 Dec 2024 13:15:35 -0800
Subject: [PATCH] Can reconfigure parameters during sim, can load and unload
 nodelet, granite seq working

---
 .../config/mobility/planner_scp_gusto.config  |  8 +++++++
 mobility/planner_scp_gusto/src/optim.cc       | 11 +++++----
 .../src/planner_scp_gusto_nodelet.cc          | 24 ++++++++++++++++++-
 3 files changed, 38 insertions(+), 5 deletions(-)

diff --git a/astrobee/config/mobility/planner_scp_gusto.config b/astrobee/config/mobility/planner_scp_gusto.config
index 9e3df61320..5a92c069b2 100644
--- a/astrobee/config/mobility/planner_scp_gusto.config
+++ b/astrobee/config/mobility/planner_scp_gusto.config
@@ -21,5 +21,13 @@ parameters = {
     id = "epsilon", reconfigurable = true, type = "double",
     default = 1e-3, min = 1e-6, max = 1e-1, unit = "unitless",
     description = "Threshold below which magnitudes are ignored"
+  },{
+    id = "is_granite", reconfigurable = true, type = "boolean",
+    default = true, unit = "unitless",
+    description = "Flag for granite sim"
+  },{
+    id = "enforce_obs_avoidance_const", reconfigurable = true, type = "boolean",
+    default = true, unit = "unitless",
+    description = "Flag for avoiding (virtual) obstacle"
   }
 }
diff --git a/mobility/planner_scp_gusto/src/optim.cc b/mobility/planner_scp_gusto/src/optim.cc
index 8b78b61584..e5f3320023 100644
--- a/mobility/planner_scp_gusto/src/optim.cc
+++ b/mobility/planner_scp_gusto/src/optim.cc
@@ -225,9 +225,6 @@ void TOP::ResetSCPParams() {
 
 void TOP::UpdateProblemDimension(size_t N_) {
   // Allocate matrices for variables and constraints
-  if (solver && N == N_) {
-    return;
-  }
 
   N = N_;
 
@@ -241,7 +238,7 @@ void TOP::UpdateProblemDimension(size_t N_) {
     solver->data()->clearLinearConstraintsMatrix();
   }
 
-  std::cout << "cleared stuff" << std::endl;
+  std::cout << "TOP::UpdateProblemDimension: Cleared solver" << std::endl;
 
   Xprev.resize(N);
   Uprev.resize(N-1);
@@ -258,6 +255,8 @@ void TOP::UpdateProblemDimension(size_t N_) {
   size_t num_vars = GetNumTOPVariables();
   size_t num_cons = GetNumTOPConstraints();
 
+  std::cout << "TOP::UpdateProblemDimension: Num vars: " << num_vars << " Num cons: " << num_cons << std::endl;
+
   hessian.resize(num_vars, num_vars);
   linear_con_mat.resize(num_cons, num_vars);
   gradient.resize(num_vars);
@@ -1119,6 +1118,10 @@ void TOP::SetSimpleCosts() {
 
 bool TOP::Solve() {
   solved_ = false;
+  ResetSCPParams();
+  UpdateProblemDimension(N);
+  std::cout << "TOP::Solve: Updated problem dimension" << std::endl;
+  std::cout << "linear_con_mat size: " << linear_con_mat.rows() << " x " << linear_con_mat.cols() << std::endl;
   InitTrajStraightline();
   bool add_custom_keep_out_zone = true;
 
diff --git a/mobility/planner_scp_gusto/src/planner_scp_gusto_nodelet.cc b/mobility/planner_scp_gusto/src/planner_scp_gusto_nodelet.cc
index a0524c6cfd..c4a798aab5 100644
--- a/mobility/planner_scp_gusto/src/planner_scp_gusto_nodelet.cc
+++ b/mobility/planner_scp_gusto/src/planner_scp_gusto_nodelet.cc
@@ -69,6 +69,8 @@ class PlannerSCPGustoNodelet : public planner::PlannerImplementation {
   float control_rate_;    // Control frequency
   double max_time_;       // Max generation time
   double epsilon_;
+  bool enforce_obs_avoidance_const_;
+  bool is_granite_;
   bool use_2d;            // true for granite table
   std::string flight_mode_;
   ros::NodeHandle *nh_;
@@ -93,6 +95,8 @@ class PlannerSCPGustoNodelet : public planner::PlannerImplementation {
     nh_ = nh;
     // Save the epsilon value
     epsilon_ = cfg_.Get<double>("epsilon");
+    enforce_obs_avoidance_const_ = cfg_.Get<bool>("enforce_obs_avoidance_const");
+    is_granite_ = cfg_.Get<bool>("is_granite");
       // Notify initialization complete
     NODELET_DEBUG_STREAM("Initialization complete");
     // Success
@@ -104,9 +108,13 @@ class PlannerSCPGustoNodelet : public planner::PlannerImplementation {
   }
 
   bool ReconfigureCallback(dynamic_reconfigure::Config &config) {
+    std::cout << "ReconfigureCallback" << std::endl;
     if (!cfg_.Reconfigure(config))
       return false;
     epsilon_ = cfg_.Get<double>("epsilon");
+    enforce_obs_avoidance_const_ = cfg_.Get<bool>("enforce_obs_avoidance_const");
+    std::cout << "set enforce_obs_avoidance_const_ to " << enforce_obs_avoidance_const_ << std::endl;
+    is_granite_ = cfg_.Get<bool>("is_granite");
     return true;
   }
 
@@ -380,7 +388,21 @@ class PlannerSCPGustoNodelet : public planner::PlannerImplementation {
       top->x_min(ii) = min(ii);
       top->x_max(ii) = max(ii);
     }
-    top->is_granite = true;  // TODO(Somrita): Read from config
+
+    // if (!ReconfigureCallback(cfg_)) {
+    //   std::cout << "Failed to reconfigure" << std::endl;
+    //   return false;
+    // }
+    // std::cout << "Reconfigured" << std::endl;
+    std::cout << "Now the value of enforce_obs_avoidance_const_ is " << enforce_obs_avoidance_const_ << std::endl;
+    if (!cfg_.Get<bool>("enforce_obs_avoidance_const", enforce_obs_avoidance_const_)){
+      std::cout << "Failed to get param enforce_obs_avoidance_const_" << std::endl;
+      enforce_obs_avoidance_const_ = true;
+    }
+    std::cout << "Now the value of enforce_obs_avoidance_const_ is " << enforce_obs_avoidance_const_ << std::endl;
+    top->is_granite = cfg_.Get<bool>("is_granite");
+    top->enforce_obs_avoidance_const = enforce_obs_avoidance_const_;
+    std::cout << "calling TOP with enforce_obs_avoidance_const: " << top->enforce_obs_avoidance_const << std::endl;
     if (top->is_granite) {
       top->x_min(2) = -0.675;  // z coordinate
       top->x_max(2) = -0.67;