Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 17 additions & 1 deletion src/components/array/xy_array.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
13 changes: 5 additions & 8 deletions src/components/vehicle/body.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
13 changes: 5 additions & 8 deletions src/components/vehicle/chassis.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
31 changes: 22 additions & 9 deletions src/components/vehicle/four_wheels_vehicle.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down
15 changes: 6 additions & 9 deletions src/components/vehicle/front_axle.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
13 changes: 5 additions & 8 deletions src/components/vehicle/front_left_tire.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
15 changes: 6 additions & 9 deletions src/components/vehicle/front_right_tire.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
15 changes: 6 additions & 9 deletions src/components/vehicle/rear_axle.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
15 changes: 6 additions & 9 deletions src/components/vehicle/rear_left_tire.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
15 changes: 6 additions & 9 deletions src/components/vehicle/rear_right_tire.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
"""

import numpy as np
import numpy.typing as npt
import sys
from pathlib import Path

Expand All @@ -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])
18 changes: 2 additions & 16 deletions test/test_rear_axle.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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)
Loading