committing before changing things around

astrobee/survey_manager/scripts/command_astrobee

@@ -22,7 +22,9 @@ import argparse
 import subprocess
 import sys
 
-command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"'
+
+
+# command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"'
 
 
 def send_command_with_input(command):
@@ -97,6 +99,30 @@ def repeat_inspection():
             print("Invalid input. Please enter 'y' or 'n'.")
 
 
+def survey_manager_executer(goal, bay, run):
+
+    if goal == "dock":
+        send_command("rosrun executive teleop -dock")
+
+    elif goal == "dock":
+        send_command("rosrun executive teleop -undock")
+
+    elif goal == "move":
+        send_command("rosrun executive teleop -move -pos " + bay)
+
+    elif goal == "panorama":
+        send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + bay + "_" + run)
+        if send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(bay))) != 0:
+            repeat_inspection()
+        send_command("python gds_helper_batch.py -i cmd -- bagger -stop")
+
+    elif goal == "stereo":
+        send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + bay + "_" + run)
+        send_command("python gds_helper_batch.py -i cmd -- plan -load plans/ISAAC/" + bay + "_stereo_mapping.fplan")
+        send_command("python gds_helper_batch.py -i cmd -- plan -run")
+        send_command("python gds_helper_batch.py -i cmd -- bagger -stop")
+
+
 class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter):
     pass
 
@@ -111,34 +137,16 @@ if __name__ == "__main__":
         help="Prefix for bagfiles to merge. Bags should all be in the current working directory.",
     )
     parser.add_argument(
-        "arg1",
+        "bay",
         default="",
-        help="Prefix for bagfiles to merge. Bags should all be in the current working directory.",
+        help="Target bay start.",
     )
+
     parser.add_argument(
-        "arg2",
+        "run",
         default="",
-        help="Prefix for bagfiles to merge. Bags should all be in the current working directory.",
+        help="Run number, increases as we add attempts.",
     )
     args = parser.parse_args()
 
-
-    if args.command_name == "dock":
-        send_command("rosrun executive teleop -dock")
-
-    elif args.command_name == "dock":
-        send_command("rosrun executive teleop -undock")
-
-    elif args.command_name == "move":
-        send_command("rosrun executive teleop -move -pos " + args.arg1)
-
-    elif args.command_name == "panorama":
-        send_command("python gds_helper_batch.py -i cmd -- bagger -start pano_" + args.arg1 + "_" + args.arg2)
-        if send_command_with_input("rosrun inspection inspection_tool -panorama -panorama_mode '5_mapper_and_hugin' -pos " + str(get_position(args.arg1))) != 0:
-            repeat_inspection()
-        send_command("python gds_helper_batch.py -i cmd -- bagger -stop")
-
-    elif args.command_name == "stereo":
-        send_command("python gds_helper_batch.py -i cmd -- bagger -start stereo_" + args.arg1 + "_" + args.arg2)
-        send_command("rosrun executive teleop -undock")
-        send_command("python gds_helper_batch.py -i cmd -- bagger -stop")
+    survey_manager_executer(args.command_name, args.bay, args.run)