diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index a131ef1a..ea7d22a7 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/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); } diff --git a/astrobee/survey_manager/CMakeLists.txt b/astrobee/survey_manager/CMakeLists.txt new file mode 100644 index 00000000..93daf02c --- /dev/null +++ b/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}) \ No newline at end of file diff --git a/astrobee/survey_manager/package.xml b/astrobee/survey_manager/package.xml new file mode 100644 index 00000000..ecb3d537 --- /dev/null +++ b/astrobee/survey_manager/package.xml @@ -0,0 +1,21 @@ + + + survey_manager + 0.0.0 + The survey_manager package + + Apache License, Version 2.0 + + + ISAAC Flight Software + + + ISAAC Flight Software + + + catkin + + + + + diff --git a/astrobee/survey_manager/scripts/command_astrobee b/astrobee/survey_manager/scripts/command_astrobee new file mode 100755 index 00000000..0fc40659 --- /dev/null +++ b/astrobee/survey_manager/scripts/command_astrobee @@ -0,0 +1,146 @@ +#!/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'.") + + +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( + "arg1", + default="", + help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + ) + parser.add_argument( + "arg2", + default="", + help="Prefix for bagfiles to merge. Bags should all be in the current working directory.", + ) + 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")