commented out rvcprint import

Author: jhavl

roboticstoolbox/mobile/RRTPlanner.py

@@ -1,28 +1,33 @@
 import math
 from collections import namedtuple
 from roboticstoolbox.mobile.OccGrid import PolygonMap
-import rvcprint
+
+# import rvcprint
 from roboticstoolbox import *
 import numpy as np
 import matplotlib.pyplot as plt
 
 from spatialmath import Polygon2, SE2, base
 from roboticstoolbox.mobile.PlannerBase import PlannerBase
 from roboticstoolbox.mobile.DubinsPlanner import DubinsPlanner
+
 # from roboticstoolbox.mobile.OccupancyGrid import OccupancyGrid
 from pgraph import DGraph
 
-class RRTPlanner(PlannerBase):
 
-    def __init__(self,
-                map=None,
-                vehicle=None,
-                curvature=None,
-                expand_dis=3.0,
-                path_resolution=0.5,
-                stepsize=0.2,
-                showsamples=False,
-                npoints=50, **kwargs):
+class RRTPlanner(PlannerBase):
+    def __init__(
+        self,
+        map=None,
+        vehicle=None,
+        curvature=None,
+        expand_dis=3.0,
+        path_resolution=0.5,
+        stepsize=0.2,
+        showsamples=False,
+        npoints=50,
+        **kwargs
+    ):
 
         super().__init__(ndims=2, **kwargs)
 
@@ -32,7 +37,7 @@ def __init__(self,
         self.map = map
         self.showsamples = showsamples
 
-        self.g = DGraph(metric='SE2')
+        self.g = DGraph(metric="SE2")
 
         self.vehicle = vehicle
         if curvature is None:
@@ -41,7 +46,7 @@ def __init__(self,
             else:
                 curvature = 1
 
-        print('curvature', curvature)
+        print("curvature", curvature)
         self.dubins = DubinsPlanner(curvature=curvature, stepsize=stepsize)
 
         # self.goal_yaw_th = np.deg2rad(1.0)
@@ -68,7 +73,7 @@ def plan(self, goal, animation=True, search_until_npoints=True):
             random_point = self.qrandom_free()
 
             if self.showsamples:
-                plt.plot(random_point[0], random_point[1], 'ok', markersize=2)
+                plt.plot(random_point[0], random_point[1], "ok", markersize=2)
 
             vnearest, d = self.g.closest(random_point)
 
@@ -100,8 +105,7 @@ def plan(self, goal, animation=True, search_until_npoints=True):
             self.g.add_edge(vnew, vnearest, cost=pstatus.length)
             vnew.path = path
 
-
-            self.vehicle.polygon(random_point).plot(color='b', alpha=0.1)
+            self.vehicle.polygon(random_point).plot(color="b", alpha=0.1)
             plt.show()
 
         self.progress_end()
@@ -119,7 +123,9 @@ def query(self, start):
             if vertex.path is not None:
                 path = np.vstack((path, vertex.path))
 
-        status = namedtuple('RRTStatus', ['length', 'initial_d', 'vertices'])(cost, d, vpath)
+        status = namedtuple("RRTStatus", ["length", "initial_d", "vertices"])(
+            cost, d, vpath
+        )
 
         return path, status
 
@@ -142,7 +148,8 @@ def calc_dist_to_goal(self, x, y):
     def qrandom(self):
         return self.random.uniform(
             low=(self.map.workspace[0], self.map.workspace[2], -np.pi),
-            high=(self.map.workspace[1], self.map.workspace[3], np.pi))
+            high=(self.map.workspace[1], self.map.workspace[3], np.pi),
+        )
 
     def qrandom_free(self):
 
@@ -155,23 +162,24 @@ def qrandom_free(self):
     def iscollision(self, q):
         return self.map.iscollision(self.vehicle.polygon(q))
 
+
 if __name__ == "__main__":
 
     from roboticstoolbox.mobile.Vehicle import Bicycle
 
     # start and goal configuration
-    qs = (2, 8, -np.pi/2)
-    qg = (8, 2, -np.pi/2)
+    qs = (2, 8, -np.pi / 2)
+    qg = (8, 2, -np.pi / 2)
 
     # obstacle map
     map = PolygonMap(workspace=[0, 10])
     map.add([(5, 50), (5, 6), (6, 6), (6, 50)])
     # map.add([(5, 0), (6, 0), (6, 4), (5, 4)])
     map.add([(5, 4), (5, -50), (6, -50), (6, 4)])
 
-    l=3
-    w=1.5
-    v0 = Polygon2([(-l/2, w/2), (-l/2, -w/2), (l/2, -w/2), (l/2, w/2)])
+    l = 3
+    w = 1.5
+    v0 = Polygon2([(-l / 2, w / 2), (-l / 2, -w / 2), (l / 2, -w / 2), (l / 2, w / 2)])
 
     vehicle = Bicycle(steer_max=0.4, l=2, polygon=v0)
 
@@ -181,5 +189,4 @@ def iscollision(self, q):
     path, status = rrt.query(start=qs)
     rrt.plot(path)
 
-
-    plt.show(block=True)
+    plt.show(block=True)