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/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 6643116f..0ebb5fc3 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -128,8 +128,9 @@ def pf_localization(px, pw, z, u): xEst = px.dot(pw.T) PEst = calc_covariance(xEst, px, pw) - px, pw = re_sampling(px, pw) - + N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number + if N_eff < NTh: + px, pw = re_sampling(px, pw) return xEst, PEst, px, pw @@ -138,21 +139,18 @@ def re_sampling(px, pw): low variance re-sampling """ - N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number - if N_eff < NTh: - w_cum = np.cumsum(pw) - base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP - re_sample_id = base + np.random.rand(base.shape[0]) / NP + w_cum = np.cumsum(pw) + base = np.arange(0.0, 1.0, 1/NP) + re_sample_id = base + np.random.uniform(0, 1/NP) + indexes = [] + ind = 0 + for ip in range(NP): + while re_sample_id[ip] > w_cum[ind]: + ind += 1 + indexes.append(ind) - indexes = [] - ind = 0 - for ip in range(NP): - while re_sample_id[ip] > w_cum[ind]: - ind += 1 - indexes.append(ind) - - px = px[:, indexes] - pw = np.zeros((1, NP)) + 1.0 / NP # init weight + px = px[:, indexes] + pw = np.zeros((1, NP)) + 1.0 / NP # init weight return px, pw 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 6047660f..17574918 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -73,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) @@ -84,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 < cBest: - 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, @@ -109,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')) @@ -193,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) @@ -233,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: diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index 73651d8e..3512ef97 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -1,25 +1,26 @@ """ Iterative Closest Point (ICP) SLAM example -author: Atsushi Sakai (@Atsushi_twi) +author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı """ import math -import numpy as np + import matplotlib.pyplot as plt +import numpy as np # ICP parameters EPS = 0.0001 -MAXITER = 100 +MAX_ITER = 100 show_animation = True -def ICP_matching(ppoints, cpoints): +def icp_matching(previous_points, current_points): """ Iterative Closest Point matching - input - ppoints: 2D points in the previous frame - cpoints: 2D points in the current frame + previous_points: 2D points in the previous frame + current_points: 2D points in the current frame - output R: Rotation matrix T: Translation vector @@ -35,17 +36,17 @@ def ICP_matching(ppoints, cpoints): if show_animation: # pragma: no cover plt.cla() - plt.plot(ppoints[0, :], ppoints[1, :], ".r") - plt.plot(cpoints[0, :], cpoints[1, :], ".b") + plt.plot(previous_points[0, :], previous_points[1, :], ".r") + plt.plot(current_points[0, :], current_points[1, :], ".b") plt.plot(0.0, 0.0, "xr") plt.axis("equal") - plt.pause(1.0) + plt.pause(0.1) - inds, error = nearest_neighbor_assosiation(ppoints, cpoints) - Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) + indexes, error = nearest_neighbor_association(previous_points, current_points) + Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points) # update current points - cpoints = (Rt @ cpoints) + Tt[:, np.newaxis] + current_points = (Rt @ current_points) + Tt[:, np.newaxis] H = update_homogeneous_matrix(H, Rt, Tt) @@ -56,7 +57,7 @@ def ICP_matching(ppoints, cpoints): if dError <= EPS: print("Converge", error, dError, count) break - elif MAXITER <= count: + elif MAX_ITER <= count: print("Not Converge...", error, dError, count) break @@ -85,39 +86,29 @@ def update_homogeneous_matrix(Hin, R, T): return Hin @ H -def nearest_neighbor_assosiation(ppoints, cpoints): +def nearest_neighbor_association(previous_points, current_points): # calc the sum of residual errors - dcpoints = ppoints - cpoints - d = np.linalg.norm(dcpoints, axis=0) + delta_points = previous_points - current_points + d = np.linalg.norm(delta_points, axis=0) error = sum(d) # calc index with nearest neighbor assosiation - inds = [] - for i in range(cpoints.shape[1]): - minid = -1 - mind = float("inf") - for ii in range(ppoints.shape[1]): - d = np.linalg.norm(ppoints[:, ii] - cpoints[:, i]) + d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1) + - np.tile(previous_points, (1, current_points.shape[1])), axis=0) + indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1) - if mind >= d: - mind = d - minid = ii - - inds.append(minid) - - return inds, error + return indexes, error -def SVD_motion_estimation(ppoints, cpoints): +def svd_motion_estimation(previous_points, current_points): + pm = np.mean(previous_points, axis=1) + cm = np.mean(current_points, axis=1) - pm = np.mean(ppoints, axis=1) - cm = np.mean(cpoints, axis=1) + p_shift = previous_points - pm[:, np.newaxis] + c_shift = current_points - cm[:, np.newaxis] - pshift = ppoints - pm[:, np.newaxis] - cshift = cpoints - cm[:, np.newaxis] - - W = cshift @ pshift.T + W = c_shift @ p_shift.T u, s, vh = np.linalg.svd(W) R = (u @ vh).T @@ -130,7 +121,7 @@ def main(): print(__file__ + " start!!") # simulation parameters - nPoint = 10 + nPoint = 1000 fieldLength = 50.0 motion = [0.5, 2.0, np.deg2rad(-10.0)] # movement [x[m],y[m],yaw[deg]] @@ -141,19 +132,19 @@ def main(): # previous points px = (np.random.rand(nPoint) - 0.5) * fieldLength py = (np.random.rand(nPoint) - 0.5) * fieldLength - ppoints = np.vstack((px, py)) + previous_points = np.vstack((px, py)) # current points cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0] for (x, y) in zip(px, py)] cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1] for (x, y) in zip(px, py)] - cpoints = np.vstack((cx, cy)) + current_points = np.vstack((cx, cy)) - R, T = ICP_matching(ppoints, cpoints) + R, T = icp_matching(previous_points, current_points) print("R:", R) print("T:", T) if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py index 5ece27f8..2bcf828c 100644 --- a/tests/test_LQR_planner.py +++ b/tests/test_LQR_planner.py @@ -1,6 +1,6 @@ +import sys from unittest import TestCase -import sys sys.path.append("./PathPlanning/LQRPlanner") from PathPlanning.LQRPlanner import LQRplanner as m @@ -11,5 +11,5 @@ print(__file__) class Test(TestCase): def test1(self): - m.show_animation = False + m.SHOW_ANIMATION = False m.main()