working?

zhanwenchen · zhanwenchen · commit 2f51017530c5 · 2018-05-16T22:11:49.000-04:00
diff --git a/README.md b/README.md
@@ -16,4 +16,5 @@ consideration is that Phil sucks at C++ but no one sucks at Python.
 
 # TODOs
 
-- [] BUG: Histogram grid active region is upside down. Potential cause: have been using \[x\]\[y\] but should have been \[y\]\[x\]
+- [x] (Fixed) BUG: Histogram grid active region is upside down. Potential cause: have been using \[x\]\[y\] but should have been \[y\]\[x\] (it was the plotting x.append(y))
+- [] FIXME: Polar Histogram pie chart switch angle and maybe counterclockwise.
diff --git a/lib/histogram_grid.py b/lib/histogram_grid.py
@@ -1,3 +1,4 @@
+import numpy as np
 import csv
 from operator import sub # for get_distance_between_discrete_points
 import math
@@ -29,8 +30,8 @@ def from_map(cls, map_fname, active_region_dimension, resolution, robot_location
 
         # Convert string "1"s and "0"s to integer 1s and 0s
         lines = list(map(lambda l: list(map(int, l)), lines))
-        print("histogram_grid: histogram =")
-        print(*lines, sep="\n")
+        # print("histogram_grid: histogram =")
+        # print(*lines, sep="\n")
         dimension = (len(lines[0]), len(lines))
         hg = cls(dimension, resolution, robot_location, active_region_dimension)
         hg.histogram_grid = lines
@@ -91,14 +92,17 @@ def get_angle_between_discrete_points(cls, discrete_start, discrete_end):
         """
         Returns the angle between the line between pos2 and posRef and the horizontal along positive i direction.
         """
-        discrete_displacement = get_discrete_displacement(discrete_start, discrete_end)
-
-        delta_x, delta_y = discrete_displacement
-        # print("histogram_grid: (delta_x, delta_y) =", discrete_displacement)
-
-        angle_radian = math.atan2(delta_y, delta_x)
-        angle_degrees = math.degrees(angle_radian)
-        return angle_degrees
+        # discrete_displacement = get_discrete_displacement(discrete_start, discrete_end)
+
+        ang1 = np.arctan2(*discrete_start[::-1])
+        ang2 = np.arctan2(*discrete_end[::-1])
+        return np.rad2deg((ang1 - ang2) % (2 * np.pi))
+        # delta_x, delta_y = discrete_displacement
+        # # print("histogram_grid: (delta_x, delta_y) =", discrete_displacement)
+        #
+        # angle_radian = math.atan2(delta_y, delta_x)
+        # angle_degrees = math.degrees(angle_radian)
+        # return angle_degrees
 
     def get_active_region(self, robot_location):
         # REVIEW: the four coordinates are discrete?
diff --git a/lib/path_planner.py b/lib/path_planner.py
@@ -67,8 +67,10 @@ def generate_histogram(self, robot_location):
                 distance = histogram_grid.get_continuous_distance_between_discrete_points(node_considered, robot_location)
                 delta_certainty = (certainty ** 2) * (self.a - self.b * distance)
                 robot_to_node_angle = histogram_grid.get_angle_between_discrete_points(robot_location, node_considered)
-                polar_histogram.add_certainty_to_bin_at_angle(robot_to_node_angle, delta_certainty)
-                histogram_grid.get_certainty_at_discrete_point(node_considered)
+                print("path_planner: robot_to_node_angle between robot_location", robot_location, "and node_considered", node_considered, "is", robot_to_node_angle)
+                if delta_certainty != 0:
+                    print("path_planner: adding certainty %.1f to angle %s or node" % (delta_certainty, robot_to_node_angle), node_considered)
+                    polar_histogram.add_certainty_to_bin_at_angle(robot_to_node_angle, delta_certainty)
 
 
         polar_histogram.smooth_histogram(self.l)
diff --git a/lib/polar_histogram.py b/lib/polar_histogram.py
@@ -78,7 +78,9 @@ def get_certainty_from_angle(self, angle):
 
     def add_certainty_to_bin_at_angle(self, angle, delta_certainty):
         """Adds the passed value to the current value of the histogr1am grid."""
-        self._polar_histogram[self.get_bin_index_from_angle(angle)] += delta_certainty
+        bin_index = self.get_bin_index_from_angle(angle)
+        # print("polar_histogram: adding certainty %s to bin #%s = %s" % (delta_certainty, bin_index, angle))
+        self._polar_histogram[bin_index] += delta_certainty
 
 
     def smooth_histogram(self, l):
diff --git a/lib/robot.py b/lib/robot.py
@@ -23,28 +23,26 @@ class Robot:
     def __init__(self, histogram_grid, polar_histogram, init_location, target_location, init_speed):
         # CHANGED: we shouldn't need polar_histogram, only histogram_grid
         self.path_planner = PathPlanner(histogram_grid, polar_histogram, init_location, target_location)
-#         // discretePoint location; //Stores the location of the robot
-#         contPoint location; //Stores the location of the robot
         self.target_location = target_location
         self.location = init_location
         self.speed = init_speed
         self.update_angle()
 
+
     @classmethod
     def from_map(cls, map_fname, init_location, target_location, init_speed, active_region_dimension, resolution, num_bins):
         histogram_grid = HistogramGrid.from_map(map_fname, active_region_dimension, resolution, init_location)
         polar_histogram = PolarHistogram(num_bins)
         return cls(histogram_grid, polar_histogram, init_location, target_location, init_speed)
 
 
-
-
     def update_angle(self):
         continuous_displacement = (self.target_location[0] - self.location[0], self.target_location[1] - self.location[1])
         continuous_robot_to_target_angle = math.atan2(continuous_displacement[1], continuous_displacement[0])
         self.angle = self.path_planner.get_best_angle(continuous_robot_to_target_angle)
         self.continuous_robot_to_target_angle = continuous_robot_to_target_angle
 
+
     def set_speed(self, speed):
         self.speed = speed
 
@@ -59,9 +57,6 @@ def update_location(self):
         angle_radian = self.angle * math.pi/180
         velocity_x, velocity_y = self.velocity
 
-#         delta_x = self.speed * math.cos(angle_radian)
-#         delta_y = self.speed * math.sin(angle_radian)
-
         old_x, old_y = self.location
         self.location = (old_x + velocity_x, old_y + velocity_y)
 
@@ -71,42 +66,18 @@ def update_location(self):
         self.path_planner.set_robot_location(discrete_location)
 
 
-#     def talk(self):
-#         discrete_target_location = self.histogram_grid.get_discrete_target_location();
-#         std::cout << "location is (" << location.x << ", "
-#                   << location.y << "); Desired Position is ("
-#                   << targetLoc.x << ", " << targetLoc.y << ")\n";
-#     //Constructor for the RobotTest class
-#     //CHANGED: no need for initAngle or initSpeed: robot should figure out.
-#     // RobotTest(discretePoint initPos, double initAngle, double initSpeed):
-#     RobotTest(discretePoint initPos):
-#         grid("../map.txt", initPos, ACTIVE_REGION_SIZE_I, ACTIVE_REGION_SIZE_J, HIST_WIDTH, HIST_LENGTH, NODE_SIZE_IN),
-#         hist(NUM_BINS),
-#         pather(hist, &grid, A_IN, B_IN, L_IN, MAX_NUM_NODES_FOR_VALLEY_IN, VALLEY_THRESHOLD_IN)
-#     {
-#         location.x = initPos.x;
-#         location.y = initPos.y;
-
-#         pather.updateRobotPosition(discreteLocation);
-#         grid.setTargetLoc({TARGET_X, TARGET_Y});
-#     }
-
-
-    #//main function per timestep
-    #// 1. Get angle from nothing at t=0, then
-    #// 2. get speed from nothing at t=0.
-    #// 3. Given position at 0, draw simulation at t=0,
-    #// 4. Now move from t=0 to t=1 by only updating the robot's position.
+    # Main function per timestep
+    # 1. Get angle from nothing at t=0, then
+    # 2. get speed from nothing at t=0.
+    # 3. Given position at 0, draw simulation at t=0,
+    # 4. Now move from t=0 to t=1 by only updating the robot's position.
     def step(self, draw=True):
         self.print_histogram()
-        # print("robot: angle =", self.angle)
-        self.update_angle() # // angle: Null (or optionally, t-1) => t
-        # self.set_speed() # // speed: Null (or optionally, t-1) => t
+        self.update_angle() # angle: Null (or optionally, t-1) => t
+        # self.set_speed() # speed: Null (or optionally, t-1) => t
         print("robot: step: best angle =", self.angle )
         self.update_velocity()
-         # // at t=0, angle: t=0, speed, position: t
-
-        self.update_location() #// position: t => t+1
+        self.update_location() # position: t => t+1
 
     def loop(self, num_steps, draw=True):
         plt.ion() # enable interactive plotting mode
@@ -139,39 +110,24 @@ def loop(self, num_steps, draw=True):
             bin_certainties = [certainty for angle, certainty in polar_histogram_by_angle]
             colors = ['blue' if certainty < valley_threshold else 'red' for angle, certainty in polar_histogram_by_angle]
             labels = [angle for angle, certainty in polar_histogram_by_angle]
-            # index = 0
             generator = enumerate(polar_histogram_by_angle)
             def make_autopct(bin_percentages):
                 def my_autopct(pct):
-                    # index = 0
-                    # total = sum(values)
-                    # val = int(round(pct*total/100.0))
                     index, (angle, certainty) = next(generator)
-                    # index += 1
                     return '{angle:.0f}:  {certainty:.1f}'.format(angle=angle, certainty=certainty)
                 return my_autopct
 
-            (pie_patches, pie_texts, pie_autotexts) = polar_plot.pie(bin_percentages, colors=colors, labels=labels, startangle=0, counterclock=True, autopct=make_autopct(bin_percentages))
+            pie_patches, pie_texts, pie_autotexts = polar_plot.pie(bin_percentages, colors=colors, labels=labels, startangle=0, counterclock=True, autopct=make_autopct(bin_percentages))
 
 
             # 3. Plot the valley
             histogram_grid_active_region = self.path_planner.histogram_grid.get_histogram_grid_active_region(active_region_min_x, active_region_min_y, active_region_max_x, active_region_max_y)
-            print('active region histogram =')
-            print(*histogram_grid_active_region, sep='\n')
+            # print('active region histogram =')
+            # print(*histogram_grid_active_region, sep='\n')
             histogram_grid_plot.clear()
             histogram_grid_plot.matshow(histogram_grid_active_region, origin="upper")
 
 
-            # self.draw(ax)
-            # display.clear_output(wait=True)
-            # display.display(plt.gcf())
-            # plt.draw()
-            # bnext = Button(ax, 'Next')
-            # bnext.on_clicked(self.step_callback)
-            # bprev = Button(ax, 'Previous')
-            # bprev.on_clicked(callback.prev)
-
-
         for i in range(num_steps):
 
             self.step()
@@ -186,7 +142,6 @@ def my_autopct(pct):
                 active_region_min_x, active_region_min_y, active_region_max_x, active_region_max_y = self.path_planner.histogram_grid.get_active_region(self.location)
                 rectangle.set_bounds(active_region_min_x, active_region_min_y, active_region_max_x - active_region_min_x, active_region_max_y - active_region_min_y)
 
-                # simulation_plot.invert_yaxis()
 
                 # 2. Replot the polar histogram
                 # sectors = self.path_planner.get_sectors() # NOTE: sectors are only valid
@@ -198,40 +153,29 @@ def my_autopct(pct):
                 bin_certainties = [certainty for angle, certainty in polar_histogram_by_angle]
                 colors = ['blue' if certainty < valley_threshold else 'red' for angle, certainty in polar_histogram_by_angle]
                 labels = [angle for angle, certainty in polar_histogram_by_angle]
-                # index = 0
                 generator = enumerate(polar_histogram_by_angle)
                 def make_autopct(bin_percentages):
                     def my_autopct(pct):
-                        # index = 0
-                        # total = sum(values)
-                        # val = int(round(pct*total/100.0))
                         index, (angle, certainty) = next(generator)
-                        # index += 1
                         return '{certainty:.1f}'.format(certainty=certainty)
                     return my_autopct
 
                 polar_plot.clear()
-                polar_plot.pie(bin_percentages, colors=colors, labels=labels, startangle=0, counterclock=True, autopct=make_autopct(bin_percentages))
+                # polar_plot.pie(bin_percentages, colors=colors, labels=labels, startangle=0, counterclock=True, autopct=make_autopct(bin_percentages))
+                polar_plot.pie(bin_percentages, colors=colors, labels=labels, startangle=0, autopct=make_autopct(bin_percentages))
 
 
                 # 3. Replot the histogram_grid
                 histogram_grid_active_region = self.path_planner.histogram_grid.get_histogram_grid_active_region(active_region_min_x, active_region_min_y, active_region_max_x, active_region_max_y)
-                print('active region histogram =')
-                print(*histogram_grid_active_region, sep='\n')
+                # print('active region histogram =')
+                # print(*histogram_grid_active_region, sep='\n')
                 histogram_grid_plot.clear()
                 histogram_grid_plot.matshow(histogram_grid_active_region, origin="upper")
 
 
                 # 4. Actually display the plots
                 # display.clear_output(wait=True) # NOTE: Uncomment this for animation. Comment this out if you want to see all steps.
                 display.display(plt.gcf())
-                # self.print_histogram()
-                # figure.canvas.draw_idle()
-                # plt.pause(0.1)
-
-    def step_callback(self, event):
-        self.step()
-        self.print_histogram()
 
 
     def print_histogram(self):
diff --git a/playground.ipynb b/playground.ipynb
