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
60 changes: 60 additions & 0 deletions src/components/vehicle/VehiclePart.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
"""
VehiclePart.py

Author: Aikaterini Katsampoula
"""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../array")
from xy_array import XYArray


class VehiclePart:
"""
One drawable part of a vehicle.
"""

def __init__(self, spec, outline_xy, offset_x_m, offset_y_m, steerable=False):
"""
Constructor
spec: object of VehicleSpecification class
outline_xy: part outline as a 2 by n array of x y coordinates
offset_x_m: part's x offset from the vehicle origin[m]
offset_y_m: part's y offset from the vehicle origin[m]
steerable: whether the part rotates with the steering angle
"""

self.spec = spec
self.offset_x_m = offset_x_m
self.offset_y_m = offset_y_m
self.steerable = steerable
self.array = XYArray(np.asarray(outline_xy))

def draw(self, axes, pose, steer_rad, elems=None):
"""
Plot the part's outline.
axes: Axes object of figure
pose: Vehicle's pose vector
steer_rad: steering angle[rad]
elems: List of plot objects
"""
if elems is None:
elems = steer_rad
steer_rad = 0.0

part_steer_rad = steer_rad if self.steerable else 0.0
translated_array = self.array.homogeneous_transformation(
self.offset_x_m, self.offset_y_m, part_steer_rad
)
transformed_array = translated_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0])
part_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(part_plot)
46 changes: 9 additions & 37 deletions src/components/vehicle/body.py
Original file line number Diff line number Diff line change
@@ -1,43 +1,15 @@
"""
body.py

Author: Shisato Yano
"""
"""Compatibility for the vehicle body."""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../array")
from xy_array import XYArray


class Body:
"""
Vehicle Body class
"""

def __init__(self, spec):
"""
Constructor
spec: object of VehicleSpecification class
"""

self.spec = spec
from VehiclePart import VehiclePart

contour = np.array([[self.spec.f_edge_m, -self.spec.r_edge_m, -self.spec.r_edge_m, self.spec.f_edge_m, self.spec.f_edge_m],
[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):
"""
Function to plot vehicle's body lines
axes: Axes object of figure
pose: Vehicle's pose vector
elems: List of plot objects
"""
def Body(spec):
"""Create the vehicle body as a VehiclePart."""

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)
outline = np.array([
[spec.f_edge_m, -spec.r_edge_m, -spec.r_edge_m, spec.f_edge_m, spec.f_edge_m],
[spec.width_m, spec.width_m, -spec.width_m, -spec.width_m, spec.width_m],
])
return VehiclePart(spec, outline, 0.0, 0.0)
43 changes: 6 additions & 37 deletions src/components/vehicle/chassis.py
Original file line number Diff line number Diff line change
@@ -1,43 +1,12 @@
"""
chassis.py

Author: Shisato Yano
"""
"""Compatibility for the vehicle chassis."""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../array")
from xy_array import XYArray


class Chassis:
"""
Vehicle Chassis class
"""
from VehiclePart import VehiclePart

def __init__(self, spec):
"""
Constructor
spec: object of VehicleSpecification class
"""

self.spec = spec

contour = np.array([[self.spec.f_len_m, -self.spec.r_len_m],
[0.0, 0.0]])
self.array = XYArray(contour)

def draw(self, axes, pose, elems):
"""
Function to plot vehicle's chassis lines
axes: Axes object of figure
pose: Vehicle's pose vector
elems: List of plot objects
"""
def Chassis(spec):
"""Create the vehicle chassis as a VehiclePart."""

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)
outline = np.array([[spec.f_len_m, -spec.r_len_m], [0.0, 0.0]])
return VehiclePart(spec, outline, 0.0, 0.0)
94 changes: 70 additions & 24 deletions src/components/vehicle/four_wheels_vehicle.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,9 @@
Author: Shisato Yano
"""

from body import Body
from chassis import Chassis
from front_left_tire import FrontLeftTire
from front_right_tire import FrontRightTire
from rear_left_tire import RearLeftTire
from rear_right_tire import RearRightTire
from front_axle import FrontAxle
from rear_axle import RearAxle
import numpy as np

from VehiclePart import VehiclePart


class FourWheelsVehicle:
Expand All @@ -36,14 +31,7 @@ def __init__(self, state, spec, controller=None, sensors=None,
self.state = state

self.spec = spec
self.body = Body(spec)
self.chassis = Chassis(spec)
self.front_left_tire = FrontLeftTire(spec)
self.front_right_tire = FrontRightTire(spec)
self.rear_left_tire = RearLeftTire(spec)
self.rear_right_tire = RearRightTire(spec)
self.front_axle = FrontAxle(spec)
self.rear_axle = RearAxle(spec)
self._create_vehicle_parts()

self.controller = controller
self.sensors = sensors
Expand All @@ -54,6 +42,70 @@ def __init__(self, state, spec, controller=None, sensors=None,

self.show_zoom = show_zoom

def _create_vehicle_parts(self):
"""Create the vehicle outline, frame, axles, and tires."""

tire_outline = np.array([
[self.spec.tire_r_m, -self.spec.tire_r_m, -self.spec.tire_r_m,
self.spec.tire_r_m, self.spec.tire_r_m],
[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],
])
axle_outline = np.array([
[0.0, 0.0],
[self.spec.axle_half_m, -self.spec.axle_half_m],
])

self.body = VehiclePart(
self.spec,
np.array([
[self.spec.f_edge_m, -self.spec.r_edge_m, -self.spec.r_edge_m,
self.spec.f_edge_m, self.spec.f_edge_m],
[self.spec.width_m, self.spec.width_m, -self.spec.width_m,
-self.spec.width_m, self.spec.width_m],
]),
0.0,
0.0,
)
self.chassis = VehiclePart(
self.spec,
np.array([
[self.spec.f_len_m, -self.spec.r_len_m],
[0.0, 0.0],
]),
0.0,
0.0,
)
self.front_left_tire = VehiclePart(
self.spec, tire_outline, self.spec.f_len_m, self.spec.axle_half_m, True
)
self.front_right_tire = VehiclePart(
self.spec, tire_outline, self.spec.f_len_m, -self.spec.axle_half_m, True
)
self.rear_left_tire = VehiclePart(
self.spec, tire_outline, -self.spec.r_len_m, self.spec.axle_half_m
)
self.rear_right_tire = VehiclePart(
self.spec, tire_outline, -self.spec.r_len_m, -self.spec.axle_half_m
)
self.front_axle = VehiclePart(
self.spec, axle_outline, self.spec.f_len_m, 0.0
)
self.rear_axle = VehiclePart(
self.spec, axle_outline, -self.spec.r_len_m, 0.0
)

self.parts = (
self.body,
self.chassis,
self.front_left_tire,
self.front_right_tire,
self.rear_left_tire,
self.rear_right_tire,
self.front_axle,
self.rear_axle,
)

def _install_sensors(self, state):
"""
Private function to calculate each sensor's installation position on vehicle
Expand Down Expand Up @@ -213,14 +265,8 @@ def draw(self, axes, elems):
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)
for part in self.parts:
part.draw(axes, x_y_yaw_array, steer_rad, elems)

