localization_analysis remove extra flags + docs (#729)

* simplifying localization_analysis arguments based on env variables that need to be set for sparse_mapping anyway

* review comments

localization/localization_common/scripts/localization_common/utilities.py

@@ -73,11 +73,54 @@ def load_dataframe(files):
     return dataframe
 
 
-def run_command_and_save_output(command, output_filename, print_command=True):
+def run_command_and_save_output(command, output_filename="", print_command=True):
+    """
+    Run a command in the shell, save the output to a file, and print the output.
+
+    Parameters:
+        command (str): The shell command to be executed.
+        output_filename (str): (optional) The filename to save the command output. Default is an empty string.
+        print_command (bool): (optional) If True, print the command before executing. Default is True.
+
+    Returns:
+        tuple: A tuple containing the return code, standard output, and standard error.
+    """
+
     if print_command:
         print(command)
-    with open(output_filename, "w") as output_file:
-        subprocess.call(command, shell=True, stdout=output_file, stderr=output_file)
+
+    # If an output_filename is provided, open the file for writing
+    if output_filename != "":
+        f = open(output_filename, "w")
+
+    # Run the command in a subprocess and capture the output
+    stdout = ""
+    stderr = ""
+    popen = subprocess.Popen(
+        command,
+        shell=True,
+        stdout=subprocess.PIPE,
+        stderr=subprocess.STDOUT,
+        universal_newlines=True,
+    )
+    for stdout_line in iter(popen.stdout.readline, ""):
+        if output_filename != "":
+            f.write(stdout_line)
+        stdout += stdout_line
+
+    popen.stdout.close()
+
+    # Print the standard output and standard error
+    print(stdout)
+    print(stderr)
+
+    # Wait for the subprocess to finish and get the return code
+    return_code = popen.wait()
+    # If the return code is not 0, raise a subprocess.CalledProcessError
+    if return_code:
+        raise subprocess.CalledProcessError(return_code, command)
+
+    return (return_code, stdout, stderr)
 
 
 def basename(filename):

tools/localization_analysis/readme.md

@@ -2,6 +2,14 @@
 
 # Package Overview
 The localization analysis package provides several tools for measuring localization performance as described below.
+Before running these scripts make sure you define the appropriate env variables (the scripts will complain if you don't)
+Example:
+
+	export ASTROBEE_RESOURCE_DIR=$HOME/astrobee/src/astrobee/resources
+	export ASTROBEE_CONFIG_DIR=$HOME/astrobee/src/astrobee/config
+	export ASTROBEE_WORLD=iss
+	export ASTROBEE_ROBOT=bumble
+
 
 ## Usage Instructions
 For each tool and script, run `rosrun localization_analysis tool_or_script_name -h` for further details and 
@@ -38,8 +46,8 @@ on parameter and bag sweeps.
 Runs the graph bag tool in parallel on multiple bag files.  It takes config file with bag names, map names, and robot configs and produces pdfs and result bagfiles for each entry.
 Example config file:   
 ```
-/home/bag_name.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record /home/astrobee/src/astrobee config/robots/bumble.config iss false  
-/home/bag_name_2.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record /home/astrobee/src/astrobee config/robots/bumble.config iss false
+/home/bag_name.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record false
+/home/bag_name_2.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record false
 ```
 Example bag sweep command:  
 ```

tools/localization_analysis/scripts/bag_sweep.py

@@ -17,7 +17,9 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 """
-The bag sweep tool runs the graph bag tool in parallel on multiple bag files.  It takes a config file with bag names, map names, and robot configs and produces pdfs and result bagfiles for each entry.
+The bag sweep tool runs the graph bag tool in parallel on multiple bag files.
+It takes a config file with bag names, map names, and robot configs and produces
+pdfs and result bagfiles for each entry.
 """
 
 
@@ -39,9 +41,6 @@ def __init__(
         bagfile,
         map_file,
         image_topic,
-        config_path,
-        robot_config_file,
-        world,
         use_image_features,
         groundtruth_bagfile,
         rmse_rel_start_time,
@@ -50,9 +49,6 @@ def __init__(
         self.bagfile = bagfile
         self.map_file = map_file
         self.image_topic = image_topic
-        self.config_path = config_path
-        self.robot_config_file = robot_config_file
-        self.world = world
         self.use_image_features = use_image_features
         self.groundtruth_bagfile = groundtruth_bagfile
         self.rmse_rel_start_time = rmse_rel_start_time
@@ -73,9 +69,6 @@ def load_params(param_file):
                     row[4],
                     row[5],
                     row[6],
-                    row[7],
-                    row[8],
-                    row[9],
                 )
             )
 
@@ -123,16 +116,10 @@ def run_graph_bag(params, output_dir):
         + params.bagfile
         + " "
         + params.map_file
-        + " "
-        + params.config_path
         + " -i "
         + params.image_topic
         + " -o "
         + output_bag_path
-        + " -r "
-        + params.robot_config_file
-        + " -w "
-        + params.world
         + " -s "
         + output_csv_file
         + " -f "

tools/localization_analysis/scripts/groundtruth_sweep.py

@@ -39,20 +39,14 @@ def __init__(
         self,
         bagfile,
         base_surf_map,
-        maps_directory,
         loc_map,
-        config_path,
-        world,
         image_topic,
         robot_name,
         use_image_features,
     ):
         self.bagfile = bagfile
         self.base_surf_map = base_surf_map
-        self.maps_directory = maps_directory
         self.loc_map = loc_map
-        self.config_path = config_path
-        self.world = world
         self.image_topic = image_topic
         self.robot_name = robot_name
         self.use_image_features = use_image_features
@@ -71,9 +65,6 @@ def load_params(param_file):
                     row[3],
                     row[4],
                     row[5],
-                    row[6],
-                    row[7],
-                    row[8],
                 )
             )
 
@@ -108,19 +99,11 @@ def run_groundtruth(params):
         + " "
         + params.base_surf_map
         + " "
-        + params.maps_directory
-        + " "
         + params.loc_map
-        + " "
-        + params.config_path
         + " -o "
         + output_directory
-        + " -w "
-        + params.world
         + " -i "
         + params.image_topic
-        + " -r "
-        + params.robot_name
     )
     if not bool(params.use_image_features):
         groundtruth_command += " --generate-image-features"

tools/localization_analysis/scripts/make_groundtruth.py

@@ -41,24 +41,19 @@
         help="Existing map to use as basis for groundtruth.  Should largely overlap area covered in input bagfile.",
     )
     parser.add_argument(
-        "maps_directory",
-        help="Location of images used for each bagfile use to generate base_surf_map.",
+        "--loc-map",
+        default="",
+        help="Full path to Localization map for bagfile to test localization performance. If not passed the localization test is not run",
     )
-    parser.add_argument(
-        "loc_map", help="Localization map for bagfile to test localization performance."
-    )
-    parser.add_argument("config_path", help="Full path to config path.")
     parser.add_argument(
         "-o", "--output-directory", default="groundtruth_creation_output"
     )
-    parser.add_argument("-w", "--world", default="iss")
     parser.add_argument(
         "-i",
         "--image-topic",
         default="/mgt/img_sampler/nav_cam/image_record",
         help="Image topic.",
     )
-    parser.add_argument("-r", "--robot-name", default="bumble")
     parser.add_argument(
         "-m",
         "--map-name",
@@ -92,19 +87,17 @@
     if not os.path.isfile(args.base_surf_map):
         print("Base surf map " + args.base_surf_map + " does not exist.")
         sys.exit()
-    if not os.path.isfile(args.loc_map):
-        print("Loc map " + args.loc_map + " does not exist.")
-        sys.exit()
-    if not os.path.isdir(args.maps_directory):
-        print("Maps directory " + args.maps_directory + " does not exist.")
+    if args.loc_map == "":
+        print("Not running map localization comparison part that part")
+    elif not os.path.isfile(args.loc_map):
+        print("Loc map does not exist")
         sys.exit()
     if os.path.isdir(args.output_directory):
         print("Output directory " + args.output_directory + " already exists.")
         sys.exit()
 
     bagfile = os.path.abspath(args.bagfile)
     base_surf_map = os.path.abspath(args.base_surf_map)
-    maps_directory = os.path.abspath(args.maps_directory)
 
     os.mkdir(args.output_directory)
     os.chdir(args.output_directory)
@@ -117,15 +110,11 @@
     make_map.make_map(
         bagfile,
         map_name,
-        args.world,
-        args.robot_name,
         args.histogram_equalization,
         args.max_low_movement_mean_distance,
         base_surf_map,
-        maps_directory,
     )
 
-    robot_config = "config/robots/" + args.robot_name + ".config"
     groundtruth_bag = map_name + ".bag"
     groundtruth_map_file = map_name + ".brisk.vocabdb.map"
     groundtruth_pdf = "groundtruth.pdf"
@@ -135,14 +124,8 @@
         + bagfile
         + " "
         + groundtruth_map_file
-        + " "
-        + args.config_path
         + " -i "
         + args.image_topic
-        + " -r "
-        + robot_config
-        + " -w "
-        + args.world
         + " -o "
         + groundtruth_bag
         + " --output-file "
@@ -154,32 +137,27 @@
     lu.run_command_and_save_output(make_groundtruth_command, "make_groundtruth.txt")
     os.rename("run_graph_bag_command.txt", "groundtruth_run_graph_bag_command.txt")
 
-    loc_results_bag = bag_prefix + "_results.bag"
-    loc_pdf = "loc_results.pdf"
-    loc_csv = "loc_results.csv"
-    get_loc_results_command = (
-        "rosrun localization_analysis run_graph_bag_and_plot_results.py "
-        + bagfile
-        + " "
-        + args.loc_map
-        + " "
-        + args.config_path
-        + " -i "
-        + args.image_topic
-        + " -r "
-        + robot_config
-        + " -w "
-        + args.world
-        + " -o "
-        + loc_results_bag
-        + " --output-file "
-        + loc_pdf
-        + " --output-csv-file "
-        + loc_csv
-        + " -g "
-        + groundtruth_bag
-    )
-    if not args.use_image_features:
-        get_loc_results_command += " --generate-image-features"
-    lu.run_command_and_save_output(get_loc_results_command, "get_loc_results.txt")
-    os.rename("run_graph_bag_command.txt", "loc_run_graph_bag_command.txt")
+    if args.loc_map != "":
+        loc_results_bag = bag_prefix + "_results.bag"
+        loc_pdf = "loc_results.pdf"
+        loc_csv = "loc_results.csv"
+        get_loc_results_command = (
+            "rosrun localization_analysis run_graph_bag_and_plot_results.py "
+            + bagfile
+            + " "
+            + args.loc_map
+            + " -i "
+            + args.image_topic
+            + " -o "
+            + loc_results_bag
+            + " --output-file "
+            + loc_pdf
+            + " --output-csv-file "
+            + loc_csv
+            + " -g "
+            + groundtruth_bag
+        )
+        if not args.use_image_features:
+            get_loc_results_command += " --generate-image-features"
+        lu.run_command_and_save_output(get_loc_results_command, "get_loc_results.txt")
+        os.rename("run_graph_bag_command.txt", "loc_run_graph_bag_command.txt")