Survey manager execute goals script

astrobee/behaviors/inspection/tools/inspection_tool.cc

@@ -185,7 +185,7 @@ geometry_msgs::PoseArray ReadPosesFile(std::string file) {
           quat_robot.setRPY(euler_roll * DEG2RAD, euler_pitch * DEG2RAD, euler_yaw * DEG2RAD);
 
         } else {
-          std::cout << "Ignoring invalid line: " << line  << std::endl;
+          // std::cout << "Ignoring invalid line: " << line  << std::endl;
           continue;
         }
       }
@@ -390,7 +390,7 @@ void ConnectedCallback(
   if (!client->IsConnected()) return;
   // Print out a status message
   std::cout << "\r                                                   "
-            << "\rState: CONNECTED" << std::flush;
+            << "\rState: CONNECTED\n" << std::flush;
   SendGoal(client);
 }
 

astrobee/survey_manager/CMakeLists.txt

@@ -0,0 +1,56 @@
+# Copyright (c) 2021, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+# 
+# All rights reserved.
+# 
+# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
+# platform" software is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+# 
+#     http://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+
+cmake_minimum_required(VERSION 3.0)
+project(survey_manager)
+
+## Compile as C++14, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+## Find catkin macros and libraries
+SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake")
+find_package(catkin2 REQUIRED COMPONENTS
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+###################################
+## catkin specific configuration ##
+###################################
+
+catkin_package(
+)
+
+###########
+## Build ##
+###########
+
+# Specify additional locations of header files
+# Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+#############
+## Install ##
+#############
+
+catkin_install_python(PROGRAMS scripts/command_astrobee
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

astrobee/survey_manager/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>survey_manager</name>
+  <version>0.0.0</version>
+  <description>The survey_manager package</description>
+  <license>
+    Apache License, Version 2.0
+  </license>
+  <author email="[email protected]">
+    ISAAC Flight Software
+  </author>
+  <maintainer email="[email protected]">
+    ISAAC Flight Software
+  </maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
+</package>

astrobee/survey_manager/scripts/command_astrobee

@@ -0,0 +1,152 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2021, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+# 
+# All rights reserved.
+# 
+# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
+# platform" software is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+# 
+#     http://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+
+import argparse
+import subprocess
+import sys
+
+
+
+# command = 'rosrun inspection inspection_tool -panorama -panorama_poses /resources/panorama_iss.txt -panorama_mode "5_mapper_and_hugin"'
+
+
+def send_command_with_input(command):
+    print(command)
+
+    # Start the process
+    process = subprocess.Popen(command, shell=True, stdin=subprocess.PIPE, stdout=sys.stdout, stderr=subprocess.PIPE, text=True)
+
+    try:
+        while process.poll() is None:
+
+            # Get user input dynamically
+            user_input = input()
+
+            # Check if the user wants to exit
+            if user_input.lower().strip() == 'exit':
+                break
+
+            # Send the user input to the process
+            process.stdin.write(user_input + "\n")
+            process.stdin.flush()
+
+        # Close stdin to indicate no more input will be sent
+        process.stdin.close()
+
+        # Read output and error (if any)
+        output, error = process.communicate()
+
+        # Get the final exit code
+        return  process.returncode
+
+    except:
+        return -1
+
+
+def send_command(command):
+    print(command)
+    process = subprocess.Popen(command, shell=True, stdout=sys.stdout, stderr=sys.stdout, text=True)
+
+    # Get the output and error (if any)
+    return process.wait()
+
+def get_position(bay):
+
+    if bay == "jem_bay1":
+        return "'11, -4, 4.8'"
+    if bay == "jem_bay2":
+        return "'11, -5, 4.8'"
+    if bay == "jem_bay3":
+        return "'11, -6, 4.8'"
+    if bay == "jem_bay4":
+        return "'11, -7, 4.8'"
+    if bay == "jem_bay5":
+        return "'11, -8, 4.8'"
+    if bay == "jem_bay6":
+        return "'11, -9, 4.8'"
+    if bay == "jem_bay7":
+        return "'11, -9.7, 4.8'"
+
+
+def repeat_inspection():
+    while True:
+        user_input = input("Do you want to continue? (y/n): ").lower().strip()
+        if user_input == 'y':
+            if send_command_with_input("rosrun inspection inspection_tool -resume") != 0:
+                repeat_inspection()  # Continue recursively
+            break
+        elif user_input == 'n':
+            print("Exiting.")
+            break
+        else:
+            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
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description=__doc__, formatter_class=CustomFormatter
+    )
+
+    parser.add_argument(
+        "command_name",
+        help="Prefix for bagfiles to merge. Bags should all be in the current working directory.",
+    )
+    parser.add_argument(
+        "bay",
+        default="",
+        help="Target bay start.",
+    )
+
+    parser.add_argument(
+        "run",
+        default="",
+        help="Run number, increases as we add attempts.",
+    )
+    args = parser.parse_args()
+
+    survey_manager_executer(args.command_name, args.bay, args.run)