smoothing experiments added

python/Metrics.ipynb

@@ -214,7 +214,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Python 3",
+   "display_name": "Python 3 (ipykernel)",
    "language": "python",
    "name": "python3"
   },
@@ -228,7 +228,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.5"
+   "version": "3.9.10"
   }
  },
  "nbformat": 4,

python/SBPL Debugging.ipynb

@@ -1004,7 +1004,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Python 3",
+   "display_name": "Python 3 (ipykernel)",
    "language": "python",
    "name": "python3"
   },
@@ -1018,7 +1018,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.5"
+   "version": "3.9.10"
   }
  },
  "nbformat": 4,

python/definitions.py

@@ -90,7 +90,9 @@
     'grips': 'GRIPS',
     'ompl_bspline': 'B-Spline',
     'ompl_shortcut': 'Shortcut',
-    'ompl_simplify_max': 'SimplifyMax'
+    'ompl_simplify_max': 'SimplifyMax',
+    'constrained_onf_smoother': 'ONF',
+    'chomp': 'chomp'
 }
 
 smoothers = list(smoother_names.values())

python/mpb.py

@@ -80,7 +80,7 @@ def _update_pss(self):
         self._planners = [planner for planner, used in self["benchmark.planning"].items(
         ) if used]  # type: [str]
         self._smoothers = [smoother for smoother,
-                           used in self["benchmark.smoothing"].items() if used]  # type: [str]
+                                        used in self["benchmark.smoothing"].items() if used]  # type: [str]
         self._steer_functions = [steer_functions[index]
                                  for index in self["benchmark.steer_functions"]]  # type: [str]
         self._robot_models = [robot_models[index]
@@ -91,8 +91,8 @@ def get_config(config_file: str = os.path.join(MPB_BINARY_DIR, 'benchmark_templa
         if not os.path.exists(config_file):
             raise Exception('Could not find configuration template file at %s. ' % os.path.abspath(
                 os.path.join(MPB_BINARY_DIR, 'benchmark_template.json')) +
-                'Make sure you run the benchmark binary without CLI arguments to generate ' +
-                'benchmark_template.json.')
+                            'Make sure you run the benchmark binary without CLI arguments to generate ' +
+                            'benchmark_template.json.')
         with open(config_file, 'r') as f:
             config = json.load(f)["settings"]  # type: dict
         return config
@@ -344,7 +344,7 @@ def kill_process():
             if code is not None and code != 0:
                 print("Error (%i) occurred for MPB with ID %s using planner %s." % (
                     code, self.id, convert_planner_name(planner)),
-                    file=sys.stderr)
+                      file=sys.stderr)
                 success = False
                 continue
             if ip > 0:
@@ -367,6 +367,62 @@ def kill_process():
                       results_filename, file=sys.stderr)
         return code
 
+    def run_custom_smoother(self, kill_after_timeout=True):
+        run = 0
+        log_filename = os.path.join('.', self.id + ".log")
+        logfile = open(log_filename, 'w')
+        success = True
+        # self["benchmark.log_file"] = os.path.abspath(self.results_filename)
+        binary = MPB_PYTHON_BINARY
+        tsk = subprocess.Popen([binary, os.path.abspath(self.results_filename)],
+                               stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
+                               cwd=os.path.abspath(MPB_BINARY_DIR), env=self._env)
+        proc = psutil.Process(tsk.pid)
+        create_time = time.time()
+        kill_timer = None
+        if kill_after_timeout:
+            # kill process after 2 * max planning time
+            def kill_process():
+                nonlocal success, code
+                try:
+                    proc.kill()
+                    print("Killed %s with planner %s after %.2fs exceeded timeout."
+                          % (self.id, planner, time.time() - create_time))
+                    success = False
+                    code = -9
+                except psutil.NoSuchProcess:
+                    pass
+                except:
+                    print('Error occurred while trying to kill %s with planner %s.'
+                          % (self.id, planner), file=sys.stderr)
+                    success = False
+                    code = -9
+
+            kill_timer = Timer(
+                self["max_planning_time"] * self["benchmark.runs"] * 2, kill_process)
+            kill_timer.start()
+        while True:
+            line = tsk.stdout.readline()
+            if line is None:
+                break
+            line = line.decode('UTF-8')
+            if line == '':
+                break
+            if '<stats>' in line:
+                run += 1
+            logfile.write(line)
+        code = tsk.poll()
+        if code is None:
+            code = 0
+        if code is not None and code != 0:
+            print("Error (%i) occurred for MPB with ID %s using planner" % (
+                code, self.id),
+                  file=sys.stderr)
+        if kill_timer is not None:
+            kill_timer.cancel()
+        logfile.close()
+
+
     def print_info(self):
         if not os.path.exists(self.results_filename):
             print("No results file exists for MPB %s." % self.id)
@@ -391,7 +447,7 @@ def visualize_trajectory_grid(self, set_suptitle=True, **kwargs):
         from trajectory import visualize_grid
         if set_suptitle:
             kwargs["suptitle"] = self.id
-        visualize_grid(self.results_filename,  **kwargs)
+        visualize_grid(self.results_filename, **kwargs)
 
     def plot_planner_stats(self, **kwargs):
         if not os.path.exists(self.results_filename):

python/plot_trajectory.py

@@ -47,6 +47,7 @@ def plot_trajectory(traj, planner: str, settings, color, add_label=True, alpha:
                     i % plot_every_nth_polygon != 0 and not (plot_last_polygon and i == traj.shape[0] - 1)):
                 continue
             state = traj[i, :]
+            state = np.append(state, 0)
             c, s = np.cos(-state[2]), np.sin(-state[2])
             rotation = np.array([[c, -s], [s, c]])
             poly = Polygon(state[:2] + np.matmul(points, rotation), True, fill=False, linestyle='--', edgecolor=color,