added human roadmap cleaning

SentientPlatypus · Aug 11, 2023 · fd06e81 · fd06e81
1 parent 3b00ccb
commit fd06e81
Show file tree

Hide file tree

Showing 10 changed files with 205 additions and 18 deletions.
diff --git a/README.md b/README.md
diff --git a/__pycache__/roadmap.cpython-39.pyc b/__pycache__/roadmap.cpython-39.pyc
diff --git a/bloated.txt → bloating_walls/bloated.txt b/bloated.txt → bloating_walls/bloated.txt
diff --git a/wallbloat.ipynb → bloating_walls/wallbloat.ipynb b/wallbloat.ipynb → bloating_walls/wallbloat.ipynb
@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 42,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -12,7 +12,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 43,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -55,7 +55,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 44,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [
     {
@@ -92,7 +92,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 45,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [
     {
@@ -126,7 +126,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 46,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [
     {
@@ -158,7 +158,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 47,
+   "execution_count": 6,
    "metadata": {},
    "outputs": [
     {
@@ -208,7 +208,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 48,
+   "execution_count": 7,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -225,7 +225,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 49,
+   "execution_count": 8,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -238,7 +238,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 50,
+   "execution_count": 9,
    "metadata": {},
    "outputs": [
     {
@@ -276,7 +276,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 51,
+   "execution_count": 10,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -285,7 +285,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 52,
+   "execution_count": 11,
    "metadata": {},
    "outputs": [
     {
@@ -332,7 +332,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 53,
+   "execution_count": 12,
    "metadata": {},
    "outputs": [
     {
@@ -354,7 +354,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 54,
+   "execution_count": 13,
    "metadata": {},
    "outputs": [
     {
@@ -376,7 +376,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 55,
+   "execution_count": 14,
    "metadata": {},
    "outputs": [
     {
@@ -398,7 +398,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 56,
+   "execution_count": 15,
    "metadata": {},
    "outputs": [
     {
@@ -427,7 +427,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 57,
+   "execution_count": 16,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -445,7 +445,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 61,
+   "execution_count": 17,
    "metadata": {},
    "outputs": [
     {
@@ -463,7 +463,8 @@
    ],
    "source": [
     "rhodes = walls_from_file(\"walls.txt\")\n",
-    "plot_walls(rhodes, bloat(rhodes, 0.5))\n"
+    "plot_walls(rhodes, bloat(rhodes, 0.5))\n",
+    "walls_to_file(bloat(rhodes, 0.5), \"bloated.txt\")"
    ]
   }
  ],

diff --git a/wallbloat.py → bloating_walls/wallbloat.py b/wallbloat.py → bloating_walls/wallbloat.py
diff --git a/walls.txt → bloating_walls/walls.txt b/walls.txt → bloating_walls/walls.txt
diff --git a/human_roadmap/rhd3_map_3_walls.txt b/human_roadmap/rhd3_map_3_walls.txt
@@ -0,0 +1,40 @@
+20.1609195402299 -16.0648786717752 17.6513043478261 -18.581074168798
+29.6113409961686 -8.55816091954023 28.7400255427842 -7.5527969348659
+28.7400255427842 -7.5527969348659 24.0483269476373 -12.1104469987229
+24.0483269476373 -12.1104469987229 22.4397445721584 -14.4562962962963
+22.4397445721584 -14.4562962962963 20.1609195402299 -16.1319029374202
+33.0510485933504 -13.7491560102302 30.9035294117647 -12.3398465473146
+30.9035294117647 -12.3398465473146 29.7626598465473 -10.326547314578
+29.7626598465473 -10.326547314578 28.5546803069054 -9.31989769820972
+28.5546803069054 -9.31989769820972 29.6284398976982 -8.58168797953964
+33.3865984654732 -14.9571355498721 34.9301278772379 -17.5744245524297
+34.9301278772379 -17.5744245524297 37.5474168797954 -20.1246035805627
+37.5474168797954 -20.1246035805627 38.4198465473146 -19.1850639386189
+38.4198465473146 -19.1850639386189 33.0510485933504 -13.8162659846547
+27.0111508951407 -11.0647570332481 27.9506905370844 -10.1252173913043
+27.9506905370844 -10.1252173913043 28.8902301790281 -11.2660869565217
+28.8902301790281 -11.2660869565217 31.2390792838875 -13.6820460358056
+31.2390792838875 -13.6820460358056 33.3865984654732 -15.0913554987212
+5.13329923273657 -6.43416879795397 17.817084398977 -19.923273657289
+17.817084398977 -19.8561636828645 24.1925319693095 -13.7491560102302
+24.1925319693095 -13.7491560102302 25.2662915601023 -11.9371867007673
+25.2662915601023 -11.9371867007673 27.0782608695652 -11.0647570332481
+7.07948849104859 -0.662710997442453 6.81104859335038 -0.327161125319691
+6.81104859335038 -0.327161125319691 4.99907928388747 -0.528491048593352
+4.99907928388747 -0.528491048593352 -1.10792838874681 -0.528491048593352
+-1.10792838874681 -0.528491048593352 -1.17503836317136 0.343938618925833
+5.06618925831202 -6.50127877237851 5.33462915601023 -2.81023017902813
+5.33462915601023 -2.81023017902813 6.47549872122762 -3.01156010230179
+6.47549872122762 -3.01156010230179 7.68347826086956 -1.73647058823529
+7.68347826086956 -1.73647058823529 7.07948849104859 -0.662710997442453
+6.87815856777494 -6.97104859335039 8.28746803069054 -8.04480818414322
+8.28746803069054 -8.04480818414322 16.6762148337596 -17.1046547314578
+16.6762148337596 -17.1046547314578 17.6513043478261 -18.581074168798
+6.82309067688378 -3.46431673052362 7.29226053639847 -5.07289910600255
+7.29226053639847 -5.07289910600255 6.82309067688378 -7.01660280970626
+6.82309067688378 -7.01660280970626 6.82309067688378 -7.01660280970626
+8.36464878671775 -8.02196679438059 8.36464878671775 -8.02196679438059
+-1.35386973180077 0.557139208173695 12.1850319284802 0.154993614303962
+12.1850319284802 0.154993614303962 12.3190804597701 -1.58763729246488
+12.3190804597701 -1.58763729246488 8.2976245210728 -3.06217113665389
+8.2976245210728 -3.06217113665389 6.82309067688378 -3.46431673052362
diff --git a/human_roadmap/rhd3_map_3_waypoints.txt b/human_roadmap/rhd3_map_3_waypoints.txt
@@ -0,0 +1,25 @@
+6.94526854219949 -0.125831202046037
+8.01902813299233 -8.7830179028133
+11.8442966751918 -12.8096163682864
+15.4682352941176 -16.7019948849105
+19.2935038363171 -17.7086445012788
+31.1048593350384 -13.0109462915601
+32.3799488491049 -13.8162659846547
+33.5879283887468 -14.822915601023
+35.7354475703325 -17.373094629156
+37.61452685422 -19.319283887468
+23.1187723785166 -14.2860358056266
+24.6623017902813 -12.2056265984655
+26.2729411764706 -10.5949872122762
+27.9506905370844 -9.45411764705882
+29.8297698209719 -11.6016368286445
+0.106598465473149 0.0754987212276248
+3.12 -0.0587212276214828
+10.3007672634271 -0.394271099744245
+7.14659846547314 -3.34710997442455
+5.9386189258312 -4.48797953964194
+6.54260869565217 -6.97104859335039
+9.8981074168798 -10.7292071611253
+14.058925831202 -15.1584654731458
+17.6157544757033 -18.7824040920716
+20.7028132992327 -16.5006649616368
diff --git a/human_roadmap/roadmap.py b/human_roadmap/roadmap.py
@@ -0,0 +1,121 @@
+import numpy as np
+import pprint
+from matplotlib import pyplot as plt
+import math
+
+
+def orientation(p, q, r):
+    val = (q[1] - p[1]) * (r[0] - q[0]) - (q[0] - p[0]) * (r[1] - q[1])
+    if val == 0:
+        return 0
+    return 1 if val > 0 else 2
+
+def on_segment(p, q, r):
+    return (q[0] <= max(p[0], r[0]) and q[0] >= min(p[0], r[0]) and
+            q[1] <= max(p[1], r[1]) and q[1] >= min(p[1], r[1]))
+
+def do_intersect(seg1, seg2):
+    p1, q1 = seg1
+    p2, q2 = seg2
+
+    o1 = orientation(p1, q1, p2)
+    o2 = orientation(p1, q1, q2)
+    o3 = orientation(p2, q2, p1)
+    o4 = orientation(p2, q2, q1)
+
+    if (o1 != o2 and o3 != o4) or (o1 == 0 and on_segment(p1, p2, q1)) or (o2 == 0 and on_segment(p1, q2, q1)) or (o3 == 0 and on_segment(p2, p1, q2)) or (o4 == 0 and on_segment(p2, q1, q2)):
+        return True
+
+    return False
+
+
+def distance(p1, p2):
+    return math.sqrt((p2[0] - p1[0])**2 + (p2[1]-p1[1])**2)
+
+def project_point_onto_line_segment(h, p1, p2):
+    v = p2 - p1
+    w = h - p1
+    projection = np.dot(w, v) / np.dot(v, v)
+    proj_h = p1 + np.clip(projection, 0, 1) * v
+    return proj_h
+
+
+class RoadMap():
+    def __init__(self, wallpath:str, waypointpath:str) -> None:
+        walls = np.loadtxt(wallpath)
+        self.walls = walls.reshape(-1, 2, 2)
+
+        waypoints = np.loadtxt(waypointpath)
+        self.waypoints = waypoints
+
+        self.roadmap = []
+        # Go through each waypoint
+        for i in range(len(self.waypoints)):
+            #go through all other waypoints
+            for j in range(len(self.waypoints)):
+                if i== j:
+                    continue
+
+                w = self.waypoints[i]
+                w2 = self.waypoints[j]
+
+                w_w2_segment = np.array([w, w2])
+
+                if not any([do_intersect(wall, w_w2_segment) for wall in self.walls]):
+                    self.roadmap.append(w_w2_segment)
+
+        self.roadmap = np.array(self.roadmap)
+        print("Initialized".center(20, "-"))
+
+    def project(self, h):
+        closest = 0
+        for segments in self.roadmap:
+            try:
+                projected = project_point_onto_line_segment(h, segments[0], segments[1])
+                if distance(h, projected) < distance(h,closest):
+                    closest = projected
+            except:
+                closest = projected
+        return closest
+
+
+
+
+    def plot(self, human = None):
+        for wall in self.walls:
+            x_values = [point[0] for point in wall]
+            y_values = [point[1] for point in wall]
+            plt.plot(x_values, y_values, color="green")
+
+        for line in self.roadmap:
+            x_values = [point[0] for point in line]
+            y_values = [point[1] for point in line]
+            plt.plot(x_values, y_values, color="red", linestyle="dotted")
+
+
+
+        plt.scatter(self.waypoints[:, 0], self.waypoints[:, 1], color="blue", marker="*", label="waypoint")
+
+        if human is not None:
+            plt.scatter([human[0]], [human[1]], color="orange", marker="o", label="human")
+            projected = self.project(h=human)
+            print(projected)
+            plt.scatter([projected[0]], [projected[1]], color="black", marker="D", label="projected")
+        plt.xlabel('X-axis')
+        plt.ylabel('Y-axis')
+        plt.title('Plot of Walls')
+        plt.legend()
+        plt.grid(True)
+        plt.show()
+
+
+
+
+
+if __name__ == "__main__":
+
+    print(do_intersect([[-1, 0], [1, 0]], [[0, -1], [0, 1]]))
+    rm = RoadMap(r"W:\Code\workorinternship\jackal\human_roadmap\rhd3_map_3_walls.txt", r"W:\Code\workorinternship\jackal\human_roadmap\rhd3_map_3_waypoints.txt")
+    human = np.array([10, -9])
+    # rm.project(human)
+    rm.plot(human= human)
diff --git a/human_roadmap/roadmaptesting.ipynb b/human_roadmap/roadmaptesting.ipynb