Add shifting start and end positions in random forest dataset

.gitignore

@@ -58,4 +58,6 @@ mlruns
 *.pth
 *.ckpt
 
-*egg*
+*egg*
+
+bencmark/thirdparty

neural_field_optimal_planner/utils/shift_start_position.py

@@ -0,0 +1,42 @@
+import json
+import os
+from bitarray import bitarray
+import numpy as np
+import math as math
+from matplotlib.patches import Polygon, Circle
+
+def checker(map_cell, centr, max_dist, x1, y1):
+    valid = True
+    points = []
+    for i in range(map_cell.shape[1]):
+        for j in range(map_cell.shape[0]):
+            if (i-centr[1])**2 + (j-centr[0])**2 <= (max_dist)**2 and map_cell[i,j] == 0:
+                valid = False
+                points.append([i,j])
+    return valid, points
+
+def max_dist(poly, start_point):
+    coord = poly.get_xy()
+    distance = []
+    for i in coord: distance.append(math.dist(i, start_point))    
+    return np.max(distance)
+
+def nearest_point(suit_points, centr):
+    distance = []
+    for i in suit_points: distance.append(math.dist(i, centr))
+    return suit_points[np.argmin(distance)]
+
+def read_data(filename):
+    file = open(filename, "r")
+    data = json.load(file)
+    file.close()
+    shape = data['settings']['env']['collision']['robot_shape']
+    run = data['runs'][0]
+    points = np.array(shape)
+    env = run['environment']
+    w = env["width"]
+    h = env["height"]
+    map_data = np.array(list(bitarray(env["map"]))).reshape((h, w))
+    map_data = 1. - map_data
+    start_point = data['runs'][0]['environment']['start'][:2]
+    return map_data, points, start_point

notebooks/benchmark/__init__.py

@@ -0,0 +1 @@
+from .benchmark_adapter import BenchmarkAdapter

notebooks/benchmark/definitions.py

@@ -0,0 +1,173 @@
+stat_names = {
+    'max_curvature': 'Maximum Curvature',
+    'normalized_curvature': 'Normalized Curvature',
+    'aol': 'AOL',
+    'max_clearing_distance': 'Maximum Clearing',
+    'mean_clearing_distance': 'Mean Clearing',
+    'median_clearing_distance': 'Median Clearing',
+    'min_clearing_distance': 'Minimum Clearing',
+    'path_length': 'Path Length',
+    'smoothness': 'Smoothness',
+    'planning_time': 'Computation Time',
+    'cusps': 'Cusps',
+    'aggregate': 'Aggregate',
+}
+
+metric_properties = {
+    'path_found': {
+        'sum': True
+    },
+    'planning_time': {
+        'show_std': True,
+        'highlight_optimum': True
+    },
+    'path_length': {
+        'show_std': True
+    },
+    'mean_clearing_distance': {
+        'show_std': True
+    },
+    'min_clearing_distance': {
+        'show_std': True
+    },
+    'max_clearing_distance': {
+        'show_std': True
+    },
+    'median_clearing_distance': {
+        'show_std': True
+    },
+    'max_curvature': {
+        'show_std': True,
+        'minimize': True
+    },
+    'normalized_curvature': {
+        'show_std': True,
+        'minimize': True
+    },
+    'aol': {
+        'show_std': True,
+        'minimize': True
+    },
+    'cusps': {
+        'minimize': True,
+        'sum': True
+    }
+}
+
+steer_function_names = {
+    'reeds_shepp': 'Reeds-Shepp',
+    'dubins': 'Dubins',
+    'posq': 'POSQ',
+    'clothoid': 'G1 Clothoid',
+    'linear': 'Linear',
+    'cc_dubins': 'CC Dubins',
+    'hc_reeds_shepp': 'HC Reeds-Shepp',
+    'cc_reeds_shepp': 'CC Reeds-Shepp'
+}
+
+steer_functions = [
+    'reeds_shepp',
+    'dubins',
+    'posq',
+    'clothoid',
+    'linear',
+    'cc_dubins',
+    'hc_reeds_shepp',
+    'cc_reeds_shepp'
+]
+
+robot_models = [
+    'kinematic_car',
+    'kinematic_single_track'
+]
+
+robot_models_names = {
+    'kinematic_car': 'Kinematic Car',
+    'kinematic_single_track': 'Kinematic Single Track'
+}
+
+smoother_names = {
+    'grips': 'GRIPS',
+    'ompl_bspline': 'B-Spline',
+    'ompl_shortcut': 'Shortcut',
+    'ompl_simplify_max': 'SimplifyMax'
+}
+
+smoothers = list(smoother_names.values())
+
+sampling_planners = ['rrt', 'est', 'sbl', 'prm',
+                     'theta_star', 'sst', 'kpiece', 'pdst', 'stride']
+anytime_planners = ['rrt_star', 'rrt_sharp', 'informed_rrt_star', 'sorrt_star', 'prm_star', 'fmt', 'bfmt', 'cforest',
+                    'bit_star', 'spars', 'spars2']
+controlbased_planners = ['fpkpiece', 'fpest', 'fpsst', 'fprrt', 'fppdst']
+sbpl_planners = ['sbpl_adstar', 'sbpl_anastar',
+                 'sbpl_arastar', 'sbpl_lazy_ara', 'sbpl_mha']
+all_planners = sampling_planners + anytime_planners + sbpl_planners
+
+# Mapping internal planner names to their printable counterparts
+planner_names = {
+    'rrt': 'RRT',
+    'est': 'EST',
+    'sbl': 'SBL',
+    'prm': 'PRM',
+    'theta_star': 'Theta*',
+    'sst': 'SST',
+    'fmt': 'FMT',
+    'kpiece': 'KPIECE',
+    'pdst': 'PDST',
+    'stride': 'STRIDE',
+    'rrt_star': 'RRT*',
+    'rrt_sharp': 'RRT#',
+    'informed_rrt_star': 'Informed RRT*',
+    'sorrt_star': 'SORRT*',
+    'prm_star': 'PRM*',
+    'bfmt': 'BFMT',
+    'cforest': 'CForest',
+    'bit_star': 'BIT*',
+    'spars': 'SPARS',
+    'spars2': 'SPARS2',
+    'sbpl_adstar': 'SBPL AD*',
+    'sbpl_anastar': 'SBPL ANA*',
+    'sbpl_arastar': 'SBPL ARA*',
+    'sbpl_lazy_ara': 'SBPL Lazy ARA*',
+    'sbpl_mha': 'SBPL MHA*',
+    'fpkpiece': 'FP KPIECE',
+    'fpest': 'FP EST',
+    'fpsst': 'FP SST',
+    'fprrt': 'FP RRT',
+    'fppdst': 'FP PDST'
+}
+
+# Names internally used by MPB/OMPL to appear in the "plans" dictionary of the results files
+planner_internal_names = {
+    'rrt': 'RRT',
+    'est': 'EST',
+    'sbl': 'SBL',
+    'prm': 'PRM',
+    'theta_star': 'Theta*',
+    'sst': 'SST',
+    'fmt': 'FMT',
+    'kpiece': 'KPIECE1',
+    'pdst': 'PDST',
+    'stride': 'STRIDE',
+    'rrt_star': 'RRTstar',
+    'rrt_sharp': 'RRT#',
+    'informed_rrt_star': 'InformedRRTstar',
+    'sorrt_star': 'SORRTstar',
+    'prm_star': 'PRMstar',
+    'bfmt': 'BFMT',
+    'cforest': 'CForest',
+    'bit_star': 'kBITstar',
+    'spars': 'SPARS',
+    'spars2': 'SPARStwo',
+    'sbpl_adstar': 'SBPL_ADstar',
+    'sbpl_anastar': 'SBPL_ANAstar',
+    'sbpl_arastar': 'SBPL_ARAstar',
+    'sbpl_lazy_ara': 'SBPL_Lazy_ARA',
+    'sbpl_mha': 'SBPL_MHA',
+    'fpkpiece': 'FP KPIECE',
+    'fpest': 'FP EST',
+    'fpsst': 'FP SST',
+    'fprrt': 'FP RRT',
+    'fppdst': 'FP PDST'
+}

scripts/pytorch_motion_planner_node.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 import rospy
 
 from neural_field_optimal_planner.ros.goal_planner_adapter_factory import GoalPlannerAdapterFactory

scripts/run_bench_mr.py

@@ -1,17 +1,23 @@
-#! /usr/bin/python3.6
+#! /usr/bin/python3
+
 import numpy as np
 import torch
 from matplotlib import pyplot as plt
 import argparse
-
+import os
 from pytorch_lightning.utilities import AttributeDict
+import json
 
 from neural_field_optimal_planner.planner_factory import PlannerFactory
 from neural_field_optimal_planner.plotting_utils import prepare_figure, plot_planner_data, plot_nerf_opt_planner, \
     plot_collision_positions
 from neural_field_optimal_planner.benchmark_adapter import BenchmarkAdapter
 from neural_field_optimal_planner.benchmark_adapter.benchmark_collision_checker import BenchmarkCollisionChecker
 
+start_point = np.array(list(map(int, os.environ.get('START').split())))
+goal_point = np.array(list(map(int, os.environ.get('END').split())))
+start_point = np.append(start_point, 0)
+goal_point = np.append(goal_point, 0)
 torch.random.manual_seed(100)
 np.random.seed(400)
 
@@ -38,7 +44,7 @@
     planner=AttributeDict(
         name="ConstrainedNERFOptPlanner",
         trajectory_random_offset=0.02,
-        collision_weight=1,
+        collision_weight=20,
         velocity_hessian_weight=0.5,
         random_field_points=10,
         init_collision_iteration=0,
@@ -65,19 +71,24 @@
 collision_checker = BenchmarkCollisionChecker(benchmark, benchmark.bounds())
 print("Collision checker initialized")
 planner = PlannerFactory.make_constrained_onf_planner(collision_checker, planner_parameters)
-goal_point = benchmark.goal().as_vec()
-start_point = benchmark.start().as_vec()
+# goal_point = benchmark.goal().as_vec()
+# goal_point = np.array([95,92,0])
+# start_point = benchmark.start().as_vec()
+# start_point = np.array([9,7,0])
 trajectory_boundaries = benchmark.bounds()
+#test_arr = [start_point, goal_point]
+#benchmark.evaluate_and_save_results([start_point, goal_point], "constrained_onf_planner")
 
 planner.init(start_point, goal_point, trajectory_boundaries)
 device = planner._device
 collision_model = planner._collision_model
 is_show = args.show
 fig = None
+
 if is_show:
     fig = plt.figure(dpi=200)
 
-for i in range(100):
+for i in range(1000):
     planner.step()
     if is_show:
         trajectory = planner.get_path()
@@ -87,5 +98,15 @@
         # plot_nerf_opt_planner(planner)
         # plot_collision_positions(planner.checked_positions, planner.truth_collision)
         plt.pause(0.01)
+
 
 benchmark.evaluate_and_save_results(planner.get_path(), "constrained_onf_planner")
+# with open("/home/evgeny/pytorch-motion-planner/notebooks/benchmark/corridor_results.json") as data_file:
+#     data = json.load(data_file)
+# #    for el in data['runs']:
+# del data['runs'][0]
+# os.remove("/home/evgeny/pytorch-motion-planner/notebooks/benchmark/corridor_results.json") 
+# jsonString = json.dumps(data)
+# jsonFile = open("/home/evgeny/pytorch-motion-planner/notebooks/benchmark/corridor_results.json", "w")
+# jsonFile.write(jsonString)
+# jsonFile.close()