diff --git a/.github/workflows/greetings.yml b/.github/workflows/greetings.yml new file mode 100644 index 00000000..3da4a694 --- /dev/null +++ b/.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' diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml new file mode 100644 index 00000000..a3203ff1 --- /dev/null +++ b/.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 diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index c2a8674a..c8a44349 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/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 diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 4a32801d..17574918 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/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: