Merge branch 'master' of https://github.com/AtsushiSakai/PythonRobotics

.github/workflows/greetings.yml

@@ -0,0 +1,13 @@
+name: Greetings
+
+on: [pull_request, issues]
+
+jobs:
+  greeting:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/first-interaction@v1
+      with:
+        repo-token: ${{ secrets.GITHUB_TOKEN }}
+        issue-message: 'Message that will be displayed on users'' This is first issue for you on this project'
+        pr-message: 'Message that will be displayed on users'' This is first pr for you on this project'

.github/workflows/pythonpackage.yml

@@ -0,0 +1,34 @@
+name: Python package
+
+on: [push]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+    strategy:
+      max-parallel: 4
+      matrix:
+        python-version: [3.6, 3.7]
+
+    steps:
+    - uses: actions/checkout@v1
+    - name: Set up Python ${{ matrix.python-version }}
+      uses: actions/setup-python@v1
+      with:
+        python-version: ${{ matrix.python-version }}
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install -r requirements.txt
+    - name: Lint with flake8
+      run: |
+        pip install flake8
+        # stop the build if there are Python syntax errors or undefined names
+        flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
+        # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide
+        flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
+    - name: install coverage
+      run: pip install coverage   
+    - name: Test
+      run: bash runtests.sh

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -2,7 +2,7 @@
 
 Mobile robot motion planning sample with Dynamic Window Approach
 
-author: Atsushi Sakai (@Atsushi_twi), Goktug Karakasli
+author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
 
 """
 
@@ -172,7 +172,7 @@ def calc_obstacle_cost(trajectory, ob, config):
         rot = np.transpose(rot, [2, 0, 1])
         local_ob = ob[:, None] - trajectory[:, 0:2]
         local_ob = local_ob.reshape(-1, local_ob.shape[-1])
-        local_ob = np.array([local_ob @ -x for x in rot])
+        local_ob = np.array([local_ob @ x for x in rot])
         local_ob = local_ob.reshape(-1, local_ob.shape[-1])
         upper_check = local_ob[:, 0] <= config.robot_length / 2
         right_check = local_ob[:, 1] <= config.robot_width / 2

PathPlanning/InformedRRTStar/informed_rrt_star.py

@@ -40,7 +40,6 @@ class InformedRRTStar:
         self.node_list = [self.start]
         # max length we expect to find in our 'informed' sample space, starts as infinite
         cBest = float('inf')
-        pathLen = float('inf')
         solutionSet = set()
         path = None
 
@@ -74,10 +73,9 @@ class InformedRRTStar:
             newNode = self.get_new_node(theta, nind, nearestNode)
             d = self.line_cost(nearestNode, newNode)
 
-            isCollision = self.collision_check(newNode, self.obstacle_list)
-            isCollisionEx = self.check_collision_extend(nearestNode, theta, d)
+            noCollision = self.check_collision(nearestNode, theta, d)
 
-            if isCollision and isCollisionEx:
+            if noCollision:
                 nearInds = self.find_near_nodes(newNode)
                 newNode = self.choose_parent(newNode, nearInds)
 
@@ -85,14 +83,14 @@ class InformedRRTStar:
                 self.rewire(newNode, nearInds)
 
                 if self.is_near_goal(newNode):
-                    solutionSet.add(newNode)
-                    lastIndex = len(self.node_list) - 1
-                    tempPath = self.get_final_course(lastIndex)
-                    tempPathLen = self.get_path_len(tempPath)
-                    if tempPathLen < pathLen:
-                        path = tempPath
-                        cBest = tempPathLen
-
+                    if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y):
+                        solutionSet.add(newNode)
+                        lastIndex = len(self.node_list) - 1
+                        tempPath = self.get_final_course(lastIndex)
+                        tempPathLen = self.get_path_len(tempPath)
+                        if tempPathLen < cBest:
+                            path = tempPath
+                            cBest = tempPathLen
             if animation:
                 self.draw_graph(xCenter=xCenter,
                                 cBest=cBest, cMin=cMin,
@@ -110,7 +108,7 @@ class InformedRRTStar:
             dy = newNode.y - self.node_list[i].y
             d = math.sqrt(dx ** 2 + dy ** 2)
             theta = math.atan2(dy, dx)
-            if self.check_collision_extend(self.node_list[i], theta, d):
+            if self.check_collision(self.node_list[i], theta, d):
                 dList.append(self.node_list[i].cost + d)
             else:
                 dList.append(float('inf'))
@@ -194,17 +192,6 @@ class InformedRRTStar:
         minIndex = dList.index(min(dList))
         return minIndex
 
-    @staticmethod
-    def collision_check(newNode, obstacleList):
-        for (ox, oy, size) in obstacleList:
-            dx = ox - newNode.x
-            dy = oy - newNode.y
-            d = dx * dx + dy * dy
-            if d <= 1.1 * size ** 2:
-                return False  # collision
-
-        return True  # safe
-
     def get_new_node(self, theta, nind, nearestNode):
         newNode = copy.deepcopy(nearestNode)
 
@@ -234,21 +221,41 @@ class InformedRRTStar:
             if nearNode.cost > scost:
                 theta = math.atan2(newNode.y - nearNode.y,
                                    newNode.x - nearNode.x)
-                if self.check_collision_extend(nearNode, theta, d):
+                if self.check_collision(nearNode, theta, d):
                     nearNode.parent = n_node - 1
                     nearNode.cost = scost
+                    
+    @staticmethod
+    def distance_squared_point_to_segment(v, w, p):
+        # Return minimum distance between line segment vw and point p
+        if (np.array_equal(v, w)):
+            return (p-v).dot(p-v) # v == w case
+        l2 = (w-v).dot(w-v) # i.e. |w-v|^2 -  avoid a sqrt
+        # Consider the line extending the segment, parameterized as v + t (w - v).
+        # We find projection of point p onto the line. 
+        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
+        # We clamp t from [0,1] to handle points outside the segment vw.
+        t = max(0, min(1, (p - v).dot(w - v) / l2))
+        projection = v + t * (w - v) # Projection falls on the segment
+        return (p-projection).dot(p-projection)
 
-    def check_collision_extend(self, nearNode, theta, d):
-        tmpNode = copy.deepcopy(nearNode)
-
-        for i in range(int(d / self.expand_dis)):
-            tmpNode.x += self.expand_dis * math.cos(theta)
-            tmpNode.y += self.expand_dis * math.sin(theta)
-            if not self.collision_check(tmpNode, self.obstacle_list):
-                return False
-
+    def check_segment_collision(self, x1, y1, x2, y2):
+        for (ox, oy, size) in self.obstacle_list:
+            dd = self.distance_squared_point_to_segment(
+                np.array([x1, y1]),
+                np.array([x2, y2]),
+                np.array([ox, oy]))
+            if dd <= size**2:
+                return False  # collision
         return True
 
+
+    def check_collision(self, nearNode, theta, d):
+        tmpNode = copy.deepcopy(nearNode)
+        endx = tmpNode.x + math.cos(theta)*d
+        endy = tmpNode.y + math.sin(theta)*d
+        return self.check_segment_collision(tmpNode.x, tmpNode.y, endx, endy)
+
     def get_final_course(self, lastIndex):
         path = [[self.goal.x, self.goal.y]]
         while self.node_list[lastIndex].parent is not None: