diff --git a/src/components/array/xy_array.py b/src/components/array/xy_array.py index 0569856a..f0cbc939 100644 --- a/src/components/array/xy_array.py +++ b/src/components/array/xy_array.py @@ -39,7 +39,23 @@ def homogeneous_transformation(self, x, y, angle_rad): translated_data = rotated_data + np.array([[x], [y]]) return XYArray(translated_data) - + + @classmethod + def concatenate(cls, xy_arrays: list["XYArray"]) -> "XYArray": + """ + Function to concatenate multiple array data with NaN separators + NaN breaks the line between each array, so concatenated data can be drawn by a single plot call + xy_arrays: List of XYArray objects + Return concatenated XYArray object + """ + + nan_column = np.full((2, 1), np.nan) + data_list = [] + for xy_array in xy_arrays: + data_list.append(xy_array.get_data()) + data_list.append(nan_column) + return cls(np.hstack(data_list)) + def get_data(self): """ Return array data diff --git a/src/components/vehicle/body.py b/src/components/vehicle/body.py index e48ad22e..11c57c3a 100644 --- a/src/components/vehicle/body.py +++ b/src/components/vehicle/body.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -29,15 +30,11 @@ def __init__(self, spec): [self.spec.width_m, self.spec.width_m, -self.spec.width_m, -self.spec.width_m, self.spec.width_m]]) self.array = XYArray(contour) - def draw(self, axes, pose, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64]) -> XYArray: """ - Function to plot vehicle's body lines - axes: Axes object of figure + Function to get vehicle's body outline transformed by vehicle's pose pose: Vehicle's pose vector - elems: List of plot objects + Return transformed XYArray object """ - transformed_array = self.array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - body_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(body_plot) + return self.array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/src/components/vehicle/chassis.py b/src/components/vehicle/chassis.py index 87edb59c..62ef66ed 100644 --- a/src/components/vehicle/chassis.py +++ b/src/components/vehicle/chassis.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -29,15 +30,11 @@ def __init__(self, spec): [0.0, 0.0]]) self.array = XYArray(contour) - def draw(self, axes, pose, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64]) -> XYArray: """ - Function to plot vehicle's chassis lines - axes: Axes object of figure + Function to get vehicle's chassis outline transformed by vehicle's pose pose: Vehicle's pose vector - elems: List of plot objects + Return transformed XYArray object """ - transformed_array = self.array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - chassis_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(chassis_plot) + return self.array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/src/components/vehicle/four_wheels_vehicle.py b/src/components/vehicle/four_wheels_vehicle.py index 109da489..229a78a0 100644 --- a/src/components/vehicle/four_wheels_vehicle.py +++ b/src/components/vehicle/four_wheels_vehicle.py @@ -4,6 +4,12 @@ Author: Shisato Yano """ +import sys +from pathlib import Path + +sys.path.append(str(Path(__file__).absolute().parent) + "/../array") +from xy_array import XYArray + from body import Body from chassis import Chassis from front_left_tire import FrontLeftTire @@ -212,15 +218,22 @@ def draw(self, axes, elems): x_y_yaw_array = self.state.x_y_yaw() x_m = self.state.get_x_m() y_m = self.state.get_y_m() - - self.body.draw(axes, x_y_yaw_array, elems) - self.chassis.draw(axes, x_y_yaw_array, elems) - self.front_left_tire.draw(axes, x_y_yaw_array, steer_rad, elems) - self.front_right_tire.draw(axes, x_y_yaw_array, steer_rad, elems) - self.rear_left_tire.draw(axes, x_y_yaw_array, elems) - self.rear_right_tire.draw(axes, x_y_yaw_array, elems) - self.front_axle.draw(axes, x_y_yaw_array, elems) - self.rear_axle.draw(axes, x_y_yaw_array, elems) + + # all the part outlines are concatenated with NaN separators and + # drawn by a single plot call, which is faster than one call per part + vehicle_outline = XYArray.concatenate([ + self.body.transformed_outline(x_y_yaw_array), + self.chassis.transformed_outline(x_y_yaw_array), + self.front_left_tire.transformed_outline(x_y_yaw_array, steer_rad), + self.front_right_tire.transformed_outline(x_y_yaw_array, steer_rad), + self.rear_left_tire.transformed_outline(x_y_yaw_array), + self.rear_right_tire.transformed_outline(x_y_yaw_array), + self.front_axle.transformed_outline(x_y_yaw_array), + self.rear_axle.transformed_outline(x_y_yaw_array), + ]) + vehicle_plot, = axes.plot(vehicle_outline.get_x_data(), vehicle_outline.get_y_data(), + lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) + elems.append(vehicle_plot) if self.show_zoom: axes.set_xlim(x_m - self.spec.area_size, x_m + self.spec.area_size) diff --git a/src/components/vehicle/front_axle.py b/src/components/vehicle/front_axle.py index a6cf8c0e..c17d8654 100644 --- a/src/components/vehicle/front_axle.py +++ b/src/components/vehicle/front_axle.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -31,16 +32,12 @@ def __init__(self, spec): [self.offset_y_m, -self.offset_y_m]]) self.array = XYArray(contour) - def draw(self, axes, pose, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64]) -> XYArray: """ - Function to plot vehicle's front axle's lines - axes: Axes object of figure + Function to get vehicle's front axle outline transformed by vehicle's pose pose: Vehicle's pose vector - elems: List of plot objects + Return transformed XYArray object """ - + translated_array = self.array.homogeneous_transformation(self.offset_x_m, 0.0, 0.0) - transformed_array = translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - axle_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(axle_plot) + return translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/src/components/vehicle/front_left_tire.py b/src/components/vehicle/front_left_tire.py index e03f702f..a443e12b 100644 --- a/src/components/vehicle/front_left_tire.py +++ b/src/components/vehicle/front_left_tire.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -31,17 +32,13 @@ def __init__(self, spec): [self.spec.tire_w_m, self.spec.tire_w_m, -self.spec.tire_w_m, -self.spec.tire_w_m, self.spec.tire_w_m]]) self.array = XYArray(contour) - def draw(self, axes, pose, steer_rad, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64], steer_rad: float) -> XYArray: """ - Function to plot vehicle's front left tire's lines - axes: Axes object of figure + Function to get vehicle's front left tire outline transformed by vehicle's pose pose: Vehicle's pose vector steer_rad: Tire's steering angle[rad] - elems: List of plot objects + Return transformed XYArray object """ translated_array = self.array.homogeneous_transformation(self.offset_x_m, self.offset_y_m, steer_rad) - transformed_array = translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - tire_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(tire_plot) + return translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/src/components/vehicle/front_right_tire.py b/src/components/vehicle/front_right_tire.py index 2612fd93..e0ab308b 100644 --- a/src/components/vehicle/front_right_tire.py +++ b/src/components/vehicle/front_right_tire.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -31,17 +32,13 @@ def __init__(self, spec): [self.spec.tire_w_m, self.spec.tire_w_m, -self.spec.tire_w_m, -self.spec.tire_w_m, self.spec.tire_w_m]]) self.array = XYArray(contour) - def draw(self, axes, pose, steer_rad, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64], steer_rad: float) -> XYArray: """ - Function to plot vehicle's front right tire's lines - axes: Axes object of figure + Function to get vehicle's front right tire outline transformed by vehicle's pose pose: Vehicle's pose vector steer_rad: Tire's steering angle[rad] - elems: List of plot objects + Return transformed XYArray object """ - + translated_array = self.array.homogeneous_transformation(self.offset_x_m, self.offset_y_m, steer_rad) - transformed_array = translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - tire_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(tire_plot) + return translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/src/components/vehicle/rear_axle.py b/src/components/vehicle/rear_axle.py index f4e3ffba..1f840b0b 100644 --- a/src/components/vehicle/rear_axle.py +++ b/src/components/vehicle/rear_axle.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -31,16 +32,12 @@ def __init__(self, spec): [self.offset_y_m, -self.offset_y_m]]) self.array = XYArray(contour) - def draw(self, axes, pose, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64]) -> XYArray: """ - Function to plot vehicle's rear axle's lines - axes: Axes object of figure + Function to get vehicle's rear axle outline transformed by vehicle's pose pose: Vehicle's pose vector - elems: List of plot objects + Return transformed XYArray object """ - + translated_array = self.array.homogeneous_transformation(self.offset_x_m, 0.0, 0.0) - transformed_array = translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - axle_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(axle_plot) + return translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/src/components/vehicle/rear_left_tire.py b/src/components/vehicle/rear_left_tire.py index 3cbbb61c..2c00d5f7 100644 --- a/src/components/vehicle/rear_left_tire.py +++ b/src/components/vehicle/rear_left_tire.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -31,16 +32,12 @@ def __init__(self, spec): [self.spec.tire_w_m, self.spec.tire_w_m, -self.spec.tire_w_m, -self.spec.tire_w_m, self.spec.tire_w_m]]) self.array = XYArray(contour) - def draw(self, axes, pose, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64]) -> XYArray: """ - Function to plot vehicle's rear left tire's lines - axes: Axes object of figure + Function to get vehicle's rear left tire outline transformed by vehicle's pose pose: Vehicle's pose vector - elems: List of plot objects + Return transformed XYArray object """ - + translated_array = self.array.homogeneous_transformation(self.offset_x_m, self.offset_y_m, 0.0) - transformed_array = translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - tire_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(tire_plot) + return translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/src/components/vehicle/rear_right_tire.py b/src/components/vehicle/rear_right_tire.py index f163e150..13081f9c 100644 --- a/src/components/vehicle/rear_right_tire.py +++ b/src/components/vehicle/rear_right_tire.py @@ -5,6 +5,7 @@ """ import numpy as np +import numpy.typing as npt import sys from pathlib import Path @@ -31,16 +32,12 @@ def __init__(self, spec): [self.spec.tire_w_m, self.spec.tire_w_m, -self.spec.tire_w_m, -self.spec.tire_w_m, self.spec.tire_w_m]]) self.array = XYArray(contour) - def draw(self, axes, pose, elems): + def transformed_outline(self, pose: npt.NDArray[np.float64]) -> XYArray: """ - Function to plot vehicle's rear right tire's lines - axes: Axes object of figure + Function to get vehicle's rear right tire outline transformed by vehicle's pose pose: Vehicle's pose vector - elems: List of plot objects + Return transformed XYArray object """ - + translated_array = self.array.homogeneous_transformation(self.offset_x_m, self.offset_y_m, 0.0) - transformed_array = translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) - tire_plot, = axes.plot(transformed_array.get_x_data(), transformed_array.get_y_data(), - lw=self.spec.line_w, color=self.spec.color, ls=self.spec.line_type) - elems.append(tire_plot) + return translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0]) diff --git a/test/test_rear_axle.py b/test/test_rear_axle.py index dfc159ae..2b2b2646 100644 --- a/test/test_rear_axle.py +++ b/test/test_rear_axle.py @@ -15,19 +15,6 @@ from rear_right_tire import RearRightTire -class MockAxes: - """ - Axes stand-in that records the x data of each plotted line - """ - - def __init__(self): - self.plotted_x = [] - - def plot(self, x_data, y_data, **kwargs): - self.plotted_x.append(np.array(x_data)) - return (None,) - - # vehicle whose rear axle is behind the origin spec = VehicleSpecification(r_len_m=1.5) @@ -40,6 +27,5 @@ def test_rear_axle_shares_rear_tires_offset(): def test_rear_axle_is_drawn_behind_origin(): - axes = MockAxes() - RearAxle(spec).draw(axes, np.array([[0.0], [0.0], [0.0]]), []) - assert np.allclose(axes.plotted_x[0], -spec.r_len_m) + outline = RearAxle(spec).transformed_outline(np.array([[0.0], [0.0], [0.0]])) + assert np.allclose(outline.get_x_data(), -spec.r_len_m)