diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 5e8632d..ff64677 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -44,6 +44,7 @@ "/home/dev-user/workspace/src/components/control/rear_wheel_feedback", "/home/dev-user/workspace/src/components/control/speed_profile", "/home/dev-user/workspace/src/components/control/stanley", + "/home/dev-user/workspace/src/components/control/vfh", "/home/dev-user/workspace/src/components/course/cubic_spline_course", "/home/dev-user/workspace/src/components/detection/l_shape_fitting", "/home/dev-user/workspace/src/components/localization/kalman_filter", @@ -87,7 +88,13 @@ "/home/dev-user/workspace/src/simulations/mapping/cost_grid_map_construction", "/home/dev-user/workspace/src/simulations/mapping/ndt_map_construction", "/home/dev-user/workspace/src/simulations/mapping/potential_field_map_construction", + "/home/dev-user/workspace/src/simulations/mapping/vfh_candidate_valley_detection", + "/home/dev-user/workspace/src/simulations/mapping/vfh_direction_selection", + "/home/dev-user/workspace/src/simulations/mapping/vfh_dynamic_speed_control", + "/home/dev-user/workspace/src/simulations/mapping/vfh_performance_benchmarking", "/home/dev-user/workspace/src/simulations/mapping/vfh_polar_histogram_construction", + "/home/dev-user/workspace/src/simulations/mapping/vfh_trajectory_verification", + "/home/dev-user/workspace/src/simulations/mapping/vfh_vehicle_motion_integration", "/home/dev-user/workspace/src/simulations/path_planning/aco_path_planning", "/home/dev-user/workspace/src/simulations/path_planning/astar_bidirectional_path_planning", "/home/dev-user/workspace/src/simulations/path_planning/astar_hybrid_path_planning", diff --git a/pyrightconfig.json b/pyrightconfig.json index d2df6d3..7c40c4b 100644 --- a/pyrightconfig.json +++ b/pyrightconfig.json @@ -9,6 +9,7 @@ "src/components/control/rear_wheel_feedback", "src/components/control/speed_profile", "src/components/control/stanley", + "src/components/control/vfh", "src/components/course/cubic_spline_course", "src/components/detection/l_shape_fitting", "src/components/localization/kalman_filter", @@ -52,7 +53,13 @@ "src/simulations/mapping/cost_grid_map_construction", "src/simulations/mapping/ndt_map_construction", "src/simulations/mapping/potential_field_map_construction", + "src/simulations/mapping/vfh_candidate_valley_detection", + "src/simulations/mapping/vfh_direction_selection", + "src/simulations/mapping/vfh_dynamic_speed_control", + "src/simulations/mapping/vfh_performance_benchmarking", "src/simulations/mapping/vfh_polar_histogram_construction", + "src/simulations/mapping/vfh_trajectory_verification", + "src/simulations/mapping/vfh_vehicle_motion_integration", "src/simulations/path_planning/aco_path_planning", "src/simulations/path_planning/astar_bidirectional_path_planning", "src/simulations/path_planning/astar_hybrid_path_planning", diff --git a/src/components/control/vfh/performance_benchmark.py b/src/components/control/vfh/performance_benchmark.py new file mode 100644 index 0000000..7a316b6 --- /dev/null +++ b/src/components/control/vfh/performance_benchmark.py @@ -0,0 +1,96 @@ +""" +performance_benchmark.py + +Author: Khushi +""" + + +class PerformanceBenchmark: + """ + Live performance benchmarking class for the VFH+ pipeline(Step 7 of + the VFH roadmap). Each cycle, it reads how long the mapper(histogram + construction, candidate valley detection and direction selection) and + the controller(target direction/speed, acceleration and yaw rate) + each took on their own last update call, and keeps a running mean and + worst case so the displayed number settles down instead of jittering + frame to frame. + """ + + def __init__(self, mapper, controller): + """ + Constructor + mapper: PolarHistogramMapper instance, read for its own last + update duration via get_last_update_duration_s() + controller: VfhController instance, read for its own last update + duration via get_last_update_duration_s() + """ + + self.mapper = mapper + self.controller = controller + + self.frame_count = 0 + self.total_duration_s = 0.0 + self.max_duration_s = 0.0 + self.last_total_duration_s = 0.0 + + def update(self, time_s): + """ + Function to update benchmark statistics from the mapper's and + controller's own last recorded update durations + time_s: Simulation interval time[sec] + """ + + mapper_duration_s = self.mapper.get_last_update_duration_s() + controller_duration_s = self.controller.get_last_update_duration_s() + self.last_total_duration_s = mapper_duration_s + controller_duration_s + + self.frame_count += 1 + self.total_duration_s += self.last_total_duration_s + if self.last_total_duration_s > self.max_duration_s: + self.max_duration_s = self.last_total_duration_s + + def get_last_duration_s(self): + """ + Function to get the last cycle's combined mapper+controller + update duration[sec] + """ + + return self.last_total_duration_s + + def get_mean_duration_s(self): + """ + Function to get the mean per-cycle combined mapper+controller + update duration[sec] across the whole run so far + """ + + if self.frame_count == 0: + return 0.0 + return self.total_duration_s / self.frame_count + + def get_max_duration_s(self): + """ + Function to get the worst-case per-cycle combined mapper+controller + update duration[sec] seen so far + """ + + return self.max_duration_s + + def draw(self, axes, elems): + """ + Function to draw a small live readout of the last cycle's update + time plus the running mean and worst case, in milliseconds + axes: Axes object of figure + elems: List of plot objects + """ + + text = ("Step 7 benchmark\n" + "last: {0:.2f}[ms]\n" + "mean: {1:.2f}[ms]\n" + "max: {2:.2f}[ms]").format(self.last_total_duration_s * 1000.0, + self.get_mean_duration_s() * 1000.0, + self.get_max_duration_s() * 1000.0) + + readout = axes.text(0.98, 0.98, text, transform=axes.transAxes, + fontsize=9, va="top", ha="right", color="black", + bbox=dict(boxstyle="round", facecolor="white", alpha=0.8)) + elems.append(readout) diff --git a/src/components/control/vfh/trajectory_verifier.py b/src/components/control/vfh/trajectory_verifier.py new file mode 100644 index 0000000..7227fde --- /dev/null +++ b/src/components/control/vfh/trajectory_verifier.py @@ -0,0 +1,122 @@ +""" +trajectory_verifier.py + +Author: Khushi +""" + +from math import hypot + + +class TrajectoryVerifier: + """ + Verification class for a VFH+ driven trajectory(Step 6 of the VFH + roadmap). Each cycle, it reads the same polar histogram a VfhController + steers by(see PolarHistogramMapper, get_histogram()) and the vehicle's + current position, to confirm - using the VFH+ pipeline's own sensed + data, rather than re-deriving obstacle geometry independently - that + the driven trajectory stays clear of obstacles and reaches its goal. + + A sector's smoothed density rises toward a_gain(default 1.0, see + PolarHistogram) as a sensed obstacle gets closer, and can exceed it + when multiple points land in one sector. danger_density is the + density, anywhere around the vehicle, at/above which this trajectory + is flagged as a near-collision: too close for comfort even when the + controller still avoided an actual geometric overlap. + """ + + def __init__(self, mapper, state, target_x_m, target_y_m, + danger_density=1.0, goal_tolerance_m=2.0): + """ + Constructor + mapper: PolarHistogramMapper instance already driving this vehicle, + used to read the smoothed density around it each cycle + state: Vehicle's state object, read each cycle for its position + target_x_m, target_y_m: Goal point[m] the trajectory should reach + danger_density: Smoothed density at/above which, anywhere around + the vehicle, this cycle counts as a near-collision + goal_tolerance_m: Distance[m] from the goal counted as reaching it + """ + + if danger_density <= 0.0: + raise ValueError("danger density must be greater than 0") + if goal_tolerance_m < 0.0: + raise ValueError("goal tolerance must be 0 or greater") + + self.mapper = mapper + self.state = state + self.target_x_m = target_x_m + self.target_y_m = target_y_m + self.danger_density = float(danger_density) + self.goal_tolerance_m = float(goal_tolerance_m) + + self.max_density_seen = 0.0 + self.near_collision_count = 0 + + def update(self, time_s): + """ + Function to update verification data from the mapper's latest + histogram + time_s: Simulation interval time[sec] + """ + + frame_max_density = float(self.mapper.get_histogram().get_smoothed_density().max()) + + if frame_max_density > self.max_density_seen: + self.max_density_seen = frame_max_density + + if frame_max_density >= self.danger_density: + self.near_collision_count += 1 + + def distance_to_goal_m(self): + """ + Function to get the vehicle's current distance to the goal[m] + """ + + return hypot(self.target_x_m - self.state.get_x_m(), + self.target_y_m - self.state.get_y_m()) + + def reached_goal(self): + """ + Function to get whether the vehicle is currently within + goal_tolerance_m of the goal + """ + + return self.distance_to_goal_m() <= self.goal_tolerance_m + + def get_max_density_seen(self): + """ + Function to get the highest smoothed density sensed anywhere + around the vehicle across the whole trajectory so far + """ + + return self.max_density_seen + + def get_near_collision_count(self): + """ + Function to get the number of simulation cycles where the sensed + density anywhere around the vehicle reached danger_density + """ + + return self.near_collision_count + + def draw(self, axes, elems): + """ + Function to draw a small status readout: current distance to + goal, closest density sensed so far, and a warning once any + cycle has been a near-collision + axes: Axes object of figure + elems: List of plot objects + """ + + near_collision = self.max_density_seen >= self.danger_density + status = "COLLISION RISK" if near_collision else "clear" + color = "red" if near_collision else "green" + text = ("Step 6 verification\n" + "distance to goal: {0:.1f}[m]\n" + "closest density seen: {1:.2f}\n" + "status: {2}").format(self.distance_to_goal_m(), self.max_density_seen, status) + + readout = axes.text(0.02, 0.98, text, transform=axes.transAxes, + fontsize=9, va="top", ha="left", color=color, + bbox=dict(boxstyle="round", facecolor="white", alpha=0.8)) + elems.append(readout) diff --git a/src/components/control/vfh/vfh_controller.py b/src/components/control/vfh/vfh_controller.py new file mode 100644 index 0000000..05f4bfa --- /dev/null +++ b/src/components/control/vfh/vfh_controller.py @@ -0,0 +1,247 @@ +""" +vfh_controller.py + +Author: Khushi +""" + +import sys +from pathlib import Path +from math import atan2, pi +from time import perf_counter + +sys.path.append(str(Path(__file__).absolute().parent) + "/../../common") +from angle_lib import pi_to_pi + + +class VfhController: + """ + Controller class to drive a vehicle by Vector Field Histogram(VFH) + based reactive obstacle avoidance. Each cycle, it reads the global + frame direction currently selected by a mapper's direction selector + (see DirectionSelector, Step 3 of the VFH roadmap) and converts it + into acceleration / yaw rate inputs through simple proportional + control. Target speed is the configured cruise speed, scaled down + toward min_speed_mps as the smoothed obstacle density ahead - read + from the mapper's polar histogram, in the current target direction - + rises toward danger_density(Step 5 of the VFH roadmap: Dynamic Speed + Control) + """ + + def __init__(self, spec, mapper, cruise_speed_mps=3.0, + speed_gain=1.0, yaw_rate_gain=1.5, + max_yaw_rate_rps=1.0, min_speed_mps=0.0, + danger_density=1.0, caution_half_angle_rad=pi / 18, + color='b'): + """ + Constructor + spec: Vehicle specification object + mapper: Mapper object exposing get_direction_selector(), whose + DirectionSelector exposes get_selected_angle_rad(), and + get_histogram(), whose PolarHistogram exposes + max_density_in_angle_range() + cruise_speed_mps: Target speed[m/s] with no obstacle ahead + speed_gain: Proportional gain from speed error to acceleration + yaw_rate_gain: Proportional gain from heading error to yaw rate + max_yaw_rate_rps: Saturation limit of commanded yaw rate[rad/s] + min_speed_mps: Target speed[m/s] once density ahead reaches + danger_density or higher(Step 5: Dynamic Speed Control) + danger_density: Smoothed density ahead at/above which target speed + is already down to min_speed_mps. Below it, target + speed scales linearly between cruise_speed_mps + (density 0) and min_speed_mps(density danger_density) + caution_half_angle_rad: Half width[rad], to each side of the target + direction, of the angular range checked for + obstacle density. Looking slightly wider + than a single sector avoids being fooled by + noise right at a sector boundary + color: Reserved for drawing, kept for interface parity with + other controllers + """ + + if cruise_speed_mps < 0.0: + raise ValueError("cruise speed must be 0 or greater") + if speed_gain < 0.0 or yaw_rate_gain < 0.0: + raise ValueError("gains must be 0 or greater") + if max_yaw_rate_rps <= 0.0: + raise ValueError("max yaw rate must be greater than 0") + if min_speed_mps < 0.0 or min_speed_mps > cruise_speed_mps: + raise ValueError("min speed must be between 0 and cruise speed") + if danger_density <= 0.0: + raise ValueError("danger density must be greater than 0") + if caution_half_angle_rad < 0.0: + raise ValueError("caution half angle must be 0 or greater") + + self.WHEEL_BASE_M = spec.wheel_base_m + self.DRAW_COLOR = color + + self.mapper = mapper + self.cruise_speed_mps = float(cruise_speed_mps) + self.speed_gain = float(speed_gain) + self.yaw_rate_gain = float(yaw_rate_gain) + self.max_yaw_rate_rps = float(max_yaw_rate_rps) + self.min_speed_mps = float(min_speed_mps) + self.danger_density = float(danger_density) + self.caution_half_angle_rad = float(caution_half_angle_rad) + + self.target_angle_rad = None + self.target_speed_mps = self.cruise_speed_mps + self.target_accel_mps2 = 0.0 + self.target_yaw_rate_rps = 0.0 + self.target_steer_rad = 0.0 + self.last_update_duration_s = 0.0 + + def _decide_target_direction_rad(self, state): + """ + Private function to decide the global frame angle to steer + toward this cycle. Falls back to the vehicle's current heading + when the mapper has no direction selected yet, for example + when every sector is blocked and no valley exists + state: Vehicle's state object + """ + + selected_angle_rad = self.mapper.get_direction_selector().get_selected_angle_rad() + if selected_angle_rad is None: + self.target_angle_rad = state.get_yaw_rad() + else: + self.target_angle_rad = selected_angle_rad + + def _decide_target_speed_mps(self, state): + """ + Private function to decide the target speed this cycle: the + configured cruise speed, scaled down toward min_speed_mps as the + obstacle density ahead - in the current target direction, read + from the mapper's polar histogram - rises toward danger_density + (Step 5 of the VFH roadmap: Dynamic Speed Control) + state: Vehicle's state object + """ + + heading_relative_angle_rad = pi_to_pi(self.target_angle_rad - state.get_yaw_rad()) + density_ahead = self.mapper.get_histogram().max_density_in_angle_range( + heading_relative_angle_rad, self.caution_half_angle_rad) + + speed_scale = 1.0 - density_ahead / self.danger_density + if speed_scale < 0.0: + speed_scale = 0.0 + elif speed_scale > 1.0: + speed_scale = 1.0 + + self.target_speed_mps = self.min_speed_mps + speed_scale * (self.cruise_speed_mps - self.min_speed_mps) + + def _calculate_target_acceleration_mps2(self, state): + """ + Private function to calculate acceleration input by simple + proportional control toward this cycle's target speed(see + _decide_target_speed_mps) + state: Vehicle's state object + """ + + diff_speed_mps = self.target_speed_mps - state.get_speed_mps() + self.target_accel_mps2 = self.speed_gain * diff_speed_mps + + def _calculate_target_yaw_rate_rps(self, state): + """ + Private function to calculate yaw rate input by simple + proportional control toward the target direction, saturated to + the configured maximum magnitude + state: Vehicle's state object + """ + + diff_angle_rad = pi_to_pi(self.target_angle_rad - state.get_yaw_rad()) + yaw_rate_rps = self.yaw_rate_gain * diff_angle_rad + + if yaw_rate_rps > self.max_yaw_rate_rps: + yaw_rate_rps = self.max_yaw_rate_rps + elif yaw_rate_rps < -self.max_yaw_rate_rps: + yaw_rate_rps = -self.max_yaw_rate_rps + + self.target_yaw_rate_rps = yaw_rate_rps + + def _calculate_target_steer_rad(self, state): + """ + Private function to back out a front tire steering angle from + the commanded yaw rate, for visualization only, using the + bicycle model relation yaw_rate = speed * tan(steer) / wheel_base + state: Vehicle's state object + """ + + speed_mps = state.get_speed_mps() + if abs(speed_mps) < 1e-3: + self.target_steer_rad = 0.0 + else: + self.target_steer_rad = atan2(self.WHEEL_BASE_M * self.target_yaw_rate_rps, speed_mps) + + def update(self, state, time_s): + """ + Function to update data for VFH based obstacle avoidance driving + state: Vehicle's state object + time_s: Simulation interval time[sec] + """ + + start_s = perf_counter() + + self._decide_target_direction_rad(state) + + self._decide_target_speed_mps(state) + + self._calculate_target_acceleration_mps2(state) + + self._calculate_target_yaw_rate_rps(state) + + self._calculate_target_steer_rad(state) + + self.last_update_duration_s = perf_counter() - start_s + + def get_target_accel_mps2(self): + """ + Function to get acceleration input[m/s2] + """ + + return self.target_accel_mps2 + + def get_target_yaw_rate_rps(self): + """ + Function to get yaw rate input[rad/s] + """ + + return self.target_yaw_rate_rps + + def get_target_steer_rad(self): + """ + Function to get steering angle input[rad], for visualization only + """ + + return self.target_steer_rad + + def get_target_angle_rad(self): + """ + Function to get the global frame angle currently targeted + """ + + return self.target_angle_rad + + def get_target_speed_mps(self): + """ + Function to get this cycle's target speed[m/s]: the cruise speed + scaled down by nearby obstacle density(Step 5: Dynamic Speed Control) + """ + + return self.target_speed_mps + + def get_last_update_duration_s(self): + """ + Function to get how long(sec) the last update() call took(Step 7: + Performance Benchmarking) + """ + + return self.last_update_duration_s + + def draw(self, axes, elems): + """ + Function to draw controller data. The targeted direction is + already drawn by the mapper's own selection arrow(Step 3), so + this intentionally adds nothing to avoid a duplicate overlay + axes: Axes object of figure + elems: List of plot object + """ + + pass diff --git a/src/components/mapping/polar_histogram/candidate_valley_detector.py b/src/components/mapping/polar_histogram/candidate_valley_detector.py new file mode 100644 index 0000000..273dced --- /dev/null +++ b/src/components/mapping/polar_histogram/candidate_valley_detector.py @@ -0,0 +1,166 @@ +""" +candidate_valley_detector.py + +Author: Khushi +""" + +import numpy as np + + +class Valley: + """ + Represents one contiguous run of low-density polar histogram sectors, + i.e. a candidate direction range the vehicle could steer into. + + Sector indices are circular(sector num_sectors-1 is adjacent to sector 0), + so a valley may wrap around the 0[deg] boundary. + """ + + def __init__(self, start_index, end_index, width_sectors, center_angle_rad): + """ + Constructor + start_index: Sector index where the valley begins + end_index: Sector index where the valley ends(inclusive) + width_sectors: Number of sectors spanned by the valley + center_angle_rad: Valley's center angle, normalized between -pi and pi[rad] + """ + + self.start_index = start_index + self.end_index = end_index + self.width_sectors = width_sectors + self.center_angle_rad = center_angle_rad + + def get_start_index(self): + """ + Function to get the sector index where the valley begins + """ + + return self.start_index + + def get_end_index(self): + """ + Function to get the sector index where the valley ends(inclusive) + """ + + return self.end_index + + def get_width_sectors(self): + """ + Function to get the number of sectors spanned by the valley + """ + + return self.width_sectors + + def get_center_angle_rad(self): + """ + Function to get the valley's center angle[rad], normalized between -pi and pi + """ + + return self.center_angle_rad + + +class CandidateValleyDetector: + """ + Candidate valley detection class + + This is the second step of the Vector Field Histogram(VFH) algorithm. + Sectors whose smoothed obstacle density falls at or below a configurable + threshold are considered navigable(free enough to steer into). Runs of + consecutive navigable sectors around the circle are grouped into + "valleys", each representing one candidate direction range for the next + step(direction selection). + """ + + def __init__(self, density_threshold=0.2): + """ + Constructor + density_threshold: Sectors at or below this smoothed density value + are considered navigable. Must be 0 or greater + """ + + if density_threshold < 0.0: + raise ValueError("density_threshold must be 0 or greater") + + self.density_threshold = float(density_threshold) + self.valleys = [] + + def detect(self, polar_histogram): + """ + Function to detect candidate valleys from a PolarHistogram instance + polar_histogram: PolarHistogram instance already updated this frame + """ + + density = polar_histogram.get_smoothed_density() + num_sectors = polar_histogram.get_num_sectors() + navigable = density <= self.density_threshold + + self.valleys = self._find_circular_valleys(navigable, num_sectors, polar_histogram) + return self.valleys + + def _find_circular_valleys(self, navigable, num_sectors, polar_histogram): + """ + Private function to group a circular boolean array of navigable + sectors into contiguous Valley instances, handling wrap-around + across the 0[deg] boundary + navigable: Boolean array, True where a sector is navigable + num_sectors: Number of sectors(length of navigable) + polar_histogram: Used to look up each valley's center angle + """ + + if not np.any(navigable): + return [] + + if np.all(navigable): + return [self._make_valley(0, num_sectors - 1, num_sectors, polar_histogram)] + + # rotate the array so it always starts just after a blocked sector. + # At least one blocked sector exists here, since navigable isn't + # all True, so every run in the rotated array is guaranteed to end + # before the array wraps - no run can straddle its boundary + blocked_indices = np.where(~navigable)[0] + anchor = int(blocked_indices[0]) + rotated = np.roll(navigable, -(anchor + 1)) + + valleys = [] + start = None + for i in range(num_sectors): + if rotated[i]: + if start is None: + start = i + elif start is not None: + orig_start = (start + anchor + 1) % num_sectors + orig_end = (i - 1 + anchor + 1) % num_sectors + valleys.append(self._make_valley(orig_start, orig_end, num_sectors, polar_histogram)) + start = None + + return valleys + + def _make_valley(self, start_index, end_index, num_sectors, polar_histogram): + """ + Private function to build a Valley instance from a start/end sector + index pair, computing its width and center angle + start_index: Sector index where the valley begins + end_index: Sector index where the valley ends(inclusive) + num_sectors: Number of sectors, used to compute circular width + polar_histogram: Used to look up the center angle + """ + + width_sectors = (end_index - start_index) % num_sectors + 1 + mid_index = start_index + (width_sectors - 1) / 2.0 + center_angle_rad = polar_histogram.sector_center_angle_rad(mid_index) + + return Valley(start_index, end_index, width_sectors, center_angle_rad) + + def get_density_threshold(self): + """ + Function to get the configured density threshold + """ + + return self.density_threshold + + def get_valleys(self): + """ + Function to get the most recently detected list of Valley instances + """ + + return self.valleys diff --git a/src/components/mapping/polar_histogram/direction_selector.py b/src/components/mapping/polar_histogram/direction_selector.py new file mode 100644 index 0000000..fa33503 --- /dev/null +++ b/src/components/mapping/polar_histogram/direction_selector.py @@ -0,0 +1,103 @@ +""" +direction_selector.py + +Author: Khushi +""" + +import sys +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../../common") +from angle_lib import pi_to_pi + + +class DirectionSelector: + """ + Direction selection class + + This is the third step of the Vector Field Histogram(VFH) algorithm: + choose one steering direction from the candidate valleys detected in + Step 2(CandidateValleyDetector), by minimizing a weighted cost function + over three terms, following Borenstein and Koren(1991): + - distance from the target(goal) direction + - distance from the vehicle's current heading(turn effort) + - distance from the previously selected direction(to avoid + oscillating back and forth between two similarly-scored valleys) + All angles handled here are in the global frame. A valley's center + angle from CandidateValleyDetector is relative to the vehicle's + heading, so it is converted to a global angle before scoring. + """ + + def __init__(self, target_weight=1.0, heading_weight=1.0, previous_weight=1.0): + """ + Constructor + target_weight: Cost weight for distance from the target direction + heading_weight: Cost weight for distance from the current heading + previous_weight: Cost weight for distance from the previous selection + """ + + if target_weight < 0.0 or heading_weight < 0.0 or previous_weight < 0.0: + raise ValueError("weights must be 0 or greater") + + self.target_weight = float(target_weight) + self.heading_weight = float(heading_weight) + self.previous_weight = float(previous_weight) + self.selected_angle_rad = None + + def select(self, valleys, vehicle_yaw_rad, target_angle_rad): + """ + Function to select one direction(global angle) that minimizes the + weighted cost function, from the candidate valleys' center angles + valleys: List of Valley instances from CandidateValleyDetector, + whose center angles are relative to the vehicle's heading + vehicle_yaw_rad: Vehicle's current heading in the global frame[rad] + target_angle_rad: Direction toward the goal, global frame[rad] + """ + + if not valleys: + self.selected_angle_rad = None + return None + + previous_angle_rad = self.selected_angle_rad + if previous_angle_rad is None: + previous_angle_rad = vehicle_yaw_rad + + best_angle_rad = None + lowest_cost = None + for valley in valleys: + candidate_angle_rad = pi_to_pi(valley.get_center_angle_rad() + vehicle_yaw_rad) + cost = self._cost(candidate_angle_rad, target_angle_rad, + vehicle_yaw_rad, previous_angle_rad) + if lowest_cost is None or cost < lowest_cost: + lowest_cost = cost + best_angle_rad = candidate_angle_rad + + self.selected_angle_rad = best_angle_rad + return self.selected_angle_rad + + def _cost(self, candidate_angle_rad, target_angle_rad, vehicle_yaw_rad, previous_angle_rad): + """ + Private function to calculate one candidate direction's weighted + cost, all angular differences taken the short way around the circle + candidate_angle_rad: Candidate direction being scored, global frame[rad] + target_angle_rad: Direction toward the goal, global frame[rad] + vehicle_yaw_rad: Vehicle's current heading, global frame[rad] + previous_angle_rad: Previously selected direction, global frame[rad] + """ + + target_diff = abs(pi_to_pi(candidate_angle_rad - target_angle_rad)) + heading_diff = abs(pi_to_pi(candidate_angle_rad - vehicle_yaw_rad)) + previous_diff = abs(pi_to_pi(candidate_angle_rad - previous_angle_rad)) + + return (self.target_weight * target_diff + + self.heading_weight * heading_diff + + self.previous_weight * previous_diff) + + def get_selected_angle_rad(self): + """ + Function to get the most recently selected direction in the global + frame[rad], or None if no valley has been selected yet(e.g. no + candidate valleys were available) + """ + + return self.selected_angle_rad diff --git a/src/components/mapping/polar_histogram/polar_histogram.py b/src/components/mapping/polar_histogram/polar_histogram.py index 8bf3c49..a353443 100644 --- a/src/components/mapping/polar_histogram/polar_histogram.py +++ b/src/components/mapping/polar_histogram/polar_histogram.py @@ -167,3 +167,25 @@ def get_smoothed_density(self): """ return self.smoothed_density + + def max_density_in_angle_range(self, center_angle_rad, half_width_rad=0.0): + """ + Function to get the highest smoothed density among the sectors + spanning a center angle and half_width_rad to each side of it. + Used(Step 5: Dynamic Speed Control) to gauge how close the + nearest obstacle is in roughly one direction without depending + on exact sector alignment + center_angle_rad: Center angle[rad], relative to vehicle heading + half_width_rad: Half width[rad] checked on each side of + center_angle_rad. 0 checks only the single sector + center_angle_rad falls into + """ + + if half_width_rad < 0.0: + raise ValueError("half_width_rad must be 0 or greater") + + half_width_sectors = int(np.ceil(half_width_rad / self.sector_angle_rad)) + center_index = self.angle_to_sector_index(center_angle_rad) + indices = [(center_index + offset) % self.num_sectors + for offset in range(-half_width_sectors, half_width_sectors + 1)] + return float(np.max(self.smoothed_density[indices])) diff --git a/src/components/mapping/polar_histogram/polar_histogram_mapper.py b/src/components/mapping/polar_histogram/polar_histogram_mapper.py index 0cacb4d..a042e39 100644 --- a/src/components/mapping/polar_histogram/polar_histogram_mapper.py +++ b/src/components/mapping/polar_histogram/polar_histogram_mapper.py @@ -4,25 +4,34 @@ Author: Khushi """ +from time import perf_counter + import numpy as np import matplotlib.patches as patches from matplotlib.collections import PatchCollection from polar_histogram import PolarHistogram +from candidate_valley_detector import CandidateValleyDetector +from direction_selector import DirectionSelector class PolarHistogramMapper: """ Mapper class to build and visualize a polar obstacle density histogram - around the vehicle from LiDAR point cloud data, following the mapping - stage of the Vector Field Histogram(VFH) algorithm. + around the vehicle from LiDAR point cloud data, following the mapping, + candidate valley detection, and direction selection stages of the + Vector Field Histogram(VFH) algorithm. Each frame, the histogram is drawn as a ring of colored wedges centered on the vehicle: denser(more blocked) sectors are drawn longer and closer to red, sparser(more open) sectors shorter and closer to green. Sector length and color are both normalized by the current frame's maximum density, so the ring stays readable regardless of how many obstacles - are in range. + are in range. Detected candidate valleys(navigable direction ranges, + Step 2) are drawn as green arcs just outside that ring. The direction + selected(Step 3) from those valleys is drawn as a bold blue arrow, with + a thin dashed line showing the target(goal) direction it was weighed + against. Note on drawing accuracy: each measurement's angle/distance is computed from the LiDAR's actual mounted position(see OmniDirectionalLidar), so @@ -36,13 +45,21 @@ class PolarHistogramMapper: steps derive from it - only where this ring is drawn on the plot. """ - def __init__(self, sensor_params=None, num_sectors=72, smoothing_window=5, ring_radius_m=8.0): + def __init__(self, sensor_params=None, num_sectors=72, smoothing_window=5, + ring_radius_m=8.0, valley_density_threshold=0.2, + target_x_m=None, target_y_m=None, + target_weight=1.0, heading_weight=1.0, previous_weight=1.0): """ Constructor sensor_params: LiDAR's SensorParameters object, used for max sensing range num_sectors: Number of angular sectors dividing 360[deg](resolution) smoothing_window: Half-width of the triangular smoothing filter(sectors) ring_radius_m: Drawing radius of the densest sector's wedge on the global plot[m] + valley_density_threshold: Smoothed density at/below which a sector counts as navigable + target_x_m, target_y_m: Fixed goal point[m] the target direction points toward. + When not given, the vehicle's current heading is used as + the target direction(i.e. "keep going straight") + target_weight, heading_weight, previous_weight: Direction selection cost weights """ max_range_m = sensor_params.MAX_RANGE_M if sensor_params else 40.0 @@ -50,31 +67,74 @@ def __init__(self, sensor_params=None, num_sectors=72, smoothing_window=5, ring_ self.histogram = PolarHistogram(num_sectors=num_sectors, max_range_m=max_range_m, smoothing_window=smoothing_window) + self.valley_detector = CandidateValleyDetector(density_threshold=valley_density_threshold) + self.direction_selector = DirectionSelector(target_weight=target_weight, + heading_weight=heading_weight, + previous_weight=previous_weight) self.ring_radius_m = ring_radius_m + self.target_x_m = target_x_m + self.target_y_m = target_y_m self.vehicle_x_m = 0.0 self.vehicle_y_m = 0.0 self.vehicle_yaw_rad = 0.0 + self.target_angle_rad = 0.0 + self.last_update_duration_s = 0.0 def update(self, point_cloud, state): """ - Function to update the polar histogram from the latest LiDAR point cloud + Function to update the polar histogram, candidate valleys, and + selected direction from the latest LiDAR point cloud point_cloud: List of ScanPoint objects from LiDAR. Each point's angle is expected to be relative to the vehicle's heading state: Vehicle's state object """ + start_s = perf_counter() + angle_list = [point.angle_rad for point in point_cloud] distance_list = [point.get_distance_m() for point in point_cloud] self.histogram.update(angle_list, distance_list) + self.valley_detector.detect(self.histogram) self.vehicle_x_m = state.get_x_m() self.vehicle_y_m = state.get_y_m() self.vehicle_yaw_rad = state.get_yaw_rad() + self.target_angle_rad = self._target_angle_rad() + self.direction_selector.select(self.valley_detector.get_valleys(), + self.vehicle_yaw_rad, self.target_angle_rad) + + self.last_update_duration_s = perf_counter() - start_s + + def _target_angle_rad(self): + """ + Private function to get the current target direction in the global + frame[rad]: toward(target_x_m, target_y_m) if given, otherwise + straight ahead(the vehicle's current heading) + """ + + if self.target_x_m is None or self.target_y_m is None: + return self.vehicle_yaw_rad + + return np.arctan2(self.target_y_m - self.vehicle_y_m, + self.target_x_m - self.vehicle_x_m) + def draw(self, axes, elems): """ - Function to draw the polar histogram as a ring of colored wedges around the vehicle + Function to draw the polar histogram ring, candidate valley arcs, + and the selected/target direction around the vehicle + axes: Axes object of figure + elems: List of plot objects + """ + + self._draw_density_ring(axes, elems) + self._draw_valleys(axes, elems) + self._draw_selected_direction(axes, elems) + + def _draw_density_ring(self, axes, elems): + """ + Private function to draw the polar histogram as a ring of colored wedges axes: Axes object of figure elems: List of plot objects """ @@ -113,9 +173,92 @@ def draw(self, axes, elems): axes.add_collection(collection) elems.append(collection) + def _draw_valleys(self, axes, elems): + """ + Private function to draw each detected candidate valley as a green + arc-shaped wedge just outside the density ring + axes: Axes object of figure + elems: List of plot objects + """ + + sector_deg = np.rad2deg(self.histogram.get_sector_angle_rad()) + yaw_deg = np.rad2deg(self.vehicle_yaw_rad) + inner_radius_m = self.ring_radius_m * 1.05 + outer_radius_m = self.ring_radius_m * 1.15 + + for valley in self.valley_detector.get_valleys(): + width_deg = valley.get_width_sectors() * sector_deg + center_deg = np.rad2deg(valley.get_center_angle_rad()) + yaw_deg + theta_1 = center_deg - width_deg / 2.0 + theta_2 = center_deg + width_deg / 2.0 + + valley_wedge = patches.Wedge((self.vehicle_x_m, self.vehicle_y_m), outer_radius_m, + theta_1, theta_2, width=outer_radius_m - inner_radius_m, + color="limegreen", alpha=0.5) + axes.add_patch(valley_wedge) + elems.append(valley_wedge) + + def _draw_selected_direction(self, axes, elems): + """ + Private function to draw the target direction as a thin dashed + line, and(when one was selected) the chosen steering direction as + a bold arrow, both from the vehicle's position + axes: Axes object of figure + elems: List of plot objects + """ + + arrow_length_m = self.ring_radius_m * 1.3 + + target_x = self.vehicle_x_m + arrow_length_m * np.cos(self.target_angle_rad) + target_y = self.vehicle_y_m + arrow_length_m * np.sin(self.target_angle_rad) + target_line, = axes.plot([self.vehicle_x_m, target_x], [self.vehicle_y_m, target_y], + linestyle="--", color="purple", linewidth=1.0, alpha=0.7) + elems.append(target_line) + + selected_angle_rad = self.direction_selector.get_selected_angle_rad() + if selected_angle_rad is None: + return + + end_x = self.vehicle_x_m + arrow_length_m * np.cos(selected_angle_rad) + end_y = self.vehicle_y_m + arrow_length_m * np.sin(selected_angle_rad) + arrow = axes.annotate("", xy=(end_x, end_y), xytext=(self.vehicle_x_m, self.vehicle_y_m), + arrowprops=dict(arrowstyle="->", color="blue", linewidth=2.5)) + elems.append(arrow) + def get_histogram(self): """ Function to get the underlying PolarHistogram instance """ return self.histogram + + def get_valley_detector(self): + """ + Function to get the underlying CandidateValleyDetector instance + """ + + return self.valley_detector + + def get_direction_selector(self): + """ + Function to get the underlying DirectionSelector instance + """ + + return self.direction_selector + + def get_target_angle_rad(self): + """ + Function to get the most recently computed target direction, + global frame[rad] + """ + + return self.target_angle_rad + + def get_last_update_duration_s(self): + """ + Function to get how long(sec) the last update() call took to build + the histogram, detect valleys and select a direction(Step 7: + Performance Benchmarking) + """ + + return self.last_update_duration_s diff --git a/src/simulations/mapping/vfh_candidate_valley_detection/vfh_candidate_valley_detection.py b/src/simulations/mapping/vfh_candidate_valley_detection/vfh_candidate_valley_detection.py new file mode 100644 index 0000000..6271d9b --- /dev/null +++ b/src/simulations/mapping/vfh_candidate_valley_detection/vfh_candidate_valley_detection.py @@ -0,0 +1,88 @@ +""" +vfh_candidate_valley_detection.py + +Title: VFH Candidate Valley Detection +Description: Detects runs of low-density sectors in the polar histogram as candidate steering directions +Author: Khushi +""" + +# import path setting +import numpy as np +import sys +from pathlib import Path + +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" + +sys.path.append(abs_dir_path + relative_path + "visualization") +sys.path.append(abs_dir_path + relative_path + "state") +sys.path.append(abs_dir_path + relative_path + "vehicle") +sys.path.append(abs_dir_path + relative_path + "obstacle") +sys.path.append(abs_dir_path + relative_path + "sensors") +sys.path.append(abs_dir_path + relative_path + "sensors/lidar") +sys.path.append(abs_dir_path + relative_path + "mapping/polar_histogram") + + +# import component modules +from global_xy_visualizer import GlobalXYVisualizer +from min_max import MinMax +from time_parameters import TimeParameters +from vehicle_specification import VehicleSpecification +from state import State +from four_wheels_vehicle import FourWheelsVehicle +from obstacle import Obstacle +from obstacle_list import ObstacleList +from sensors import Sensors +from sensor_parameters import SensorParameters +from omni_directional_lidar import OmniDirectionalLidar +from polar_histogram_mapper import PolarHistogramMapper + + +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True + + +def main(): + """ + Main process function + """ + + # set simulation parameters + x_lim, y_lim = MinMax(-30, 30), MinMax(-30, 30) + vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=20)) + + # create obstacle instances + # same scenario as vfh_polar_histogram_construction(Step 1), so the + # candidate valleys detected here can be compared directly against + # that step's raw density histogram + obst_list = ObstacleList() + obst1 = Obstacle(State(x_m=-5.0, y_m=15.0, speed_mps=1.0), yaw_rate_rps=np.deg2rad(10), width_m=1.0) + obst_list.add_obstacle(obst1) + obst2 = Obstacle(State(x_m=-15.0, y_m=-15.0), length_m=10.0, width_m=5.0) + obst_list.add_obstacle(obst2) + obst3 = Obstacle(State(x_m=20.0), yaw_rate_rps=np.deg2rad(15)) + obst_list.add_obstacle(obst3) + vis.add_object(obst_list) + + # create vehicle instance with the Step 2 polar histogram + candidate + # valley mapper + spec = VehicleSpecification(area_size=30.0) # spec instance + sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2) + lidar = OmniDirectionalLidar(obst_list, sensor_params) # lidar instance + mapper = PolarHistogramMapper(sensor_params=sensor_params, num_sectors=72, + smoothing_window=5, valley_density_threshold=0.2) # Step 2 mapper instance + vehicle = FourWheelsVehicle(State(color=spec.color), spec, + sensors=Sensors(lidar=lidar), mapper=mapper) # set state, spec, lidar, mapper as arguments + vis.add_object(vehicle) + + # plot figure is not shown when executed as unit test + if not show_plot: vis.not_show_plot() + + # show plot figure + vis.draw() + + +# execute main process +if __name__ == "__main__": + main() diff --git a/src/simulations/mapping/vfh_candidate_valley_detection/vfh_candidate_valley_detection_demo.gif b/src/simulations/mapping/vfh_candidate_valley_detection/vfh_candidate_valley_detection_demo.gif new file mode 100644 index 0000000..ee0be12 Binary files /dev/null and b/src/simulations/mapping/vfh_candidate_valley_detection/vfh_candidate_valley_detection_demo.gif differ diff --git a/src/simulations/mapping/vfh_direction_selection/vfh_direction_selection.py b/src/simulations/mapping/vfh_direction_selection/vfh_direction_selection.py new file mode 100644 index 0000000..1aaf235 --- /dev/null +++ b/src/simulations/mapping/vfh_direction_selection/vfh_direction_selection.py @@ -0,0 +1,94 @@ +""" +vfh_direction_selection.py + +Title: VFH Direction Selection +Description: Selects a steering direction from candidate valleys using a target, heading and previous-direction cost function +Author: Khushi +""" + +# import path setting +import numpy as np +import sys +from pathlib import Path + +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" + +sys.path.append(abs_dir_path + relative_path + "visualization") +sys.path.append(abs_dir_path + relative_path + "state") +sys.path.append(abs_dir_path + relative_path + "vehicle") +sys.path.append(abs_dir_path + relative_path + "obstacle") +sys.path.append(abs_dir_path + relative_path + "sensors") +sys.path.append(abs_dir_path + relative_path + "sensors/lidar") +sys.path.append(abs_dir_path + relative_path + "mapping/polar_histogram") + + +# import component modules +from global_xy_visualizer import GlobalXYVisualizer +from min_max import MinMax +from time_parameters import TimeParameters +from vehicle_specification import VehicleSpecification +from state import State +from four_wheels_vehicle import FourWheelsVehicle +from obstacle import Obstacle +from obstacle_list import ObstacleList +from sensors import Sensors +from sensor_parameters import SensorParameters +from omni_directional_lidar import OmniDirectionalLidar +from polar_histogram_mapper import PolarHistogramMapper + + +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True + + +def main(): + """ + Main process function + """ + + # set simulation parameters + x_lim, y_lim = MinMax(-30, 30), MinMax(-30, 30) + vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=20)) + + # create obstacle instances + # same scenario as vfh_polar_histogram_construction(Step 1) and + # vfh_candidate_valley_detection(Step 2), so this step's selected + # direction can be compared directly against those steps' output + obst_list = ObstacleList() + obst1 = Obstacle(State(x_m=-5.0, y_m=15.0, speed_mps=1.0), yaw_rate_rps=np.deg2rad(10), width_m=1.0) + obst_list.add_obstacle(obst1) + obst2 = Obstacle(State(x_m=-15.0, y_m=-15.0), length_m=10.0, width_m=5.0) + obst_list.add_obstacle(obst2) + obst3 = Obstacle(State(x_m=20.0), yaw_rate_rps=np.deg2rad(15)) + obst_list.add_obstacle(obst3) + vis.add_object(obst_list) + + # create vehicle instance with the Step 3 polar histogram + candidate + # valley + direction selection mapper. The goal point(25, 5) sits + # beyond obst3, so the selected direction(blue arrow) has to bend + # away from the straight-line target(purple dashed line) whenever + # obst3 blocks the way + spec = VehicleSpecification(area_size=30.0) # spec instance + sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2) + lidar = OmniDirectionalLidar(obst_list, sensor_params) # lidar instance + mapper = PolarHistogramMapper(sensor_params=sensor_params, num_sectors=72, + smoothing_window=5, valley_density_threshold=0.2, + target_x_m=25.0, target_y_m=5.0, + target_weight=1.0, heading_weight=0.5, + previous_weight=0.8) # Step 3 mapper instance + vehicle = FourWheelsVehicle(State(color=spec.color), spec, + sensors=Sensors(lidar=lidar), mapper=mapper) # set state, spec, lidar, mapper as arguments + vis.add_object(vehicle) + + # plot figure is not shown when executed as unit test + if not show_plot: vis.not_show_plot() + + # show plot figure + vis.draw() + + +# execute main process +if __name__ == "__main__": + main() diff --git a/src/simulations/mapping/vfh_direction_selection/vfh_direction_selection_demo.gif b/src/simulations/mapping/vfh_direction_selection/vfh_direction_selection_demo.gif new file mode 100644 index 0000000..761fbd5 Binary files /dev/null and b/src/simulations/mapping/vfh_direction_selection/vfh_direction_selection_demo.gif differ diff --git a/src/simulations/mapping/vfh_dynamic_speed_control/vfh_dynamic_speed_control.py b/src/simulations/mapping/vfh_dynamic_speed_control/vfh_dynamic_speed_control.py new file mode 100644 index 0000000..4cbd7cd --- /dev/null +++ b/src/simulations/mapping/vfh_dynamic_speed_control/vfh_dynamic_speed_control.py @@ -0,0 +1,100 @@ +""" +vfh_dynamic_speed_control.py + +Title: VFH Dynamic Speed Control +Description: Slows the VFH controller's target speed as obstacle density rises in the vehicle's steering direction +Author: Khushi +""" + +# import path setting +import numpy as np +import sys +from pathlib import Path + +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" + +sys.path.append(abs_dir_path + relative_path + "visualization") +sys.path.append(abs_dir_path + relative_path + "state") +sys.path.append(abs_dir_path + relative_path + "vehicle") +sys.path.append(abs_dir_path + relative_path + "obstacle") +sys.path.append(abs_dir_path + relative_path + "sensors") +sys.path.append(abs_dir_path + relative_path + "sensors/lidar") +sys.path.append(abs_dir_path + relative_path + "mapping/polar_histogram") +sys.path.append(abs_dir_path + relative_path + "control/vfh") + + +# import component modules +from global_xy_visualizer import GlobalXYVisualizer +from min_max import MinMax +from time_parameters import TimeParameters +from vehicle_specification import VehicleSpecification +from state import State +from four_wheels_vehicle import FourWheelsVehicle +from obstacle import Obstacle +from obstacle_list import ObstacleList +from sensors import Sensors +from sensor_parameters import SensorParameters +from omni_directional_lidar import OmniDirectionalLidar +from polar_histogram_mapper import PolarHistogramMapper +from vfh_controller import VfhController + + +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True + + +def main(): + """ + Main process function + """ + + # set simulation parameters + x_lim, y_lim = MinMax(-30, 30), MinMax(-30, 30) + vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25)) + + # create obstacle instances + # same scenario as vfh_polar_histogram_construction(Step 1) through + # vfh_vehicle_motion_integration(Step 4), so this step's driven speed + # can be compared directly against those steps' constant-speed output + obst_list = ObstacleList() + obst1 = Obstacle(State(x_m=-5.0, y_m=15.0, speed_mps=1.0), yaw_rate_rps=np.deg2rad(10), width_m=1.0) + obst_list.add_obstacle(obst1) + obst2 = Obstacle(State(x_m=-15.0, y_m=-15.0), length_m=10.0, width_m=5.0) + obst_list.add_obstacle(obst2) + obst3 = Obstacle(State(x_m=20.0), yaw_rate_rps=np.deg2rad(15)) + obst_list.add_obstacle(obst3) + vis.add_object(obst_list) + + # create vehicle instance with the Step 3 mapper for sensing plus a + # VfhController whose min_speed_mps/danger_density/caution_half_angle_rad + # now scale target speed down as obst3 is approached and steered around + # (Step 5), instead of holding cruise_speed_mps constant like Step 4 + spec = VehicleSpecification(area_size=30.0) # spec instance + sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2) + lidar = OmniDirectionalLidar(obst_list, sensor_params) # lidar instance + mapper = PolarHistogramMapper(sensor_params=sensor_params, num_sectors=72, + smoothing_window=5, valley_density_threshold=0.2, + target_x_m=25.0, target_y_m=5.0, + target_weight=1.0, heading_weight=0.5, + previous_weight=0.8) # Step 3 mapper instance + controller = VfhController(spec, mapper, cruise_speed_mps=3.0, + speed_gain=1.0, yaw_rate_gain=1.5, + max_yaw_rate_rps=1.0, min_speed_mps=0.5, + danger_density=1.0, + caution_half_angle_rad=np.deg2rad(10)) # Step 5 controller instance + vehicle = FourWheelsVehicle(State(color=spec.color), spec, controller=controller, + sensors=Sensors(lidar=lidar), mapper=mapper) # set state, spec, controller, lidar, mapper as arguments + vis.add_object(vehicle) + + # plot figure is not shown when executed as unit test + if not show_plot: vis.not_show_plot() + + # show plot figure + vis.draw() + + +# execute main process +if __name__ == "__main__": + main() diff --git a/src/simulations/mapping/vfh_dynamic_speed_control/vfh_dynamic_speed_control_demo.gif b/src/simulations/mapping/vfh_dynamic_speed_control/vfh_dynamic_speed_control_demo.gif new file mode 100644 index 0000000..915f1c8 Binary files /dev/null and b/src/simulations/mapping/vfh_dynamic_speed_control/vfh_dynamic_speed_control_demo.gif differ diff --git a/src/simulations/mapping/vfh_performance_benchmarking/vfh_performance_benchmarking.py b/src/simulations/mapping/vfh_performance_benchmarking/vfh_performance_benchmarking.py new file mode 100644 index 0000000..102fa1d --- /dev/null +++ b/src/simulations/mapping/vfh_performance_benchmarking/vfh_performance_benchmarking.py @@ -0,0 +1,107 @@ +""" +vfh_performance_benchmarking.py + +Title: VFH Performance Benchmarking +Description: Times the VFH+ mapper and controller pipeline each cycle and displays the running mean and worst case +Author: Khushi +""" + +# import path setting +import numpy as np +import sys +from pathlib import Path + +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" + +sys.path.append(abs_dir_path + relative_path + "visualization") +sys.path.append(abs_dir_path + relative_path + "state") +sys.path.append(abs_dir_path + relative_path + "vehicle") +sys.path.append(abs_dir_path + relative_path + "obstacle") +sys.path.append(abs_dir_path + relative_path + "sensors") +sys.path.append(abs_dir_path + relative_path + "sensors/lidar") +sys.path.append(abs_dir_path + relative_path + "mapping/polar_histogram") +sys.path.append(abs_dir_path + relative_path + "control/vfh") + + +# import component modules +from global_xy_visualizer import GlobalXYVisualizer +from min_max import MinMax +from time_parameters import TimeParameters +from vehicle_specification import VehicleSpecification +from state import State +from four_wheels_vehicle import FourWheelsVehicle +from obstacle import Obstacle +from obstacle_list import ObstacleList +from sensors import Sensors +from sensor_parameters import SensorParameters +from omni_directional_lidar import OmniDirectionalLidar +from polar_histogram_mapper import PolarHistogramMapper +from vfh_controller import VfhController +from performance_benchmark import PerformanceBenchmark + + +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True + + +def main(): + """ + Main process function + """ + + # set simulation parameters + x_lim, y_lim = MinMax(-30, 30), MinMax(-30, 30) + vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25)) + + # create obstacle instances + # same scenario as vfh_polar_histogram_construction(Step 1) through + # vfh_trajectory_verification(Step 6), so this step's timings are + # measured on the same workload those steps already run + obst_list = ObstacleList() + obst1 = Obstacle(State(x_m=-5.0, y_m=15.0, speed_mps=1.0), yaw_rate_rps=np.deg2rad(10), width_m=1.0) + obst_list.add_obstacle(obst1) + obst2 = Obstacle(State(x_m=-15.0, y_m=-15.0), length_m=10.0, width_m=5.0) + obst_list.add_obstacle(obst2) + obst3 = Obstacle(State(x_m=20.0), yaw_rate_rps=np.deg2rad(15)) + obst_list.add_obstacle(obst3) + vis.add_object(obst_list) + + # create vehicle instance with the Step 3 mapper plus the Step 4/5 + # controller driving it, same as earlier steps + spec = VehicleSpecification(area_size=30.0) # spec instance + sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2) + lidar = OmniDirectionalLidar(obst_list, sensor_params) # lidar instance + mapper = PolarHistogramMapper(sensor_params=sensor_params, num_sectors=72, + smoothing_window=5, valley_density_threshold=0.2, + target_x_m=25.0, target_y_m=5.0, + target_weight=1.0, heading_weight=0.5, + previous_weight=0.8) # Step 3 mapper instance + controller = VfhController(spec, mapper, cruise_speed_mps=3.0, + speed_gain=1.0, yaw_rate_gain=1.5, + max_yaw_rate_rps=1.0, min_speed_mps=0.5, + danger_density=1.0, + caution_half_angle_rad=np.deg2rad(10)) # Step 5 controller instance + vehicle = FourWheelsVehicle(State(color=spec.color), spec, controller=controller, + sensors=Sensors(lidar=lidar), mapper=mapper) # set state, spec, controller, lidar, mapper as arguments + vis.add_object(vehicle) + + # Step 7 benchmark: reads the mapper's and controller's own recorded + # update durations(both timed internally via time.perf_counter() as + # part of Step 7) and displays the running mean/worst case live. + # Added after the vehicle so it reads each cycle's durations only + # once the vehicle has updated the mapper and controller for this cycle + benchmark = PerformanceBenchmark(mapper, controller) + vis.add_object(benchmark) + + # plot figure is not shown when executed as unit test + if not show_plot: vis.not_show_plot() + + # show plot figure + vis.draw() + + +# execute main process +if __name__ == "__main__": + main() diff --git a/src/simulations/mapping/vfh_performance_benchmarking/vfh_performance_benchmarking_demo.gif b/src/simulations/mapping/vfh_performance_benchmarking/vfh_performance_benchmarking_demo.gif new file mode 100644 index 0000000..f717380 Binary files /dev/null and b/src/simulations/mapping/vfh_performance_benchmarking/vfh_performance_benchmarking_demo.gif differ diff --git a/src/simulations/mapping/vfh_polar_histogram_construction/vfh_polar_histogram_construction.py b/src/simulations/mapping/vfh_polar_histogram_construction/vfh_polar_histogram_construction.py index 17f2589..f04483d 100644 --- a/src/simulations/mapping/vfh_polar_histogram_construction/vfh_polar_histogram_construction.py +++ b/src/simulations/mapping/vfh_polar_histogram_construction/vfh_polar_histogram_construction.py @@ -1,6 +1,8 @@ """ vfh_polar_histogram_construction.py +Title: VFH Polar Histogram +Description: Builds a polar obstacle density histogram around the vehicle from LiDAR point clouds Author: Khushi """ diff --git a/src/simulations/mapping/vfh_trajectory_verification/vfh_trajectory_verification.py b/src/simulations/mapping/vfh_trajectory_verification/vfh_trajectory_verification.py new file mode 100644 index 0000000..ef64f4f --- /dev/null +++ b/src/simulations/mapping/vfh_trajectory_verification/vfh_trajectory_verification.py @@ -0,0 +1,112 @@ +""" +vfh_trajectory_verification.py + +Title: VFH Trajectory Verification +Description: Checks a VFH+ driven trajectory for near-collisions and goal arrival using the mapper's own sensed density +Author: Khushi +""" + +# import path setting +import numpy as np +import sys +from pathlib import Path + +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" + +sys.path.append(abs_dir_path + relative_path + "visualization") +sys.path.append(abs_dir_path + relative_path + "state") +sys.path.append(abs_dir_path + relative_path + "vehicle") +sys.path.append(abs_dir_path + relative_path + "obstacle") +sys.path.append(abs_dir_path + relative_path + "sensors") +sys.path.append(abs_dir_path + relative_path + "sensors/lidar") +sys.path.append(abs_dir_path + relative_path + "mapping/polar_histogram") +sys.path.append(abs_dir_path + relative_path + "control/vfh") + + +# import component modules +from global_xy_visualizer import GlobalXYVisualizer +from min_max import MinMax +from time_parameters import TimeParameters +from vehicle_specification import VehicleSpecification +from state import State +from four_wheels_vehicle import FourWheelsVehicle +from obstacle import Obstacle +from obstacle_list import ObstacleList +from sensors import Sensors +from sensor_parameters import SensorParameters +from omni_directional_lidar import OmniDirectionalLidar +from polar_histogram_mapper import PolarHistogramMapper +from vfh_controller import VfhController +from trajectory_verifier import TrajectoryVerifier + + +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True + + +def main(): + """ + Main process function + """ + + # set simulation parameters + x_lim, y_lim = MinMax(-30, 30), MinMax(-30, 30) + vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25)) + + # create obstacle instances + # same scenario as vfh_polar_histogram_construction(Step 1) through + # vfh_dynamic_speed_control(Step 5), so this step's verification result + # can be checked directly against those steps' driven trajectories + obst_list = ObstacleList() + obst1 = Obstacle(State(x_m=-5.0, y_m=15.0, speed_mps=1.0), yaw_rate_rps=np.deg2rad(10), width_m=1.0) + obst_list.add_obstacle(obst1) + obst2 = Obstacle(State(x_m=-15.0, y_m=-15.0), length_m=10.0, width_m=5.0) + obst_list.add_obstacle(obst2) + obst3 = Obstacle(State(x_m=20.0), yaw_rate_rps=np.deg2rad(15)) + obst_list.add_obstacle(obst3) + vis.add_object(obst_list) + + # create vehicle instance with the Step 3 mapper plus the Step 4/5 + # controller driving it, same as earlier steps + target_x_m, target_y_m = 25.0, 5.0 + spec = VehicleSpecification(area_size=30.0) # spec instance + sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2) + lidar = OmniDirectionalLidar(obst_list, sensor_params) # lidar instance + mapper = PolarHistogramMapper(sensor_params=sensor_params, num_sectors=72, + smoothing_window=5, valley_density_threshold=0.2, + target_x_m=target_x_m, target_y_m=target_y_m, + target_weight=1.0, heading_weight=0.5, + previous_weight=0.8) # Step 3 mapper instance + controller = VfhController(spec, mapper, cruise_speed_mps=3.0, + speed_gain=1.0, yaw_rate_gain=1.5, + max_yaw_rate_rps=1.0, min_speed_mps=0.5, + danger_density=1.0, + caution_half_angle_rad=np.deg2rad(10)) # Step 5 controller instance + # kept as its own variable(instead of built inline) so the same state + # object can also be handed to the verifier below + state = State(color=spec.color) + vehicle = FourWheelsVehicle(state, spec, controller=controller, + sensors=Sensors(lidar=lidar), mapper=mapper) # set state, spec, controller, lidar, mapper as arguments + vis.add_object(vehicle) + + # Step 6 verifier: reads the same mapper the controller steers by, to + # report - from the VFH+ pipeline's own sensed data - whether this + # trajectory ever got dangerously close to an obstacle, and whether + # it reaches the goal. Added after the vehicle so it reads each + # cycle's density/position only once the vehicle has updated them + verifier = TrajectoryVerifier(mapper, state, target_x_m=target_x_m, target_y_m=target_y_m, + danger_density=1.0, goal_tolerance_m=2.0) + vis.add_object(verifier) + + # plot figure is not shown when executed as unit test + if not show_plot: vis.not_show_plot() + + # show plot figure + vis.draw() + + +# execute main process +if __name__ == "__main__": + main() diff --git a/src/simulations/mapping/vfh_vehicle_motion_integration/vfh_vehicle_motion_integration.py b/src/simulations/mapping/vfh_vehicle_motion_integration/vfh_vehicle_motion_integration.py new file mode 100644 index 0000000..68fae35 --- /dev/null +++ b/src/simulations/mapping/vfh_vehicle_motion_integration/vfh_vehicle_motion_integration.py @@ -0,0 +1,99 @@ +""" +vfh_vehicle_motion_integration.py + +Title: VFH Vehicle Motion Integration +Description: Drives the vehicle toward a goal using a VFH controller that steers along the mapper's selected direction +Author: Khushi +""" + +# import path setting +import numpy as np +import sys +from pathlib import Path + +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" + +sys.path.append(abs_dir_path + relative_path + "visualization") +sys.path.append(abs_dir_path + relative_path + "state") +sys.path.append(abs_dir_path + relative_path + "vehicle") +sys.path.append(abs_dir_path + relative_path + "obstacle") +sys.path.append(abs_dir_path + relative_path + "sensors") +sys.path.append(abs_dir_path + relative_path + "sensors/lidar") +sys.path.append(abs_dir_path + relative_path + "mapping/polar_histogram") +sys.path.append(abs_dir_path + relative_path + "control/vfh") + + +# import component modules +from global_xy_visualizer import GlobalXYVisualizer +from min_max import MinMax +from time_parameters import TimeParameters +from vehicle_specification import VehicleSpecification +from state import State +from four_wheels_vehicle import FourWheelsVehicle +from obstacle import Obstacle +from obstacle_list import ObstacleList +from sensors import Sensors +from sensor_parameters import SensorParameters +from omni_directional_lidar import OmniDirectionalLidar +from polar_histogram_mapper import PolarHistogramMapper +from vfh_controller import VfhController + + +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True + + +def main(): + """ + Main process function + """ + + # set simulation parameters + x_lim, y_lim = MinMax(-30, 30), MinMax(-30, 30) + vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25)) + + # create obstacle instances + # same scenario as vfh_polar_histogram_construction(Step 1), + # vfh_candidate_valley_detection(Step 2) and + # vfh_direction_selection(Step 3), so this step's driven trajectory + # can be compared directly against those steps' output + obst_list = ObstacleList() + obst1 = Obstacle(State(x_m=-5.0, y_m=15.0, speed_mps=1.0), yaw_rate_rps=np.deg2rad(10), width_m=1.0) + obst_list.add_obstacle(obst1) + obst2 = Obstacle(State(x_m=-15.0, y_m=-15.0), length_m=10.0, width_m=5.0) + obst_list.add_obstacle(obst2) + obst3 = Obstacle(State(x_m=20.0), yaw_rate_rps=np.deg2rad(15)) + obst_list.add_obstacle(obst3) + vis.add_object(obst_list) + + # create vehicle instance with the Step 3 mapper for sensing plus a + # new VfhController(Step 4) that actually drives the vehicle toward + # the mapper's selected direction. The goal point(25, 5) sits beyond + # obst3, same as Step 3, so the vehicle has to steer around it + spec = VehicleSpecification(area_size=30.0) # spec instance + sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2) + lidar = OmniDirectionalLidar(obst_list, sensor_params) # lidar instance + mapper = PolarHistogramMapper(sensor_params=sensor_params, num_sectors=72, + smoothing_window=5, valley_density_threshold=0.2, + target_x_m=25.0, target_y_m=5.0, + target_weight=1.0, heading_weight=0.5, + previous_weight=0.8) # Step 3 mapper instance + controller = VfhController(spec, mapper, cruise_speed_mps=3.0, + speed_gain=1.0, yaw_rate_gain=1.5, + max_yaw_rate_rps=1.0) # Step 4 controller instance + vehicle = FourWheelsVehicle(State(color=spec.color), spec, controller=controller, + sensors=Sensors(lidar=lidar), mapper=mapper) # set state, spec, controller, lidar, mapper as arguments + vis.add_object(vehicle) + + # plot figure is not shown when executed as unit test + if not show_plot: vis.not_show_plot() + + # show plot figure + vis.draw() + + +# execute main process +if __name__ == "__main__": + main() diff --git a/src/simulations/mapping/vfh_vehicle_motion_integration/vfh_vehicle_motion_integration_demo.gif b/src/simulations/mapping/vfh_vehicle_motion_integration/vfh_vehicle_motion_integration_demo.gif new file mode 100644 index 0000000..e4664d1 Binary files /dev/null and b/src/simulations/mapping/vfh_vehicle_motion_integration/vfh_vehicle_motion_integration_demo.gif differ diff --git a/test/test_candidate_valley_detector.py b/test/test_candidate_valley_detector.py new file mode 100644 index 0000000..cda7084 --- /dev/null +++ b/test/test_candidate_valley_detector.py @@ -0,0 +1,130 @@ +""" +Unit test of CandidateValleyDetector + +Author: Khushi +""" + +import numpy as np +import pytest +import sys +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/mapping/polar_histogram") +from polar_histogram import PolarHistogram +from candidate_valley_detector import CandidateValleyDetector + + +def _histogram_with_density(density_values, max_range_m=10.0): + """ + Helper to build a PolarHistogram and force its smoothed density to a + known array, bypassing LiDAR-based accumulation so valley detection + can be tested against exact, hand-picked density patterns + """ + + hist = PolarHistogram(num_sectors=len(density_values), max_range_m=max_range_m, smoothing_window=1) + hist.smoothed_density = np.array(density_values, dtype=float) + return hist + + +def test_invalid_threshold_raises(): + with pytest.raises(ValueError): + CandidateValleyDetector(density_threshold=-0.1) + + +def test_no_obstacles_gives_one_full_valley(): + hist = _histogram_with_density([0.0] * 8) + detector = CandidateValleyDetector(density_threshold=0.2) + + valleys = detector.detect(hist) + + assert len(valleys) == 1 + assert valleys[0].get_start_index() == 0 + assert valleys[0].get_end_index() == 7 + assert valleys[0].get_width_sectors() == 8 + + +def test_all_sectors_blocked_gives_no_valleys(): + hist = _histogram_with_density([1.0] * 8) + detector = CandidateValleyDetector(density_threshold=0.2) + + valleys = detector.detect(hist) + + assert valleys == [] + + +def test_single_obstacle_leaves_one_large_valley(): + # sector 0 blocked, sectors 1-7 navigable + hist = _histogram_with_density([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + detector = CandidateValleyDetector(density_threshold=0.2) + + valleys = detector.detect(hist) + + assert len(valleys) == 1 + assert valleys[0].get_start_index() == 1 + assert valleys[0].get_end_index() == 7 + assert valleys[0].get_width_sectors() == 7 + + +def test_multiple_obstacles_create_multiple_valleys(): + # sectors 0 and 4 blocked out of 8: two valleys of width 3 each + hist = _histogram_with_density([1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + detector = CandidateValleyDetector(density_threshold=0.2) + + valleys = detector.detect(hist) + + assert len(valleys) == 2 + widths = sorted(v.get_width_sectors() for v in valleys) + assert widths == [3, 3] + + +def test_valley_wraps_across_zero_index(): + # sectors 2 and 3 blocked out of 8: the navigable run is 4,5,6,7,0,1, + # wrapping across the 0[deg] boundary + hist = _histogram_with_density([0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]) + detector = CandidateValleyDetector(density_threshold=0.2) + + valleys = detector.detect(hist) + + assert len(valleys) == 1 + assert valleys[0].get_start_index() == 4 + assert valleys[0].get_end_index() == 1 + assert valleys[0].get_width_sectors() == 6 + + +def test_valley_center_angle(): + # sectors 0-3 blocked, 4-7 navigable(8 sectors -> 45[deg] each): the + # valley's center should land on the 4/7 boundary sector split, 270[deg] + hist = _histogram_with_density([1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]) + detector = CandidateValleyDetector(density_threshold=0.2) + + valleys = detector.detect(hist) + + assert len(valleys) == 1 + assert np.isclose(valleys[0].get_center_angle_rad(), np.deg2rad(-90)) + + +def test_threshold_is_configurable(): + hist = _histogram_with_density([0.1, 0.3, 0.1, 0.3]) + + strict = CandidateValleyDetector(density_threshold=0.05) + assert strict.detect(hist) == [] + + lenient = CandidateValleyDetector(density_threshold=0.3) + valleys = lenient.detect(hist) + assert len(valleys) == 1 + assert valleys[0].get_width_sectors() == 4 + + +def test_get_valleys_returns_last_detection(): + hist = _histogram_with_density([1.0, 0.0]) + detector = CandidateValleyDetector(density_threshold=0.2) + + assert detector.get_valleys() == [] + detector.detect(hist) + assert len(detector.get_valleys()) == 1 + + +def test_get_density_threshold(): + detector = CandidateValleyDetector(density_threshold=0.4) + + assert detector.get_density_threshold() == 0.4 diff --git a/test/test_direction_selector.py b/test/test_direction_selector.py new file mode 100644 index 0000000..87fd971 --- /dev/null +++ b/test/test_direction_selector.py @@ -0,0 +1,128 @@ +""" +Unit test of DirectionSelector + +Author: Khushi +""" + +import numpy as np +import pytest +import sys +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/mapping/polar_histogram") +from candidate_valley_detector import Valley +from direction_selector import DirectionSelector + + +def _valley_at(center_angle_deg): + """ + Helper to build a minimal Valley with only its center angle set, since + DirectionSelector only reads get_center_angle_rad() + """ + + return Valley(start_index=0, end_index=0, width_sectors=1, + center_angle_rad=np.deg2rad(center_angle_deg)) + + +def test_invalid_weight_raises(): + with pytest.raises(ValueError): + DirectionSelector(target_weight=-1.0) + + +def test_selected_angle_starts_as_none(): + selector = DirectionSelector() + + assert selector.get_selected_angle_rad() is None + + +def test_no_valleys_returns_none(): + selector = DirectionSelector() + + result = selector.select([], vehicle_yaw_rad=0.0, target_angle_rad=0.0) + + assert result is None + assert selector.get_selected_angle_rad() is None + + +def test_single_valley_is_always_selected(): + selector = DirectionSelector() + valleys = [_valley_at(45)] + + result = selector.select(valleys, vehicle_yaw_rad=0.0, target_angle_rad=np.deg2rad(-170)) + + assert np.isclose(result, np.deg2rad(45)) + + +def test_selects_valley_closest_to_target_direction(): + selector = DirectionSelector(target_weight=1.0, heading_weight=0.0, previous_weight=0.0) + valleys = [_valley_at(0), _valley_at(90)] + + result = selector.select(valleys, vehicle_yaw_rad=0.0, target_angle_rad=np.deg2rad(10)) + + assert np.isclose(result, np.deg2rad(0)) + + +def test_selects_valley_closest_to_current_heading(): + selector = DirectionSelector(target_weight=0.0, heading_weight=1.0, previous_weight=0.0) + valleys = [_valley_at(10), _valley_at(90)] + + # target strongly favours the 90[deg] valley, but only heading_weight + # is active, so the candidate closest to straight-ahead(0[deg]) wins + result = selector.select(valleys, vehicle_yaw_rad=0.0, target_angle_rad=np.deg2rad(90)) + + assert np.isclose(result, np.deg2rad(10)) + + +def test_valley_angle_is_converted_from_vehicle_relative_to_global(): + selector = DirectionSelector(target_weight=1.0, heading_weight=0.0, previous_weight=0.0) + # vehicle faces global 90[deg]; the only valley is straight ahead of + # the vehicle(0[deg] relative), which should resolve to global 90[deg] + valleys = [_valley_at(0)] + + result = selector.select(valleys, vehicle_yaw_rad=np.deg2rad(90), target_angle_rad=np.deg2rad(90)) + + assert np.isclose(result, np.deg2rad(90)) + + +def test_first_call_uses_vehicle_heading_as_previous_direction(): + # with no prior selection, previous_weight should pull toward the + # vehicle's current heading, same as heading_weight would alone + selector = DirectionSelector(target_weight=0.0, heading_weight=0.0, previous_weight=1.0) + valleys = [_valley_at(5), _valley_at(90)] + + result = selector.select(valleys, vehicle_yaw_rad=0.0, target_angle_rad=0.0) + + assert np.isclose(result, np.deg2rad(5)) + + +def test_previous_direction_can_override_heading_preference(): + selector = DirectionSelector(target_weight=0.0, heading_weight=1.0, previous_weight=5.0) + + # first call: vehicle faces global 0[deg], two valleys straight ahead + # of it, at relative 0[deg](-> global 0) and 90[deg](-> global 90). + # heading_weight alone would prefer global 0(closer to current heading) + first = selector.select([_valley_at(0), _valley_at(90)], + vehicle_yaw_rad=0.0, target_angle_rad=0.0) + assert np.isclose(first, np.deg2rad(0)) + + # second call: the vehicle has turned to face global 90[deg]. The same + # two physical gaps are now at relative -90[deg](-> global 0, same as + # the previous selection) and 0[deg](-> global 90, straight ahead now). + # heading_weight alone would switch to global 90, but a strong + # previous_weight should keep global 0 selected instead + second = selector.select([_valley_at(-90), _valley_at(0)], + vehicle_yaw_rad=np.deg2rad(90), target_angle_rad=0.0) + + assert np.isclose(second, np.deg2rad(0)) + + +def test_angle_wraps_correctly_near_boundary(): + selector = DirectionSelector(target_weight=1.0, heading_weight=0.0, previous_weight=0.0) + # a valley at 170[deg] is actually close(20[deg]) to a target at + # -170[deg] going the short way around, despite the raw numbers + # looking far apart + valleys = [_valley_at(170), _valley_at(0)] + + result = selector.select(valleys, vehicle_yaw_rad=0.0, target_angle_rad=np.deg2rad(-170)) + + assert np.isclose(result, np.deg2rad(170)) diff --git a/test/test_performance_benchmark.py b/test/test_performance_benchmark.py new file mode 100644 index 0000000..984ca3b --- /dev/null +++ b/test/test_performance_benchmark.py @@ -0,0 +1,96 @@ +""" +Unit test of PerformanceBenchmark + +Author: Khushi +""" + +import pytest +import sys +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/control/vfh") +from performance_benchmark import PerformanceBenchmark + + +class _FakeTimedComponent: + """ + Minimal stand-in for PolarHistogramMapper/VfhController exposing only + the getter PerformanceBenchmark actually reads + """ + + def __init__(self, duration_s=0.0): + self.duration_s = duration_s + + def get_last_update_duration_s(self): + return self.duration_s + + def set_duration_s(self, duration_s): + self.duration_s = duration_s + + +def test_initial_stats_are_zero(): + benchmark = PerformanceBenchmark(_FakeTimedComponent(), _FakeTimedComponent()) + + assert benchmark.get_last_duration_s() == 0.0 + assert benchmark.get_mean_duration_s() == 0.0 + assert benchmark.get_max_duration_s() == 0.0 + + +def test_last_duration_is_mapper_plus_controller(): + mapper = _FakeTimedComponent(0.002) + controller = _FakeTimedComponent(0.001) + benchmark = PerformanceBenchmark(mapper, controller) + + benchmark.update(0.1) + + assert benchmark.get_last_duration_s() == pytest.approx(0.003) + + +def test_mean_duration_averages_across_updates(): + mapper = _FakeTimedComponent(0.0) + controller = _FakeTimedComponent(0.0) + benchmark = PerformanceBenchmark(mapper, controller) + + mapper.set_duration_s(0.001) + controller.set_duration_s(0.001) + benchmark.update(0.1) # total 0.002 + + mapper.set_duration_s(0.003) + controller.set_duration_s(0.001) + benchmark.update(0.1) # total 0.004 + + assert benchmark.get_mean_duration_s() == pytest.approx(0.003) + + +def test_max_duration_tracks_worst_case(): + mapper = _FakeTimedComponent(0.0) + controller = _FakeTimedComponent(0.0) + benchmark = PerformanceBenchmark(mapper, controller) + + mapper.set_duration_s(0.005) + benchmark.update(0.1) + assert benchmark.get_max_duration_s() == pytest.approx(0.005) + + mapper.set_duration_s(0.001) # a smaller reading afterwards must not lower the max + benchmark.update(0.1) + assert benchmark.get_max_duration_s() == pytest.approx(0.005) + + mapper.set_duration_s(0.009) + benchmark.update(0.1) + assert benchmark.get_max_duration_s() == pytest.approx(0.009) + + +def test_draw_does_not_raise(): + benchmark = PerformanceBenchmark(_FakeTimedComponent(0.001), _FakeTimedComponent(0.001)) + benchmark.update(0.1) + + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + figure, axes = plt.subplots() + elems = [] + + benchmark.draw(axes, elems) + + assert len(elems) == 1 + plt.close(figure) diff --git a/test/test_polar_histogram.py b/test/test_polar_histogram.py index a5d60ef..7505adf 100644 --- a/test/test_polar_histogram.py +++ b/test/test_polar_histogram.py @@ -102,3 +102,39 @@ def test_smoothing_disabled_when_window_is_one(): hist.update([0.0], [2.0]) assert np.array_equal(hist.get_smoothed_density(), hist.get_raw_density()) + + +def test_max_density_in_angle_range_defaults_to_single_sector(): + hist = PolarHistogram(num_sectors=4, max_range_m=10.0, smoothing_window=1) + + hist.update([0.0], [2.0]) # obstacle in sector 0 only + + assert hist.max_density_in_angle_range(0.0) > 0.0 # looks at sector 0 itself + assert hist.max_density_in_angle_range(np.pi) == 0.0 # sector 2, opposite side, empty + + +def test_max_density_in_angle_range_checks_neighbouring_sectors(): + hist = PolarHistogram(num_sectors=8, max_range_m=10.0, smoothing_window=1) + + hist.update([np.deg2rad(45)], [2.0]) # obstacle in sector 1 only(sector width is 45deg) + + # centered on sector 0, a half width reaching one neighbour includes sector 1 + assert hist.max_density_in_angle_range(0.0, half_width_rad=np.deg2rad(45)) > 0.0 + # too narrow a range centered on sector 0 never reaches sector 1 + assert hist.max_density_in_angle_range(0.0, half_width_rad=0.0) == 0.0 + + +def test_max_density_in_angle_range_wraps_around_boundary(): + hist = PolarHistogram(num_sectors=4, max_range_m=10.0, smoothing_window=1) + + hist.update([np.deg2rad(-1)], [2.0]) # -1deg == 359deg, sector 3 + + # sector 0 is adjacent to sector 3 across the 0deg boundary + assert hist.max_density_in_angle_range(0.0, half_width_rad=np.deg2rad(90)) > 0.0 + + +def test_max_density_in_angle_range_invalid_half_width_raises(): + hist = PolarHistogram(num_sectors=4, max_range_m=10.0) + + with pytest.raises(ValueError): + hist.max_density_in_angle_range(0.0, half_width_rad=-0.1) diff --git a/test/test_polar_histogram_mapper.py b/test/test_polar_histogram_mapper.py index 339e202..7a866bf 100644 --- a/test/test_polar_histogram_mapper.py +++ b/test/test_polar_histogram_mapper.py @@ -7,6 +7,7 @@ import matplotlib as mpl mpl.use("Agg") import matplotlib.pyplot as plt +import numpy as np import sys from pathlib import Path @@ -62,7 +63,12 @@ def test_update_and_draw_without_error(): assert len(elems) > 0 -def test_draw_with_no_detections_adds_nothing(): +def test_draw_with_no_detections_adds_full_valley_and_target_line(): + # with no LiDAR detections the density ring(Step 1) contributes + # nothing, but candidate valley detection(Step 2) still finds the + # entire circle navigable(1 element) and direction selection(Step 3) + # always draws the target reference line(1 element) plus, since a + # valley exists, a selected-direction arrow(1 element) mapper = PolarHistogramMapper(num_sectors=36) state = State() @@ -72,4 +78,51 @@ def test_draw_with_no_detections_adds_nothing(): elems = [] mapper.draw(axes, elems) - assert len(elems) == 0 + assert len(elems) == 3 + + +def test_update_detects_valley_between_two_obstacle_clusters(): + mapper = PolarHistogramMapper(num_sectors=36, smoothing_window=1, valley_density_threshold=0.2) + # two tight clusters of close obstacles roughly opposite each other, + # leaving open gaps in between + point_cloud = [DummyScanPoint(0.0, 1.0), DummyScanPoint(0.05, 1.0), + DummyScanPoint(3.14, 1.0), DummyScanPoint(3.09, 1.0)] + state = State() + + mapper.update(point_cloud, state) + + valleys = mapper.get_valley_detector().get_valleys() + assert len(valleys) >= 1 + + +def test_get_valley_detector_returns_configured_threshold(): + mapper = PolarHistogramMapper(num_sectors=36, valley_density_threshold=0.4) + + assert mapper.get_valley_detector().get_density_threshold() == 0.4 + + +def test_target_defaults_to_vehicle_heading_when_not_given(): + mapper = PolarHistogramMapper(num_sectors=36) + state = State(x_m=0.0, y_m=0.0, yaw_rad=0.7) + + mapper.update([], state) + + assert np.isclose(mapper.get_target_angle_rad(), 0.7) + + +def test_target_points_toward_configured_goal(): + mapper = PolarHistogramMapper(num_sectors=36, target_x_m=10.0, target_y_m=10.0) + state = State(x_m=0.0, y_m=0.0, yaw_rad=0.0) + + mapper.update([], state) + + assert np.isclose(mapper.get_target_angle_rad(), np.deg2rad(45)) + + +def test_direction_selector_picks_a_direction_when_valleys_exist(): + mapper = PolarHistogramMapper(num_sectors=36) + state = State() + + mapper.update([], state) + + assert mapper.get_direction_selector().get_selected_angle_rad() is not None diff --git a/test/test_trajectory_verifier.py b/test/test_trajectory_verifier.py new file mode 100644 index 0000000..9d2cca6 --- /dev/null +++ b/test/test_trajectory_verifier.py @@ -0,0 +1,139 @@ +""" +Unit test of TrajectoryVerifier + +Author: Khushi +""" + +import numpy as np +import pytest +import sys +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/state") +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/control/vfh") +from state import State +from trajectory_verifier import TrajectoryVerifier + + +class _FakeHistogram: + """ + Minimal stand-in for PolarHistogram exposing only the getter + TrajectoryVerifier actually reads + """ + + def __init__(self, density): + self.density = np.array(density, dtype=float) + + def get_smoothed_density(self): + return self.density + + +class _FakeMapper: + """ + Minimal stand-in for a mapper exposing only get_histogram() + """ + + def __init__(self, density): + self.histogram = _FakeHistogram(density) + + def get_histogram(self): + return self.histogram + + def set_density(self, density): + self.histogram.density = np.array(density, dtype=float) + + +def test_invalid_danger_density_raises(): + mapper = _FakeMapper([0.0]) + state = State(x_m=0.0, y_m=0.0) + + with pytest.raises(ValueError): + TrajectoryVerifier(mapper, state, 0.0, 0.0, danger_density=0.0) + with pytest.raises(ValueError): + TrajectoryVerifier(mapper, state, 0.0, 0.0, danger_density=-1.0) + + +def test_invalid_goal_tolerance_raises(): + mapper = _FakeMapper([0.0]) + state = State(x_m=0.0, y_m=0.0) + + with pytest.raises(ValueError): + TrajectoryVerifier(mapper, state, 0.0, 0.0, goal_tolerance_m=-1.0) + + +def test_distance_to_goal_is_euclidean(): + mapper = _FakeMapper([0.0]) + state = State(x_m=0.0, y_m=0.0) + verifier = TrajectoryVerifier(mapper, state, target_x_m=3.0, target_y_m=4.0) + + assert verifier.distance_to_goal_m() == pytest.approx(5.0) + + +def test_reached_goal_within_tolerance(): + mapper = _FakeMapper([0.0]) + state = State(x_m=9.0, y_m=0.0) + verifier = TrajectoryVerifier(mapper, state, target_x_m=10.0, target_y_m=0.0, + goal_tolerance_m=2.0) + + assert verifier.reached_goal() is True + + +def test_not_reached_goal_outside_tolerance(): + mapper = _FakeMapper([0.0]) + state = State(x_m=0.0, y_m=0.0) + verifier = TrajectoryVerifier(mapper, state, target_x_m=10.0, target_y_m=0.0, + goal_tolerance_m=2.0) + + assert verifier.reached_goal() is False + + +def test_max_density_seen_tracks_running_maximum(): + mapper = _FakeMapper([0.1, 0.2]) + state = State(x_m=0.0, y_m=0.0) + verifier = TrajectoryVerifier(mapper, state, 0.0, 0.0) + + verifier.update(0.1) + assert verifier.get_max_density_seen() == pytest.approx(0.2) + + mapper.set_density([0.05]) # a lower reading afterwards must not lower the running max + verifier.update(0.1) + assert verifier.get_max_density_seen() == pytest.approx(0.2) + + mapper.set_density([0.9]) + verifier.update(0.1) + assert verifier.get_max_density_seen() == pytest.approx(0.9) + + +def test_near_collision_count_increments_at_or_above_danger_density(): + mapper = _FakeMapper([0.5]) + state = State(x_m=0.0, y_m=0.0) + verifier = TrajectoryVerifier(mapper, state, 0.0, 0.0, danger_density=1.0) + + verifier.update(0.1) + assert verifier.get_near_collision_count() == 0 + + mapper.set_density([1.0]) + verifier.update(0.1) + assert verifier.get_near_collision_count() == 1 + + mapper.set_density([2.0]) + verifier.update(0.1) + assert verifier.get_near_collision_count() == 2 + + +def test_draw_does_not_raise(): + mapper = _FakeMapper([0.0]) + state = State(x_m=0.0, y_m=0.0) + verifier = TrajectoryVerifier(mapper, state, 0.0, 0.0) + verifier.update(0.1) + + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + figure, axes = plt.subplots() + elems = [] + + verifier.draw(axes, elems) + + assert len(elems) == 1 + plt.close(figure) diff --git a/test/test_vfh_candidate_valley_detection.py b/test/test_vfh_candidate_valley_detection.py new file mode 100644 index 0000000..889ace5 --- /dev/null +++ b/test/test_vfh_candidate_valley_detection.py @@ -0,0 +1,17 @@ +""" +Test of VFH candidate valley detection simulation + +Author: Khushi +""" + +from pathlib import Path +import sys + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/mapping/vfh_candidate_valley_detection") +import vfh_candidate_valley_detection + + +def test_simulation(): + vfh_candidate_valley_detection.show_plot = False + + vfh_candidate_valley_detection.main() diff --git a/test/test_vfh_controller.py b/test/test_vfh_controller.py new file mode 100644 index 0000000..f079441 --- /dev/null +++ b/test/test_vfh_controller.py @@ -0,0 +1,284 @@ +""" +Unit test of VfhController + +Author: Khushi +""" + +from math import atan2, pi +import pytest +import sys +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/state") +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/vehicle") +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/control/vfh") +from state import State +from vehicle_specification import VehicleSpecification +from vfh_controller import VfhController + + +class _FakeDirectionSelector: + """ + Minimal stand-in for DirectionSelector exposing only the getter + VfhController actually reads, so the controller can be unit tested + without building a full PolarHistogramMapper/valley pipeline + """ + + def __init__(self, angle_rad): + self.angle_rad = angle_rad + + def get_selected_angle_rad(self): + return self.angle_rad + + +class _FakeHistogram: + """ + Minimal stand-in for PolarHistogram exposing only the query + VfhController actually reads(Step 5: Dynamic Speed Control), so + density-based speed control can be unit tested without building a + real sector/density array. Remembers its last call's arguments so + tests can confirm the controller queries the right direction + """ + + def __init__(self, density_ahead=0.0): + self.density_ahead = density_ahead + self.last_center_angle_rad = None + self.last_half_width_rad = None + + def max_density_in_angle_range(self, center_angle_rad, half_width_rad=0.0): + self.last_center_angle_rad = center_angle_rad + self.last_half_width_rad = half_width_rad + return self.density_ahead + + +class _FakeMapper: + """ + Minimal stand-in for a mapper exposing only get_direction_selector() + and get_histogram() + """ + + def __init__(self, angle_rad, density_ahead=0.0): + self.selector = _FakeDirectionSelector(angle_rad) + self.histogram = _FakeHistogram(density_ahead) + + def get_direction_selector(self): + return self.selector + + def get_histogram(self): + return self.histogram + + def set_selected_angle_rad(self, angle_rad): + self.selector.angle_rad = angle_rad + + def set_density_ahead(self, density_ahead): + self.histogram.density_ahead = density_ahead + + +def _spec(): + return VehicleSpecification() # wheel_base_m = 2.0 by default + + +def test_invalid_cruise_speed_raises(): + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), cruise_speed_mps=-1.0) + + +def test_invalid_gains_raise(): + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), speed_gain=-1.0) + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), yaw_rate_gain=-1.0) + + +def test_invalid_max_yaw_rate_raises(): + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), max_yaw_rate_rps=0.0) + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), max_yaw_rate_rps=-1.0) + + +def test_falls_back_to_vehicle_heading_when_no_direction_selected(): + mapper = _FakeMapper(None) + state = State(yaw_rad=0.3, speed_mps=1.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0, + speed_gain=1.0, yaw_rate_gain=2.0, + max_yaw_rate_rps=1.0) + + controller.update(state, 0.1) + + assert controller.get_target_angle_rad() == pytest.approx(0.3) + assert controller.get_target_yaw_rate_rps() == pytest.approx(0.0) + assert controller.get_target_steer_rad() == pytest.approx(0.0) + assert controller.get_target_accel_mps2() == pytest.approx(2.0) + + +def test_accelerates_toward_cruise_speed_when_below_target(): + mapper = _FakeMapper(0.0) + state = State(yaw_rad=0.0, speed_mps=0.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0, speed_gain=1.0) + + controller.update(state, 0.1) + + assert controller.get_target_accel_mps2() == pytest.approx(3.0) + + +def test_decelerates_toward_cruise_speed_when_above_target(): + mapper = _FakeMapper(0.0) + state = State(yaw_rad=0.0, speed_mps=5.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0, speed_gain=1.0) + + controller.update(state, 0.1) + + assert controller.get_target_accel_mps2() == pytest.approx(-2.0) + + +def test_yaw_rate_proportional_to_heading_error(): + mapper = _FakeMapper(0.5) + state = State(yaw_rad=0.0, speed_mps=2.0) + controller = VfhController(_spec(), mapper, yaw_rate_gain=2.0, max_yaw_rate_rps=10.0) + + controller.update(state, 0.1) + + assert controller.get_target_yaw_rate_rps() == pytest.approx(1.0) + + +def test_yaw_rate_saturates_at_configured_maximum(): + mapper = _FakeMapper(2.0) + state = State(yaw_rad=0.0, speed_mps=2.0) + controller = VfhController(_spec(), mapper, yaw_rate_gain=5.0, max_yaw_rate_rps=1.0) + + controller.update(state, 0.1) + assert controller.get_target_yaw_rate_rps() == pytest.approx(1.0) + + mapper.set_selected_angle_rad(-2.0) + controller.update(state, 0.1) + assert controller.get_target_yaw_rate_rps() == pytest.approx(-1.0) + + +def test_steer_angle_derived_from_yaw_rate_and_speed(): + mapper = _FakeMapper(1.0) + state = State(yaw_rad=0.0, speed_mps=4.0) + controller = VfhController(_spec(), mapper, yaw_rate_gain=1.0, max_yaw_rate_rps=10.0) + + controller.update(state, 0.1) + + expected_yaw_rate_rps = 1.0 # gain(1.0) * diff_angle(1.0), unsaturated + expected_steer_rad = atan2(_spec().wheel_base_m * expected_yaw_rate_rps, 4.0) + assert controller.get_target_yaw_rate_rps() == pytest.approx(expected_yaw_rate_rps) + assert controller.get_target_steer_rad() == pytest.approx(expected_steer_rad) + + +def test_steer_is_zero_when_speed_is_near_zero(): + mapper = _FakeMapper(1.0) + state = State(yaw_rad=0.0, speed_mps=0.0) + controller = VfhController(_spec(), mapper, yaw_rate_gain=1.0, max_yaw_rate_rps=10.0) + + controller.update(state, 0.1) + + assert controller.get_target_steer_rad() == pytest.approx(0.0) + + +def test_get_target_angle_rad_returns_selected_direction(): + mapper = _FakeMapper(1.2) + state = State(yaw_rad=0.0, speed_mps=1.0) + controller = VfhController(_spec(), mapper) + + controller.update(state, 0.1) + + assert controller.get_target_angle_rad() == pytest.approx(1.2) + + +def test_draw_does_not_raise(): + controller = VfhController(_spec(), _FakeMapper(0.0)) + elems = [] + + controller.draw(None, elems) + + assert elems == [] + + +def test_invalid_min_speed_raises(): + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), cruise_speed_mps=3.0, min_speed_mps=-1.0) + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), cruise_speed_mps=3.0, min_speed_mps=4.0) + + +def test_invalid_danger_density_raises(): + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), danger_density=0.0) + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), danger_density=-1.0) + + +def test_invalid_caution_half_angle_raises(): + with pytest.raises(ValueError): + VfhController(_spec(), _FakeMapper(0.0), caution_half_angle_rad=-0.1) + + +def test_target_speed_is_cruise_speed_with_no_obstacle_ahead(): + mapper = _FakeMapper(0.0, density_ahead=0.0) + state = State(yaw_rad=0.0, speed_mps=3.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0) + + controller.update(state, 0.1) + + assert controller.get_target_speed_mps() == pytest.approx(3.0) + + +def test_target_speed_drops_to_minimum_at_danger_density(): + mapper = _FakeMapper(0.0, density_ahead=1.0) + state = State(yaw_rad=0.0, speed_mps=3.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0, + min_speed_mps=0.5, danger_density=1.0) + + controller.update(state, 0.1) + + assert controller.get_target_speed_mps() == pytest.approx(0.5) + + +def test_target_speed_scales_linearly_between_cruise_and_minimum(): + mapper = _FakeMapper(0.0, density_ahead=0.5) + state = State(yaw_rad=0.0, speed_mps=3.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0, + min_speed_mps=1.0, danger_density=1.0) + + controller.update(state, 0.1) + + # halfway to danger_density -> halfway from cruise(3.0) down to minimum(1.0) + assert controller.get_target_speed_mps() == pytest.approx(2.0) + + +def test_target_speed_does_not_go_below_minimum_past_danger_density(): + mapper = _FakeMapper(0.0, density_ahead=5.0) # far past danger_density + state = State(yaw_rad=0.0, speed_mps=3.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0, + min_speed_mps=0.5, danger_density=1.0) + + controller.update(state, 0.1) + + assert controller.get_target_speed_mps() == pytest.approx(0.5) + + +def test_acceleration_targets_the_scaled_down_speed(): + mapper = _FakeMapper(0.0, density_ahead=1.0) + state = State(yaw_rad=0.0, speed_mps=0.0) + controller = VfhController(_spec(), mapper, cruise_speed_mps=3.0, speed_gain=1.0, + min_speed_mps=0.5, danger_density=1.0) + + controller.update(state, 0.1) + + # target speed is scaled down to min_speed_mps(0.5), not cruise_speed_mps(3.0) + assert controller.get_target_accel_mps2() == pytest.approx(0.5) + + +def test_queries_density_relative_to_vehicle_heading_in_target_direction(): + mapper = _FakeMapper(pi / 2, density_ahead=0.0) # target direction, global frame + state = State(yaw_rad=pi / 4, speed_mps=1.0) # vehicle heading, global frame + controller = VfhController(_spec(), mapper, caution_half_angle_rad=0.2) + + controller.update(state, 0.1) + + # target direction relative to heading = pi/2 - pi/4 = pi/4 + assert mapper.histogram.last_center_angle_rad == pytest.approx(pi / 4) + assert mapper.histogram.last_half_width_rad == pytest.approx(0.2) diff --git a/test/test_vfh_direction_selection.py b/test/test_vfh_direction_selection.py new file mode 100644 index 0000000..2371a0f --- /dev/null +++ b/test/test_vfh_direction_selection.py @@ -0,0 +1,17 @@ +""" +Test of VFH direction selection simulation + +Author: Khushi +""" + +from pathlib import Path +import sys + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/mapping/vfh_direction_selection") +import vfh_direction_selection + + +def test_simulation(): + vfh_direction_selection.show_plot = False + + vfh_direction_selection.main() diff --git a/test/test_vfh_dynamic_speed_control.py b/test/test_vfh_dynamic_speed_control.py new file mode 100644 index 0000000..130fcc2 --- /dev/null +++ b/test/test_vfh_dynamic_speed_control.py @@ -0,0 +1,17 @@ +""" +Test of VFH dynamic speed control simulation + +Author: Khushi +""" + +from pathlib import Path +import sys + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/mapping/vfh_dynamic_speed_control") +import vfh_dynamic_speed_control + + +def test_simulation(): + vfh_dynamic_speed_control.show_plot = False + + vfh_dynamic_speed_control.main() diff --git a/test/test_vfh_performance_benchmarking.py b/test/test_vfh_performance_benchmarking.py new file mode 100644 index 0000000..565d96c --- /dev/null +++ b/test/test_vfh_performance_benchmarking.py @@ -0,0 +1,17 @@ +""" +Test of VFH performance benchmarking simulation + +Author: Khushi +""" + +from pathlib import Path +import sys + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/mapping/vfh_performance_benchmarking") +import vfh_performance_benchmarking + + +def test_simulation(): + vfh_performance_benchmarking.show_plot = False + + vfh_performance_benchmarking.main() diff --git a/test/test_vfh_trajectory_verification.py b/test/test_vfh_trajectory_verification.py new file mode 100644 index 0000000..2b7ff80 --- /dev/null +++ b/test/test_vfh_trajectory_verification.py @@ -0,0 +1,17 @@ +""" +Test of VFH trajectory verification simulation + +Author: Khushi +""" + +from pathlib import Path +import sys + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/mapping/vfh_trajectory_verification") +import vfh_trajectory_verification + + +def test_simulation(): + vfh_trajectory_verification.show_plot = False + + vfh_trajectory_verification.main() diff --git a/test/test_vfh_vehicle_motion_integration.py b/test/test_vfh_vehicle_motion_integration.py new file mode 100644 index 0000000..c1f62e4 --- /dev/null +++ b/test/test_vfh_vehicle_motion_integration.py @@ -0,0 +1,17 @@ +""" +Test of VFH vehicle motion integration simulation + +Author: Khushi +""" + +from pathlib import Path +import sys + +sys.path.append(str(Path(__file__).absolute().parent) + "/../src/simulations/mapping/vfh_vehicle_motion_integration") +import vfh_vehicle_motion_integration + + +def test_simulation(): + vfh_vehicle_motion_integration.show_plot = False + + vfh_vehicle_motion_integration.main()