if self.show_zoom:
axes.set_xlim(x_m - self.spec.area_size, x_m + self.spec.area_size)
Expand Down
46 changes: 6 additions & 40 deletions src/components/vehicle/front_axle.py
Original file line number Diff line number Diff line change
@@ -1,46 +1,12 @@
"""
front_axle.py

Author: Shisato Yano
"""
"""Compatibility for the front axle."""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../array")
from xy_array import XYArray


class FrontAxle:
"""
Vehicle Front Axle class
"""
from VehiclePart import VehiclePart

def __init__(self, spec):
"""
Constructor
spec: object of VehicleSpecification class
"""

self.spec = spec
self.offset_x_m = spec.f_len_m
self.offset_y_m = spec.axle_half_m
def FrontAxle(spec):
"""Create the front axle as a VehiclePart."""

contour = np.array([[0.0, 0.0],
[self.offset_y_m, -self.offset_y_m]])
self.array = XYArray(contour)

def draw(self, axes, pose, elems):
"""
Function to plot vehicle's front axle's lines
axes: Axes object of figure
pose: Vehicle's pose vector
elems: List of plot objects
"""

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)
outline = np.array([[0.0, 0.0], [spec.axle_half_m, -spec.axle_half_m]])
return VehiclePart(spec, outline, spec.f_len_m, 0.0)
50 changes: 9 additions & 41 deletions src/components/vehicle/front_left_tire.py
Original file line number Diff line number Diff line change
@@ -1,47 +1,15 @@
"""
front_left_tire.py

Author: Shisato Yano
"""
"""Compatibility for the front-left tire."""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../array")
from xy_array import XYArray


class FrontLeftTire:
"""
Vehicle Front Left Tire class
"""
from VehiclePart import VehiclePart

def __init__(self, spec):
"""
Constructor
spec: object of VehicleSpecification class
"""

self.spec = spec
self.offset_x_m = spec.f_len_m
self.offset_y_m = spec.axle_half_m

contour = np.array([[self.spec.tire_r_m, -self.spec.tire_r_m, -self.spec.tire_r_m, self.spec.tire_r_m, self.spec.tire_r_m],
[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):
"""
Function to plot vehicle's front left tire's lines
axes: Axes object of figure
pose: Vehicle's pose vector
steer_rad: Tire's steering angle[rad]
elems: List of plot objects
"""
def FrontLeftTire(spec):
"""Create the front left tire as a steerable VehiclePart."""

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)
outline = np.array([
[spec.tire_r_m, -spec.tire_r_m, -spec.tire_r_m, spec.tire_r_m, spec.tire_r_m],
[spec.tire_w_m, spec.tire_w_m, -spec.tire_w_m, -spec.tire_w_m, spec.tire_w_m],
])
return VehiclePart(spec, outline, spec.f_len_m, spec.axle_half_m, True)
Loading
Loading