diff --git a/.rosinstall b/.rosinstall index 734de65c..ae265453 100755 --- a/.rosinstall +++ b/.rosinstall @@ -35,6 +35,11 @@ uri: https://github.com/Arena-Rosnav/arena-simulation-setup.git version: v2.0.0 +- git: + local-name: ../arena-evaluation + uri: https://github.com/Arena-Rosnav/arena-evaluation.git + version: master + ### PLANNERS - git: diff --git a/evaluation/.gitignore b/evaluation/.gitignore deleted file mode 100644 index 54df0e08..00000000 --- a/evaluation/.gitignore +++ /dev/null @@ -1,8 +0,0 @@ -*/**/*.csv -*/**/*.zip -.vscode -test.ipynb -*/**/params.yaml -/**/__pycache__ -/data/** -!/data/.gitkeep \ No newline at end of file diff --git a/evaluation/CMakeLists.txt b/evaluation/CMakeLists.txt deleted file mode 100644 index 82033727..00000000 --- a/evaluation/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(arena-evaluation) - -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - nav_msgs - roscpp - rospy - sensor_msgs - std_msgs -) -catkin_package() -include_directories(${catkin_INCLUDE_DIRS}) \ No newline at end of file diff --git a/evaluation/README.md b/evaluation/README.md deleted file mode 100644 index c899e583..00000000 --- a/evaluation/README.md +++ /dev/null @@ -1,118 +0,0 @@ -# Arena Evaluation -![](http://img.shields.io/badge/stability-stable-orange.svg?style=flat) -[![Linux](https://svgshare.com/i/Zhy.svg)](https://svgshare.com/i/Zhy.svg) -[![support level: consortium / vendor](https://img.shields.io/badge/support%20level-consortium%20/%20vendor-brightgreen.svg)](http://rosindustrial.org/news/2016/10/7/better-supporting-a-growing-ros-industrial-software-platform) -> 🚧 This project is still under development - -The Arena Evaluation package provides tools to record, evaluate, and plot navigational metrics to evaluate ROS navigation planners. It is best suited for usage with our [arena-rosnav repository](https://github.com/ignc-research/arena-rosnav) but can also be integrated into any other ROS-based project. - -It consists of 3 parts: -- [Data recording](#01-data-recording) -- [Data transformation and evaluation](#02-data-transformation-and-evaluation) -- [Plotting](#03-plotting) - - - -## General - -- To integrate arena evaluation into your project, see the guide [here](docs/integration-requirements.md) -- To use it along side with the arena repository, install the following requirements: - -```bash -pip install scikit-learn seaborn pandas matplotlib -``` -## 01 Data Recording -To record data as csv file while doing evaluation runs set the flag `use_recorder:="true"` in your `roslaunch` command. For example: - -```bash -workon rosnav -roslaunch arena_bringup start_arena_gazebo.launch world:="aws_house" scenario_file:="aws_house_obs05.json" local_planner:="teb" model:="turtlebot3_burger" use_recorder:="true" -``` - -The data will be recorded in `.../catkin_ws/src/forks/arena-evaluation/01_recording`. -The script stops recording as soon as the agent finishes the scenario and stops moving or after termination criterion is met. Termination criterion as well as recording frequency can be set in `data_recorder_config.yaml`. - -```yaml -max_episodes: 15 # terminates simulation upon reaching xth episode -max_time: 1200 # terminates simulation after x seconds -record_frequency: 0.2 # time between actions recorded -``` - -> **NOTE**: Leaving the simulation running for a long time after finishing the set number of repetitions does not influence the evaluation results as long as the agent stops running. Also, the last episode of every evaluation run is pruned before evaluating the recorded data. - -> **NOTE**: Sometimes csv files will be ignored by git so you have to use git add -f . We recommend using the code below. -```bash -roscd arena-evaluation && git add -f . -git commit -m "evaluation run" -git pull -git push -``` - -## 02 Data Transformation and Evaluation -1. After finishing all the evaluation runs, recording the desired csv files and run the `get_metrics.py` script in `/02_evaluation`. -This script will evaluate the raw data recorded from the evaluation and store it (or them) `.ftr` file with the following naming convention: `data____.ftr`. During this process all the csv files will be moved from `/01_recording` to `/02_evaluation` into a directory with the naming convention `data_`. The ftr file will be stored in `/02_evaluation`.\ - Some configurations can be set in the `get_metrics_config.yaml` file. Those are: - - `robot_radius`: dictionary of robot radii, relevant for collision measurement - - `time_out_treshold`: treshold for episode timeout in seconds - - `collision_treshold`: treshold for allowed number of collisions until episode deemed as failed - > **NOTE**: Do NOT change the `get_metrics_config_default.yaml`!\ - We recommend using the code below:\ - ```bash - workon rosnav && roscd arena-evaluation/02_evaluation && python get_metrics.py - ``` - > **NOTE**: If you want to reuse csv files, simply move the desired csv files from the data directory to `/01_recording` and execute the `get_metrics.py` script again. - - The repository can be used in two ways: - - Firstly it can be used to evaluate the robot performance within the scenario run, e.g visualizing the velocity distribution within each simulation run (this usage mode is currently still under development). - - Secondly, it can be used to evaluate the robot performance compare robot performance between different scenarios. For this use-case continue with the following step 2. -2. The observations of the individual runs can be joined into one large dataset, using the following script: - ```bash - workon rosnav && roscd arena-evaluation/02_evaluation && python combine_into_one_dataset.py - ``` - This script will combine all ftr files in the `02_evaluation/ftr_data` folder into one large ftr file, taking into account the planner, robot etc. -## 03 Plotting -The data prepared in the previous steps can be visualized with two different modes, the automated or the custom mode. - -### Custom Plotting (recommended) -Open the following [notebook](03_plotting/data_visualization.ipynb) to visualize your data. It contains a step-by-step guide on how to create an accurate visual representation of your data. For examples of supported plots (and when to use which plot), refer to the documentation [here](docs/plotting_examples.md). - -### Automated Plotting (in development) - - - -# Mesure complexity of you map -1. run: `roscd arena-evaluation` -2. run: `python world_complexity.py --image_path {IMAGE_PATH} --yaml_path {YAML_PATH} --dest_path {DEST_PATH}` - -with:\ - IMAGE_PATH: path to the floor plan of your world. Usually in .pgm format\ - YAML_PATH: path to the .yaml description file of your floor plan\ - DEST_PATH: location to store the complexity data about your map - -Example launch: -```bash -python world_complexity.py --image_path ~/catkin_ws/src/forks/arena-tools/aws_house/map.pgm --yaml_path ~/catkin_ws/src/forks/arena-tools/aws_house/map.yaml --dest_path ~/catkin_ws/src/forks/arena-tools/aws_house -``` diff --git a/evaluation/create_plots.py b/evaluation/create_plots.py deleted file mode 100644 index 21726ccb..00000000 --- a/evaluation/create_plots.py +++ /dev/null @@ -1,579 +0,0 @@ -import seaborn as sns -import pandas as pd -import rospkg -import os -import numpy as np -import argparse -import matplotlib.pyplot as plt -import json -import yaml - -from utils import Utils - -""" - TODO: - - Add collisions to path map -""" - - -## Create plots for only one dataset - -aggregate_callbacks = { - "max": max, - "min": min, - "sum": sum, - "mean": lambda x: sum(x) / len(x) -} - -DIST_PLOTS = { - "strip": sns.stripplot, - "swarm": sns.swarmplot, - "box": sns.boxplot, - "boxen": sns.boxenplot, - "violin": sns.violinplot -} - -CAT_PLOTS = { - "line": sns.lineplot, - "bar": sns.barplot -} - -SHOULD_SAVE_PLOTS_KEY = "SHOULD_SAVE_PLOTS" -SAVE_PLOTS_LOCATION = "SAVE_PLOTS_LOCATION" - -def assert_dist_plot(key): - assert key in DIST_PLOTS, f"Invalid plot {key} for distribution" - -def assert_cat_plot(key): - assert key in CAT_PLOTS, f"Invalid plot {key} for categorical" - -def assert_datakey_valid(key, valid_keys): - assert key in valid_keys, f"Key {key} not valid" - -def plot(title, save_name): - plt.legend() - plt.title(title) - - if os.environ.get(SHOULD_SAVE_PLOTS_KEY, "False") == "True": - plt.savefig(os.path.join(os.environ.get(SAVE_PLOTS_LOCATION), save_name + ".pdf")) - else: - plt.show() - - plt.close() - -""" - -METRIC FILE SCHEMA: - -episode INT INCREMENT # Index of the episode -curvature FLOAT[] # (N - 2) menger curvatures -normalized_curvature FLOAT[] # (N - 2) Normalized curvature -roughness FLOAT[] # (N - 2) Roughness -path_length_values FLOAT[] # (N - 1) Length of the path the robot travelled in this step -path_length FLOAT # Complete path length of the episode -acceleration FLOAT[] # (N - 1) Difference of velocities -jerk FLOAT[] # (N - 2) Rate at which the acceleration changes -velocity FLOAT[][] # (N) Real velocity of the robot -cmd_vel FLOAT[][] # (N) Desired velocity, output of the network -collision_amount INT # Sum of collisions in this episode -collisions INT[] # (unknonw) Index of all collisions -path FLOAT[][] # (N - 1) Positions for each timestep -angle_over_length FLOAT # Mean change of the angle over the complete path -action_type (MOVE | STOP | ROTATE)[] # Action type of the robot for each timestep -time_diff INT # Time the episode took, ros time in ns -time INT[] # Time of each step -result TIMEOUT | GOAL_REACHED | COLLISION # Wether an episode was successful or not - -""" - -# Read in all metric files -# check if all metrics use the same map -# concatenate all files in big dataset - -def read_datasets(data_paths): - - datasets = [] - scenarios = [] - - for path in data_paths: - base_path = os.path.join("data", path) - metrics = os.path.join(base_path, "metrics.csv") - params = os.path.join(base_path, "params.yaml") - - assert os.path.exists( - metrics - ) and os.path.exists(params), "Metrics or params file does not exist" - - with open(params) as file: - params_content = yaml.safe_load(file) - - scenarios.append(params_content["scenario_file"]) - - dataset = pd.read_csv(metrics, converters={ - "path_length_values": Utils.string_to_float_list, - "curvature": Utils.string_to_float_list, - "roughness": Utils.string_to_float_list, - "velocity": Utils.string_to_float_list, - "jerk": Utils.string_to_float_list, - "start": Utils.string_to_float_list, - "goal": Utils.string_to_float_list, - "time": Utils.string_to_float_list, - "acceleration": lambda a: json.loads(a), - "path": lambda a: json.loads(a), - }) - - # Set parameters in dataset coloumns for better differentiation - dataset["local_planner"] = params_content["local_planner"] - dataset["agent_name"] = params_content["agent_name"] - dataset["model"] = params_content["model"] - dataset["namespace"] = params_content["namespace"] - - datasets.append(dataset) - - # If datasets used different scenario files a comparison makes no sense - assert len(set(scenarios)) == 1, "Scenario files are not the same" - - return pd.concat(datasets), scenarios[0] - - -## FOR RESULT - -class ResultPlotter: - @staticmethod - def countplot_for_result(dataset, hue="namespace", title="Results", save_name="results", plot_args={}): - """ - Shows the results for every episode in a count plot to compare - with different robots - """ - sns.countplot(data=dataset, x="result", hue=hue, **plot_args) - - plot(title, save_name) - - @staticmethod - def plot_result_from_declaration(dataset, result_declaration): - if result_declaration == None: - return - - ResultPlotter.countplot_for_result( - dataset, - result_declaration["title"], - result_declaration["save_name"], - result_declaration.get("plot_args", {}), - ) - -## FOR TIME STEP VALUES -> ARRAYS FOR EACH EPISODE -# curvature, normalized_curvature, roughness, path_length_value, acceleration, velocity - - - - -class EpisodeArrayValuePlotter: - """ - This class is used to plot coloumns, which are arrays for single episodes - That means, values that are collected in every timestep - This includes all coloumns listed in POSSIBLE_DATA_KEYS - """ - POSSIBLE_DATA_KEYS = [ - "curvature", - "normalized_curvature", - "roughness", - "path_length_values", - "acceleration", - "jerk", - "velocity" - ] - - def lineplot_for_single_episode(dataset, data_key, title, save_name, step_size=5, hue="namespace", episode=None, plot_args={}): - """ - Creates a lineplot to visualize the values for a single episode. - - Args: - data_key: str -> Name of the coloumn you want to plot - step_size: int -> Number of values that should be skipped when plotting to reduce data size - hue: str -> Name of the coloumn to differentiate - episode: int | None -> Index of the episode, if none then plot mean of all episodes - plot_args: dict -> further plot arguments - """ - assert_datakey_valid(data_key, EpisodeArrayValuePlotter.POSSIBLE_DATA_KEYS) - - local_data = dataset[[data_key, hue, "time", "episode"]] - - if episode != None: - local_data = local_data[local_data["episode"] == episode] - - local_data = local_data.apply(lambda x: EpisodeArrayValuePlotter.resize_time(x, data_key, step_size), axis=1) - - local_data = local_data.explode([data_key, "time"]).reset_index() - - sns.lineplot(data=local_data, y=data_key, x="time", hue=hue, **plot_args) - - plot(title, save_name) - - def distplot_for_single_episode(dataset, data_key, title, save_name, episode=0, plot_key="swarm", plot_args={}): - """ - Create a distributional plot for a single episode - """ - assert_datakey_valid(data_key, EpisodeArrayValuePlotter.POSSIBLE_DATA_KEYS) - - assert_dist_plot(plot_key) - - local_data = dataset[dataset["episode"] == episode][[data_key, "namespace"]].explode(data_key) - - DIST_PLOTS[plot_key](data=local_data, y=data_key, x="namespace", **plot_args) - - plot(title, save_name) - - def distplot_for_aggregated(dataset, data_key, aggregate_callback, title, save_name, differentiate="namespace", plot_key="swarm", plot_args={}): - assert_datakey_valid(data_key, EpisodeArrayValuePlotter.POSSIBLE_DATA_KEYS) - assert_dist_plot(plot_key) - - local_data = dataset[[data_key, differentiate]] - - def aggregate_value(row): - row[data_key] = aggregate_callback(row[data_key]) - - return row - - local_data = local_data.apply(aggregate_value, axis=1) - - DIST_PLOTS[plot_key](data=local_data, y=data_key, x=differentiate, **plot_args) - - plot(title, save_name) - - def lineplot_for_aggregated(dataset, data_key, aggregate_callback, title, save_name, differentiate="namespace", plot_args={}): - assert_datakey_valid(data_key, EpisodeArrayValuePlotter.POSSIBLE_DATA_KEYS) - - local_data = dataset[[data_key, differentiate, "episode"]].reset_index() - - def aggregate_value(row): - row[data_key] = aggregate_callback(row[data_key]) - - return row - - local_data = local_data.apply(aggregate_value, axis=1) - - sns.lineplot(data=local_data, x="episode", y=data_key, hue=differentiate, **plot_args) - - plot(title, save_name) - - def resize_time(row, key_reference, step_size): - time_null = row["time"][0] - - row["time"] = list(map(lambda x: int((x - time_null) / 10e8), row["time"])) - row["time"] = row["time"][0:len(row[key_reference])] - - row["time"] = row["time"][0::step_size] - row[key_reference] = row[key_reference][0::step_size] - - return row - - -## FOR DISCRETE VALUES IN EPISODE - -class DiscreteValuePlotter: - POSSIBLE_VALUES = [ - "time_diff", - "angle_over_length", - "collision_amount", - "path_length" - ] - - def catplot_over_episodes(dataset, data_key, title, save_name, differentiate="namespace", plot_key="line", plot_args={}): - assert_datakey_valid(data_key, DiscreteValuePlotter.POSSIBLE_VALUES) - assert_cat_plot(plot_key) - - CAT_PLOTS[plot_key](data=dataset.reset_index(), x="episode", y=data_key, hue=differentiate, **plot_args) - - plot(title, save_name) - - def distplot_over_episodes(dataset, data_key, title, save_name, differentiate="namespace", plot_key="swarm", plot_args={}): - assert_datakey_valid(data_key, DiscreteValuePlotter.POSSIBLE_VALUES) - assert_dist_plot(plot_key) - - DIST_PLOTS[plot_key](data=dataset.reset_index(), y=data_key, x=differentiate, **plot_args) - - plot(title, save_name) - - -## FOR SHOWING PATH THE ROBOT TOOK - -class PathVisualizer: - - def __init__(self, scenario): - self.scenario_file, self.scenario_content = PathVisualizer.read_scenario_file(scenario) - - self.map_name = self.scenario_content["map"] - self.map_path, self.map_content = PathVisualizer.read_map_file(self.map_name) - - print(self.map_content) - - def create_map_plot(self): - map_img = plt.imread(os.path.join(self.map_path, self.map_content["image"])) - - fig, ax = plt.subplots() - - ax.imshow(map_img) - - return fig, ax - - def create_episode_plots_for_namespaces(self, dataset, title, save_name, desired_results=[], should_add_obstacles=False, should_add_collisions=False): - robots_tested = list(set(dataset["namespace"].to_list())) - - plt.close() - - for namespace in robots_tested: - fig, ax = self.create_map_plot() - - paths_for_namespace = dataset[dataset["namespace"] == namespace][["path", "result", "start", "goal"]] - - path_amount = len(paths_for_namespace.index) - - for i in range(path_amount): - path = paths_for_namespace["path"][i] - result = paths_for_namespace["result"][i] - - if len(desired_results) > 0 and not result in desired_results: - break - - path = np.array(list(map(self.ros_to_real_coord, path))).transpose() - - ax.plot(path[0][:-10], path[1][:-10], label="Episode: " + str(i)) - - self.add_obstacles_to_plot(should_add_obstacles) - - self.add_start_and_goal_to_plot(ax, paths_for_namespace["start"][0], paths_for_namespace["goal"][0]) - - plot(title, save_name) - - - def create_best_plots(self, dataset, should_add_obstacles=True, should_add_collisions=False): - """ - - Only plot the paths were the goal is reached - - Plot the path that took the least amount of time - """ - possible_paths = dataset[dataset["result"] == "GOAL_REACHED"] - - namespaces = list(set(possible_paths["namespace"].to_list())) - - fig, ax = self.create_map_plot() - - for namespace in namespaces: - paths_for_namespace = possible_paths[possible_paths["namespace"] == namespace] - - minimal_path = paths_for_namespace[paths_for_namespace["time_diff"] == paths_for_namespace["time_diff"].min()][["path", "start", "goal"]].reset_index() - - print(minimal_path) - - path = np.array(list(map(self.ros_to_real_coord, minimal_path["path"][0]))).transpose() - - ax.plot(path[0][:-10], path[1][:-10], label=namespace) - - self.add_start_and_goal_to_plot(ax, minimal_path["start"][0], minimal_path["goal"][0]) - - self.add_obstacles_to_plot(ax, should_add_obstacles) - - # TODO CREATE PLOT TITLE - - plt.legend() - plt.show() - plt.close() - - def add_obstacles_to_plot(self, ax, should_add_obstacles=False): - if not should_add_obstacles: - return - - obstacles = self.scenario_content["obstacles"] - - for o in obstacles["static"]: - position = o["pos"] - map_coordinates = self.ros_to_real_coord(position) - - ax.plot(map_coordinates[0], map_coordinates[1], "o--", color="black", ms=15, alpha=0.2) - - for o in obstacles["dynamic"]: - verts = [self.ros_to_real_coord(waypoint) for waypoint in o["waypoints"]] - verts.append(verts[0]) - - x, y = zip(*verts) - - ax.plot(x, y, "x--", lw=1, color="black", ms=10, alpha=0.4) - - def add_start_and_goal_to_plot(self, ax, start, goal): - print(start) - - start = self.ros_to_real_coord(start) - - ax.plot(start[0], start[1], "x", label="Start", ms=10, color="green") - - goal = self.ros_to_real_coord(goal) - - ax.plot(goal[0], goal[1], "*", label="Goal", ms=10, color="red") - - @staticmethod - def read_scenario_file(scenario): - name = os.path.join(rospkg.RosPack().get_path("task-generator"), "scenarios", scenario) - - with open(name) as file: - content = yaml.safe_load(file) - - return name, content - - @staticmethod - def read_map_file(map_name): - map_path = os.path.join(rospkg.RosPack().get_path("arena-simulation-setup"), "maps", map_name) - - with open(os.path.join(map_path, "map.yaml")) as file: - content = yaml.safe_load(file) - - return map_path, content - - def ros_to_real_coord(self, coord): - return [i / self.map_content["resolution"] for i in coord][:2] - - -def create_plots_from_declaration_file(declaration_file): - ## Show plots setup - - show_plots = declaration_file["show_plots"] - - if not show_plots: - os.environ[SHOULD_SAVE_PLOTS_KEY] = "True" - location = os.path.join("path", declaration_file["save_location"]) - os.environ[SAVE_PLOTS_LOCATION] = location - - try: - os.mkdir(location) - except: - print("Path", location, "cannot be created") - - ## Dataset setup - - dataset, scenario = read_datasets(declaration_file["datasets"]) - - ## Plot Result - - ResultPlotter(dataset, declaration_file.get("result", None)) - - ## Plot time step values - - single_episode_line = declaration_file.get("single_episode_line", []) - - for line in single_episode_line: - EpisodeArrayValuePlotter.lineplot_for_single_episode( - dataset, - line["data_key"], - line["title"], - line["save_name"], - step_size=line.get("step_size", 5), - hue=line.get("hue", "namespace"), - episode=line.get("episode", None), - plot_args=line.get("plot_args", {}) - ) - - single_episode_distribution = declaration_file.get("single_episode_distribution", []) - - for line in single_episode_distribution: - EpisodeArrayValuePlotter.distplot_for_single_episode( - dataset, - line["data_key"], - line["title"], - line["save_name"], - episode=line["episode"], - plot_key=line.get("plot_key", "swarm"), - plot_args=line.get("plot_args", {}) - ) - - aggregated_distribution = declaration_file.get("aggregated_distribution", []) - - for line in aggregated_distribution: - EpisodeArrayValuePlotter.distplot_for_aggregated( - dataset, - line["data_key"], - aggregate_callbacks[line["aggregate"]], - line["title"], - line["save_name"], - plot_key=line.get("plot_key", "swarm"), - plot_args=line.get("plot_args", {}) - ) - - aggreagted_line = declaration_file.get("aggregated_line", []) - - for line in aggreagted_line: - EpisodeArrayValuePlotter.lineplot_for_aggregated( - dataset, - line["data_key"], - aggregate_callbacks[line["aggregate"]], - line["title"], - line["save_name"], - plot_args=line.get("plot_args", {}) - ) - - # Plot episode values - - all_episodes_categorical = declaration_file.get("all_episodes_categorical", []) - - for line in all_episodes_categorical: - DiscreteValuePlotter.catplot_over_episodes( - dataset, - line["data_key"], - line["title"], - line["save_name"], - plot_key=line["plot_key"], - plot_args=line.get("plot_args", {}) - ) - - all_episodes_distribution = declaration_file.get("all_episodes_distribution", []) - - for line in all_episodes_distribution: - DiscreteValuePlotter.distplot_over_episodes( - dataset, - line["data_key"], - line["title"], - line["save_name"], - plot_key=line["plot_key"], - plot_args=line.get("plot_args", {}) - ) - - # Plot paths - - path_visualizer = PathVisualizer(scenario) - - episode_plots_for_namespaces = declaration_file.get(episode_plots_for_namespaces, None) - - if episode_plots_for_namespaces != None: - path_visualizer.create_episode_plots_for_namespaces( - dataset, - episode_plots_for_namespaces["title"], - episode_plots_for_namespaces["save_name"], - desired_results=episode_plots_for_namespaces["desired_results"], - should_add_obstacles=episode_plots_for_namespaces.get("should_add_obstacles", False), - should_add_collisions=episode_plots_for_namespaces.get("should_add_collisions", False), - ) - - create_best_plots = declaration_file.get(create_best_plots, None) - - if create_best_plots != None: - path_visualizer.create_best_plots( - dataset, - create_best_plots["title"], - create_best_plots["save_name"], - should_add_obstacles=create_best_plots.get("should_add_obstacles", False), - should_add_collisions=create_best_plots.get("should_add_collisions", False), - ) - - -def parse_args(): - parser = argparse.ArgumentParser() - - parser.add_argument("declaration_file") - - return parser.parse_args() - - -if __name__ == "__main__": - args = parse_args() - - with open(os.path.join("plot_declarations", args.declaration_file)) as file: - declaration_file = yaml.safe_load(file) - - create_plots_from_declaration_file(declaration_file) \ No newline at end of file diff --git a/evaluation/data/.gitkeep b/evaluation/data/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/evaluation/data_recorder_config.yaml b/evaluation/data_recorder_config.yaml deleted file mode 100644 index 90ece55f..00000000 --- a/evaluation/data_recorder_config.yaml +++ /dev/null @@ -1,3 +0,0 @@ -max_episodes: 15 # terminates simulation upon reaching xth episode -max_time: 1200 # terminates simulation after x seconds -record_frequency: 400 # time interval in which data is stored in ms diff --git a/evaluation/get_metrics.py b/evaluation/get_metrics.py deleted file mode 100644 index 9f38c2fc..00000000 --- a/evaluation/get_metrics.py +++ /dev/null @@ -1,381 +0,0 @@ -#!/usr/bin/env python3 -""" -This file is used to calculate from the simulation data, various metrics, such as -- did a collision occur -- how long did the robot take form start to goal -the metrics / evaluation data will be saved to be preproccesed in the next step -""" -import numpy as np -import pandas as pd -import os -import yaml -import argparse -import rospkg -import json - -from utils import Utils - - -def parse_args(): - parser = argparse.ArgumentParser() - - parser.add_argument("--dir", "-d") - - return parser.parse_args() - - -class Action: - STOP = "STOP" - ROTATE = "ROTATE" - MOVE = "MOVE" - - -class DoneReason: - TIMEOUT = "TIMEOUT" - GOAL_REACHED = "GOAL_REACHED" - COLLISION = "COLLISION" - - -class Config: - TIMEOUT_TRESHOLD = 180e9 - MAX_COLLISIONS = 3 - - -class Metrics: - def __init__(self, dir): - self.dir = dir - - self.robot_params = Metrics.get_robot_params(self.dir) - - episode = pd.read_csv(self.dir + "/episode.csv", converters={ - "data": lambda val: 0 if len(val) <= 0 else int(val) - }) - laserscan = pd.read_csv(self.dir + "/scan.csv", converters={ - "data": Utils.string_to_float_list - }) - odom = pd.read_csv(self.dir + "/odom.csv", converters={ - "data": lambda col: json.loads(col.replace("'", "\"")) - }) - cmd_vel = pd.read_csv(self.dir + "/cmd_vel.csv", converters={ - "data": Utils.string_to_float_list - }) - start_goal = pd.read_csv(self.dir + "/start_goal.csv", converters={ - "start": Utils.string_to_float_list, - "goal": Utils.string_to_float_list - }) - - laserscan = laserscan.rename(columns={"data": "laserscan"}) - odom = odom.rename(columns={"data": "odom"}) - cmd_vel = cmd_vel.rename(columns={"data": "cmd_vel"}) - - data = pd.concat([episode, laserscan, odom, cmd_vel, start_goal], axis=1, join="inner") - data = data.loc[:,~data.columns.duplicated()].copy() - - i = 0 - - episode_data = {} - - while True: - current_episode = data[data["episode"] == i] - - if len(current_episode) <= 0: - break - - episode_data[i] = self.analyze_episode(current_episode, i) - i = i + 1 - - data = pd.DataFrame(episode_data).transpose().set_index("episode") - data.to_csv(os.path.join(dir, "metrics.csv")) - - pass - - def analyze_episode(self, episode, index): - positions, velocities = [], [] - - for odom in episode["odom"]: - positions.append(np.array(odom["position"])) - velocities.append(np.array(odom["velocity"])) - - curvature, normalized_curvature = self.get_curvature(np.array(positions)) - roughness = self.get_roughness(np.array(positions)) - - vel_absolute = self.get_velocity_abs(velocities) - acceleration = self.get_acceleration(vel_absolute) - jerk = self.get_jerk(vel_absolute) - - collisions, collision_amount = self.get_collisions( - episode["laserscan"], - self.robot_params["robot_radius"] - ) - - path_length, path_length_per_step = self.get_path_length(positions) - - time = int(list(episode["time"])[-1] - list(episode["time"])[0]) - - start_position = self.get_mean_position(episode, "start") - goal_position = self.get_mean_position(episode, "goal") - - return { - "curvature": Metrics.round_values(curvature), - "normalized_curvature": Metrics.round_values(normalized_curvature), - "roughness": Metrics.round_values(roughness), - "path_length_values": Metrics.round_values(path_length_per_step), - "path_length": path_length, - "acceleration": Metrics.round_values(acceleration), - "jerk": Metrics.round_values(jerk), - "velocity": Metrics.round_values(vel_absolute), - "collision_amount": collision_amount, - "collisions": list(collisions), - "path": [list(p) for p in positions], - "angle_over_length": self.get_angle_over_length(path_length, positions), - "action_type": list(self.get_action_type(episode["cmd_vel"])), - ## Ros time in ns - "time_diff": time, - "time": list(map(int, episode["time"].tolist())), - "episode": index, - "result": self.get_success(time, collision_amount), - "cmd_vel": list(map(list, episode["cmd_vel"].to_list())), - "goal": goal_position, - "start": start_position - } - - def get_mean_position(self, episode, key): - positions = episode[key].to_list() - counter = {} - - for p in positions: - hash = ":".join([str(pos) for pos in p]) - - counter[hash] = counter.get(hash, 0) + 1 - - sorted_positions = dict(sorted(counter.items(), key=lambda x: x)) - - return [float(r) for r in list(sorted_positions.keys())[0].split(":")] - - def get_position_for_collision(self, collisions, positions): - for i, collision in enumerate(collisions): - collisions[i][2] = positions[collision[0]] - - return collisions - - def get_angle_over_length(self, path_length, positions): - total_yaw = 0 - - for i, yaw in enumerate(positions[:-1]): - yaw = yaw[2] - next_yaw = positions[i + 1][2] - - total_yaw += abs(next_yaw - yaw) - - return total_yaw / path_length - - def get_success(self, time, collisions): - if time >= Config.TIMEOUT_TRESHOLD: - return DoneReason.TIMEOUT - - if collisions >= Config.MAX_COLLISIONS: - return DoneReason.COLLISION - - return DoneReason.GOAL_REACHED - - def get_path_length(self, positions): - path_length = 0 - path_length_per_step = [] - - for i, position in enumerate(positions[:-1]): - next_position = positions[i + 1] - - step_path_length = np.linalg.norm(position - next_position) - - path_length_per_step.append(step_path_length) - path_length += step_path_length - - return path_length, path_length_per_step - - def get_collisions(self, laser_scans, lower_bound): - """ - Calculates the collisions. Therefore, - the laser scans is examinated and all values below a - specific range are marked as collision. - - Argument: - - Array laser scans representing the scans over - time - - the lower bound for which a collisions are counted - - Returns tupel of: - - Array of tuples with indexs and time in which - a collision happened - """ - collisions = [] - collisions_marker = [] - - for i, scan in enumerate(laser_scans): - - is_collision = len(scan[scan <= lower_bound]) > 0 - - collisions_marker.append(int(is_collision)) - - if is_collision: - collisions.append(i) - - collision_amount = 0 - - for i, coll in enumerate(collisions_marker[1:]): - prev_coll = collisions_marker[i] - - if coll - prev_coll > 0: - collision_amount += 1 - - return collisions, collision_amount - - def get_action_type(self, actions): - action_type = [] - - for action in actions: - if sum(action) == 0: - action_type.append(Action.STOP) - elif action[0] == 0 and action[1] == 0: - action_type.append(Action.ROTATE) - else: - action_type.append(Action.MOVE) - - return action_type - - def get_curvature(self, positions): - """ - Calculates the curvature and the normalized curvature - for all positions in the list - - Returns an array of tuples with (curvature, normalized_curvature) - """ - curvature_list = [] - normalized_curvature = [] - - for i, position in enumerate(positions[:-2]): - first = position - second = positions[i + 1] - third = positions[i + 2] - - curvature, normalized = Metrics.calc_curvature(first, second, third) - - curvature_list.append(curvature) - normalized_curvature.append(normalized) - - return curvature_list, normalized_curvature - - def get_roughness(self, positions): - roughness_list = [] - - for i, position in enumerate(positions[:-2]): - first = position - second = positions[i + 1] - third = positions[i + 2] - - roughness_list.append(Metrics.calc_roughness(first, second, third)) - - return roughness_list - - def get_velocity_abs(self, velocities): - return [(i ** 2 + j ** 2) ** 0.5 for i, j, z in velocities] - - def get_acceleration(self, vel_abs): - acc_list = [] - - for i, vel in enumerate(vel_abs[:-1]): - acc_list.append(vel_abs[i + 1] - vel) - - return acc_list - - def get_jerk(self, vel_abs): - """ - jerk is the rate at which an objects acceleration changes with respect to time - """ - jerk_list = [] - - for i, velocity in enumerate(vel_abs[:-2]): - first = velocity - second = vel_abs[i + 1] - third = vel_abs[i + 2] - - jerk = Metrics.calc_jerk(first, second, third) - - jerk_list.append(jerk) - - return jerk_list - - @staticmethod - def calc_curvature(first, second, third): - triangle_area = Metrics.calc_triangle_area(first, second, third) - - divisor = ( - np.abs(np.linalg.norm(first - second)) - * np.abs(np.linalg.norm(second - third)) - * np.abs(np.linalg.norm(third - first)) - ) - - if divisor == 0: - return 0, 0 - - curvature = 4 * triangle_area / divisor - - normalized = ( - curvature * ( - np.abs(np.linalg.norm(first - second)) - + np.abs(np.linalg.norm(second - third)) - ) - ) - - return curvature, normalized - - @staticmethod - def round_values(values, digits=3): - return [round(v, digits) for v in values] - - @staticmethod - def calc_roughness(first, second, third): - triangle_area = Metrics.calc_triangle_area(first, second, third) - - return 2 * triangle_area / np.abs(np.linalg.norm(third - first)) ** 2 - - @staticmethod - def calc_jerk(first, second, third): - a1 = second - first - a2 = third - second - - jerk = np.abs(a2 - a1) - - return jerk - - @staticmethod - def calc_triangle_area(first, second, third): - return ( - 0.5 * np.abs( - first[0] * (second[1] - third[1]) - + second[0] * (third[1] - first[1]) - + third[0] * (first[1] - second[1]) - ) - ) - - @staticmethod - def get_robot_params(dir): - with open(os.path.join(dir, "params.yaml")) as file: - content = yaml.safe_load(file) - - model = content["model"] - - robot_model_params_file = os.path.join( - rospkg.RosPack().get_path("arena-simulation-setup"), - "robot", - model, - "model_params.yaml" - ) - - with open(robot_model_params_file, "r") as file: - return yaml.safe_load(file) - - -if __name__ == "__main__": - arguments = parse_args() - - Metrics(arguments.dir) diff --git a/evaluation/package.xml b/evaluation/package.xml deleted file mode 100644 index 82807e0f..00000000 --- a/evaluation/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - arena-evaluation - 1.0.0 - Part of the arena-benchmark system. - This package is for data recording, evaluation und plotting of performance metrics for arena-rosnav simulations. - Duc Pichel - BSD - catkin - geometry_msgs - nav_msgs - roscpp - rospy - sensor_msgs - std_msgs - geometry_msgs - nav_msgs - roscpp - rospy - sensor_msgs - std_msgs - geometry_msgs - nav_msgs - roscpp - rospy - sensor_msgs - std_msgs - - - \ No newline at end of file diff --git a/evaluation/plot_declarations/sample_schema.md b/evaluation/plot_declarations/sample_schema.md deleted file mode 100644 index 0e00f884..00000000 --- a/evaluation/plot_declarations/sample_schema.md +++ /dev/null @@ -1,127 +0,0 @@ -```yaml - -# Wether you want to show or save the plots -show_plots: boolean -# Name of the directory in ./path -save_location: string - -# List of all datasets that should be compared -# Name of the directory in ./data -datasets: string[] - -# Wether you want to plot the result counts -results: - # Should plot? - plot: boolean - # Title of the plot - title: string - # Name of the file the plot should be saved ot - save_name: string - # Additional Plot arguments - plot_args: {} # Optional - - -# Plot values that are collected in every time step. -# Thus, being arrays for each episode. -# Possible values are: -# - curvature -# - normalized_curvature -# - roughness -# - path_length_values -# - acceleration -# - jerk -# - velocity - -# It is possible to plot -# - A line plot to show the course in a single episode -# You can list multiple value to create multiple plots -single_episode_line: - # Name of the coloumn you want to plot - - data_key: string # Required - # Number of values that should be skipped to reduce datapoints - step_size: int # Optional -> Defaults to 5 - # Coloumn for differentiation - hue: "namespace" # Optional -> Defaults to namespace - # Index of the episode -> If none all episodes are plotted - episode: int # Optional -> Defaults to none - title: string - save_name: string - plot_args: {} # Optional -# - A Distributional plot for a single episode -# You can list multiple value to create multiple plots -single_episode_distribution: - - data_key: string - episode: int - plot_key: "swarm" | "violin" | "box" | "boxen" | "strip" # Optional -> Defaults to "swarm" - title: string - save_name: string - plot_args: {} # Optional -# - A line plot showing aggregated values for all episodes. -# Like a line plot for the max value of each episode -aggregated_distribution: - - data_key: string - # Function that should be used for aggregation. We offer: max, min, mean - aggregate: "max" | "min" | "mean" | "sum" - # Name of the dist plot you want to use. Can be strip, swarm, box, boxen, violin - plot_key: "swarm" | "violin" | "box" | "boxen" | "strip" # Optional -> Defaults to "swarm" - title: string - save_name: string - plot_args: {} # Optional -# - A distributional plot for aggregated values for all episodes. -aggregated_line: - - data_key: string - # Function that should be used for aggregation. We offer: max, min, mean - aggregate: "max" | "min" | "mean" | "sum" - title: string - save_name: string - plot_args: {} # Optional - - -## Plot values that are collected for each episode. -# Single values for each episode -# Possible values are: -# - time_diff -# - angle_over_length -# - path_length - -# It is possible to plot -# - A categorical plot over all episodes to show the values in a line or bar plot -all_episodes_categorical: - - data_key: string - plot_key: "line" | "bar" - title: string - save_name: string - plot_args: {} # Optional -# - Plot a distribution over all episodes -all_episodes_distribution: - - data_key: string - plot_key: "swarm" | "violin" | "box" | "boxen" | "strip" # Optional -> Defaults to "swarm" - title: string - save_name: string - plot_args: {} # Optional - - -## Plot the path the robots took - -# Plot all paths of all episodes for each robot -episode_plots_for_namespaces: - # list of desired results that should be plotted - desired_results: ("TIMEOUT" | "GOAL_REACHED" | "COLLISION")[] - # Wether or not to add the obstacles from the scenario file to the plot - should_add_obstacles: boolean # Optional -> Defaults to False - # Wether or not to mark where collisions happened - should_add_collisions: boolean # Optional -> Defaults to False - title: string - save_name: string - -# Plot the best path of each robot -# Only select the paths that reached the goal and take the path that took the least amount of time -create_best_plots: - # Wether or not to add the obstacles from the scenario file to the plot - should_add_obstacles: boolean # Optional -> Defaults to False - # Wether or not to mark where collisions happened - should_add_collisions: boolean # Optional -> Defaults to False - title: string - save_name: string - -``` diff --git a/evaluation/plots/.gitkeep b/evaluation/plots/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/evaluation/scripts/data_recorder_node.py b/evaluation/scripts/data_recorder_node.py deleted file mode 100755 index 5ba68f9d..00000000 --- a/evaluation/scripts/data_recorder_node.py +++ /dev/null @@ -1,243 +0,0 @@ -#!/usr/bin/env python3 - -# general packages -from math import nan -from threading import current_thread -import time -import numpy as np -import csv -import os -import sys -import math -import re -from rosgraph_msgs.msg import Clock -from rospy.core import traceback -import rostopic -import rospkg -import subprocess -from datetime import datetime -import rosparam -import yaml - -# ros packages -import rospy -from std_msgs.msg import Int16 -from geometry_msgs.msg import Pose2D, Pose, PoseWithCovarianceStamped -from geometry_msgs.msg import Twist -from sensor_msgs.msg import LaserScan -from nav_msgs.msg import Odometry - -# for transformations -from tf.transformations import euler_from_quaternion - - -class DataCollector: - def __init__(self, topic): - topic_callbacks = [ - ("scan", self.laserscan_callback), - ("odom", self.odometry_callback), - ("cmd_vel", self.action_callback), - ] - - try: - callback = lambda msg: [t[1] for t in topic_callbacks if t[0] == topic[1]][0](msg) - except: - traceback.print_exc() - return - - self.full_topic_name = topic[1] - self.data = None - - print(topic[0]) - - self.subscriber = rospy.Subscriber(topic[0], topic[2], callback) - - def episode_callback(self, msg_scenario_reset): - print(msg_scenario_reset) - - self.data = msg_scenario_reset.data - - def laserscan_callback(self, msg_laserscan: LaserScan): - self.data = [msg_laserscan.range_max if math.isnan(val) else round(val, 3) for val in msg_laserscan.ranges] - - def odometry_callback(self, msg_odometry: Odometry): - pose3d = msg_odometry.pose.pose - twist = msg_odometry.twist.twist - - self.data = { - "position": [ - round(val, 3) for val in [ - pose3d.position.x, - pose3d.position.y, - euler_from_quaternion( - [ - pose3d.orientation.x, - pose3d.orientation.y, - pose3d.orientation.z, - pose3d.orientation.w - ] - )[2] - ] - ], - "velocity": [ - round(val, 3) for val in [ - twist.linear.x, - twist.linear.y, - twist.angular.z - ] - ] - } - - def action_callback(self, msg_action: Twist): # variables will be written to csv whenever an action is published - self.data = [ - round(val, 3) for val in [ - msg_action.linear.x, - msg_action.linear.y, - msg_action.angular.z - ] - ] - - def get_data(self): - return ( - self.full_topic_name, - self.data - ) - - -class Recorder: - def __init__(self): - self.model = rospy.get_param(os.path.join(rospy.get_namespace(), "model"), "") - - self.dir = rospkg.RosPack().get_path("evaluation") - self.result_dir = os.path.join(self.dir, "data", datetime.now().strftime("%d-%m-%Y_%H-%M-%S")) + "_" + rospy.get_namespace().replace("/", "") - - try: - os.mkdir(self.result_dir) - except: - pass - - self.write_params() - - topics_to_monitor = self.get_topics_to_monitor() - - topics = rostopic.get_topic_list() - published_topics = topics[0] - - topic_matcher = re.compile(f"{rospy.get_namespace()}({'|'.join([t[0] for t in topics_to_monitor])})$") - - topics_to_sub = [] - - for t in published_topics: - topic_name = t[0] - - match = re.search(topic_matcher, topic_name) - - if not match: - continue - - print(match, t, topic_matcher, match.group()) - - topics_to_sub.append([topic_name, *self.get_class_for_topic_name(topic_name)]) - - # topics_to_sub.append([topic_name, *[t for t in topics_to_monitor if t[0] == match.group()][0]]) - - self.data_collectors = [] - - for topic in topics_to_sub: - self.data_collectors.append(DataCollector(topic)) - self.write_data( - topic[1], [ - "time", "data" - ], - mode="w" - ) - - self.write_data("episode", ["time", "episode"], mode="w") - self.write_data("start_goal", ["episode", "start", "goal"], mode="w") - - self.current_episode = 0 - - self.config = self.read_config() - - self.clock_sub = rospy.Subscriber("/clock", Clock, self.clock_callback) - self.scenario_reset_sub = rospy.Subscriber("/scenario_reset", Int16, self.scenario_reset_callback) - - self.current_time = None - - print(rosparam.print_params("", "/")) - - def scenario_reset_callback(self, data: Int16): - self.current_episode = data.data - - def clock_callback(self, clock: Clock): - current_simulation_action_time = clock.clock.secs * 10e9 + clock.clock.nsecs - - if not self.current_time: - self.current_time = current_simulation_action_time - - time_diff = (current_simulation_action_time - self.current_time) / 1e6 ## in ms - - if time_diff < self.config["record_frequency"]: - return - - self.current_time = current_simulation_action_time - - for collector in self.data_collectors: - topic_name, data = collector.get_data() - - self.write_data(topic_name, [self.current_time, data]) - - self.write_data("episode", [self.current_time, self.current_episode]) - self.write_data("start_goal", [ - self.current_episode, - rospy.get_param(rospy.get_namespace() + "start", [0, 0, 0]), - rospy.get_param(rospy.get_namespace() + "goal", [0, 0, 0]) - ]) - - def read_config(self): - with open(self.dir + "/data_recorder_config.yaml") as file: - return yaml.safe_load(file) - - def get_class_for_topic_name(self, topic_name): - if "/scan" in topic_name: - return ["scan", LaserScan] - if "/odom" in topic_name: - return ["odom", Odometry] - if "/cmd_vel" in topic_name: - return ["cmd_vel", Twist] - - def get_topics_to_monitor(self): - return [ - ("scan", LaserScan), - ("scenario_reset", Int16), - ("odom", Odometry), - ("cmd_vel", Twist) - ] - - def write_data(self, file_name, data, mode="a"): - with open(f"{self.result_dir}/{file_name}.csv", mode, newline = "") as file: - writer = csv.writer(file, delimiter = ',') - writer.writerow(data) - file.close() - - def write_params(self): - with open(self.result_dir + "/params.yaml", "w") as file: - yaml.dump({ - "model": self.model, - "map_file": rospy.get_param("/map_file", ""), - "scenario_file": rospy.get_param("/scenario_file", ""), - "local_planner": rospy.get_param(rospy.get_namespace() + "local_planner"), - "agent_name": rospy.get_param(rospy.get_namespace() + "agent_name", ""), - "namespace": rospy.get_namespace().replace("/", "") - }, file) - - -if __name__=="__main__": - rospy.init_node("data_recorder", anonymous=True) - - time.sleep(5) - - Recorder() - - rospy.spin() - diff --git a/evaluation/utils.py b/evaluation/utils.py deleted file mode 100644 index 563d76d9..00000000 --- a/evaluation/utils.py +++ /dev/null @@ -1,7 +0,0 @@ -import numpy as np - - -class Utils: - @staticmethod - def string_to_float_list(d): - return np.array(d.replace("[", "").replace("]", "").split(r", ")).astype(float)