moving code

Author: fangedward

.gitignore

@@ -0,0 +1,4 @@
+build/
+cmake-build-debug/
+venv/
+bin/

CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(RRTSim)
+
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
+set(CMAKE_CXX_FLAGS "-march=native -O3 -Wall")
+
+find_package(Qt5Widgets REQUIRED)
+find_package(Qt5Core REQUIRED)
+find_package(Qt5Gui REQUIRED)
+find_package(Eigen3)
+
+if(Eigen3_FOUND)
+  set(CUSTOM_EIGEN ${EIGEN3_INCLUDE_DIR})
+else()
+  find_package(Eigen)
+  set(CUSTOM_EIGEN ${EIGEN_INCLUDE_DIR})
+endif()
+
+include_directories(
+  ${CUSTOM_EIGEN}
+)
+add_library(RRTStar SHARED src/RRTStar/obstacles.cpp src/RRTStar/rrt.cpp
+        src/RRTStar/RRTStarWrapper.cpp)
+add_executable(RRTStarTest src/main.cpp src/UI/mainwindow.cpp
+        src/RRTStar/obstacles.cpp src/UI/renderarea.cpp src/RRTStar/rrt.cpp)
+target_link_libraries(RRTStar Qt5::Widgets Qt5::Gui)
+target_link_libraries(RRTStarTest Qt5::Widgets Qt5::Gui)

LICENSE

@@ -1,201 +1,22 @@
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
+The MIT License (MIT)
 
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+Copyright (c) 2015 Sourish Ghosh
 
-   1. Definitions.
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
 
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
 
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
 
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.

README.md

@@ -0,0 +1 @@
+# RRT*

RRTStar/__init__.py

@@ -0,0 +1,87 @@
+import rrtstar_wrapper
+import time
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patch
+
+
+def main():
+    """A debug script for the rrt star planner.
+
+    This script will solve the rrt star problem in a
+    standalone simulation and visualize the results or raise an error if a
+    path is not found.
+    """
+    print(__file__ + " start!!")
+    sim_loop = 50
+    area = 20.0  # animation area length [m]
+    show_animation = True
+
+    conds = {'wx': [150.0],
+             'wy': [195.0],
+             'obstacle_list': [[134., 194., 136., 196.]],
+             'x': 120.0,
+             'y': 195.0,
+             }  # paste output from debug log
+
+    # way points
+    wx = np.array(conds['wx'])
+    wy = np.array(conds['wy'])
+    wp = np.array([wx, wy]).T
+
+    # initial conditions
+    x = conds['x']
+    y = conds['y']
+
+    # obstacle lists
+    obs = np.array(conds['obstacle_list'])
+
+    for i in range(sim_loop):
+        print("Iteration: {}".format(i))
+        start_time = time.time()
+        result_x, result_y, success = \
+            rrtstar_wrapper.apply_rrt_star([x, y], wp[-1], 1,
+                                           2000, obs)
+        if success == 1:
+            x = result_x[1]
+            y = result_y[1]
+        else:
+            print("Failed")
+        end_time = time.time() - start_time
+        print("Time taken: {}".format(end_time))
+
+        if np.hypot(result_x[1] - wx[-1], result_y[1] - wy[-1]) <= 2.0:
+            print("Goal")
+            break
+
+        if show_animation:  # pragma: no cover
+            plt.cla()
+            # for stopping simulation with the esc key.
+            plt.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None]
+            )
+            ax = plt.gca()
+            rect = patch.Rectangle((obs[0, 0], obs[0, 1]),
+                                   obs[0, 2] - obs[0, 0], obs[0, 3] - obs[0, 1])
+            ax.add_patch(rect)
+            plt.plot(wp[:, 0], wp[:, 1])
+            plt.plot(result_x[1:], result_y[1:], ".r")
+            plt.plot(result_x[1], result_y[1], "vc")
+            plt.plot(wp[-1, 0], wp[-1, 1], "og")
+            plt.xlim(result_x[1] - area, result_x[1] + area)
+            plt.ylim(result_y[1] - area, result_y[1] + area)
+            plt.xlabel("X axis")
+            plt.ylabel("Y axis")
+            plt.grid(True)
+            plt.pause(0.1)
+
+    print("Finish")
+    if show_animation:  # pragma: no cover
+        plt.grid(True)
+        plt.pause(1)
+        plt.show()
+
+
+if __name__ == '__main__':
+    main()

RRTStar/__pycache__/rrtstar_wrapper.cpython-36.pyc

@@ -0,0 +1,48 @@
+import ctypes
+import numpy as np
+from ctypes import c_double, c_int
+
+cdll = ctypes.CDLL("build/libRRTStar.so")
+_c_double_p = ctypes.POINTER(c_double)
+_apply_rrt_star = cdll.ApplyRRTStar
+_apply_rrt_star.argtypes = (
+    c_double, c_double, c_double, c_double, c_int, c_int, _c_double_p,
+    _c_double_p, _c_double_p, _c_double_p, c_int, _c_double_p, _c_double_p
+)
+_apply_rrt_star.restype = c_int
+
+
+def apply_rrt_star(start, end, step_size, max_iterations, obs):
+    """ Run RRTStar given a start, end and list of obstacles.
+
+    Args:
+        start (np.ndarray): [x, y]
+        end (np.ndarray): [x, y]
+        step_size (float): distance to sample new points
+        max_iterations (int): maximum number of iterations to run
+        obs (np.ndarray): [llx, lly, urx, ury]
+
+    Returns:
+        result_x (list(float)): x positions of rrt* path, if it exists
+        result_y (list(float)): y positions of rrt* path, if it exists
+        success (bool): whether rrt* found a path or not
+    """
+    result_x = np.zeros(100)
+    result_y = np.zeros(100)
+    success = _apply_rrt_star(
+        c_double(start[0]), c_double(start[1]), c_double(end[0]),
+        c_double(end[1]), c_int(step_size), c_int(max_iterations),
+        obs[:, 0].ctypes.data_as(_c_double_p),
+        obs[:, 1].ctypes.data_as(_c_double_p),
+        obs[:, 2].ctypes.data_as(_c_double_p),
+        obs[:, 3].ctypes.data_as(_c_double_p),
+        c_int(obs.shape[0]),
+        result_x.ctypes.data_as(_c_double_p),
+        result_y.ctypes.data_as(_c_double_p),
+    )
+
+    ind = -1
+    if success and np.any(np.isnan(result_x)):
+        ind = np.where(np.isnan(result_x))[0][0]
+
+    return result_x[:ind], result_y[:ind], success