Adding scp gusto planner to the MLP launch

StanfordASL · Aug 13, 2024 · 4a4dc80 · 4a4dc80
1 parent e75f672
commit 4a4dc80
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 3 deletions.
diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch
@@ -177,6 +177,16 @@
     <arg name="debug" value="$(arg debug)" />
     <arg name="default" value="true" />
   </include>
+  <include file="$(find ff_util)/launch/ff_nodelet.launch">
+    <arg name="class" value="planner_scp_gusto/PlannerSCPGustoNodelet" />
+    <arg name="name" value="planner_scp_gusto" />
+    <arg name="manager" value="mlp_mobility" />
+    <arg name="spurn" value="$(arg spurn)" />
+    <arg name="nodes" value="$(arg nodes)" />
+    <arg name="extra" value="$(arg extra)" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="default" value="true" />
+  </include>
   <include file="$(find ff_util)/launch/ff_nodelet.launch">
     <arg name="class" value="mobility/FrameStore" />
     <arg name="name" value="framestore" />

diff --git a/management/executive/src/executive.cc b/management/executive/src/executive.cc
@@ -2833,11 +2833,13 @@ bool Executive::SetPlanner(ff_msgs::CommandStampedPtr const& cmd) {
     // Check that the planner string is valid
     if (cmd->args[0].s != CommandConstants::PARAM_NAME_PLANNER_TYPE_TRAPEZOIDAL
         && cmd->args[0].s !=
-                CommandConstants::PARAM_NAME_PLANNER_TYPE_QUADRATIC_PROGRAM) {
-      NODELET_ERROR("Planner must be either Trapezoidal or QuadraticProgram");
+                CommandConstants::PARAM_NAME_PLANNER_TYPE_QUADRATIC_PROGRAM
+        && cmd->args[0].s !=
+                "scp_gusto") {
+      NODELET_ERROR("Planner must be either Trapezoidal or QuadraticProgram or SCP_GuSTO");
       state_->AckCmd(cmd->cmd_id,
                      ff_msgs::AckCompletedStatus::BAD_SYNTAX,
-                     "Planner must be either Trapezoidal or QuadraticProgram");
+                     "Planner must be either Trapezoidal or QuadraticProgram or SCP_GuSTO");
       return false;
     }