diff --git a/src/components/vehicle/VehiclePart.py b/src/components/vehicle/VehiclePart.py new file mode 100644 index 00000000..abc0a331 --- /dev/null +++ b/src/components/vehicle/VehiclePart.py @@ -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) diff --git a/src/components/vehicle/body.py b/src/components/vehicle/body.py index e48ad22e..69bf6888 100644 --- a/src/components/vehicle/body.py +++ b/src/components/vehicle/body.py @@ -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) diff --git a/src/components/vehicle/chassis.py b/src/components/vehicle/chassis.py index 87edb59c..7c5fe427 100644 --- a/src/components/vehicle/chassis.py +++ b/src/components/vehicle/chassis.py @@ -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) diff --git a/src/components/vehicle/four_wheels_vehicle.py b/src/components/vehicle/four_wheels_vehicle.py index 109da489..8384b547 100644 --- a/src/components/vehicle/four_wheels_vehicle.py +++ b/src/components/vehicle/four_wheels_vehicle.py @@ -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: @@ -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 @@ -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 @@ -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) diff --git a/src/components/vehicle/front_axle.py b/src/components/vehicle/front_axle.py index a6cf8c0e..7fcdfe5f 100644 --- a/src/components/vehicle/front_axle.py +++ b/src/components/vehicle/front_axle.py @@ -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) diff --git a/src/components/vehicle/front_left_tire.py b/src/components/vehicle/front_left_tire.py index e03f702f..4e80c210 100644 --- a/src/components/vehicle/front_left_tire.py +++ b/src/components/vehicle/front_left_tire.py @@ -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) diff --git a/src/components/vehicle/front_right_tire.py b/src/components/vehicle/front_right_tire.py index 2612fd93..3cd2f590 100644 --- a/src/components/vehicle/front_right_tire.py +++ b/src/components/vehicle/front_right_tire.py @@ -1,47 +1,11 @@ -""" -front_right_tire.py +"""Compatibility for the front-right tire.""" -Author: Shisato Yano -""" +from front_left_tire import FrontLeftTire -import numpy as np -import sys -from pathlib import Path -sys.path.append(str(Path(__file__).absolute().parent) + "/../array") -from xy_array import XYArray +def FrontRightTire(spec): + """Create the front right tire as a steerable VehiclePart.""" - -class FrontRightTire: - """ - Vehicle Front Right Tire class - """ - - 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 right 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 - """ - - 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) + tire = FrontLeftTire(spec) + tire.offset_y_m = -spec.axle_half_m + return tire diff --git a/src/components/vehicle/rear_axle.py b/src/components/vehicle/rear_axle.py index f4e3ffba..24c43fda 100644 --- a/src/components/vehicle/rear_axle.py +++ b/src/components/vehicle/rear_axle.py @@ -1,46 +1,12 @@ -""" -rear_axle.py - -Author: Shisato Yano -""" +"""Compatibility for the rear 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 RearAxle: - """ - Vehicle Rear Axle class - """ +from VehiclePart import VehiclePart - def __init__(self, spec): - """ - Constructor - spec: object of VehicleSpecification class - """ - self.spec = spec - self.offset_x_m = -spec.r_len_m - self.offset_y_m = spec.axle_half_m +def RearAxle(spec): + """Create the rear 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 rear 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.r_len_m, 0.0) diff --git a/src/components/vehicle/rear_left_tire.py b/src/components/vehicle/rear_left_tire.py index 3cbbb61c..0f814a1b 100644 --- a/src/components/vehicle/rear_left_tire.py +++ b/src/components/vehicle/rear_left_tire.py @@ -1,46 +1,12 @@ -""" -rear_left_tire.py +"""Compatibility for the rear-left tire.""" -Author: Shisato Yano -""" +from front_left_tire import FrontLeftTire -import numpy as np -import sys -from pathlib import Path -sys.path.append(str(Path(__file__).absolute().parent) + "/../array") -from xy_array import XYArray +def RearLeftTire(spec): + """Create the rear left tire as a non-steerable VehiclePart.""" - -class RearLeftTire: - """ - Vehicle Rear Left Tire class - """ - - def __init__(self, spec): - """ - Constructor - spec: object of VehicleSpecification class - """ - - self.spec = spec - self.offset_x_m = -spec.r_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, elems): - """ - Function to plot vehicle's rear left tire'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, 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) + tire = FrontLeftTire(spec) + tire.offset_x_m = -spec.r_len_m + tire.steerable = False + return tire diff --git a/src/components/vehicle/rear_right_tire.py b/src/components/vehicle/rear_right_tire.py index f163e150..23c41a49 100644 --- a/src/components/vehicle/rear_right_tire.py +++ b/src/components/vehicle/rear_right_tire.py @@ -1,46 +1,11 @@ -""" -rear_right_tire.py +"""Compatibility for the rear-right tire.""" -Author: Shisato Yano -""" +from rear_left_tire import RearLeftTire -import numpy as np -import sys -from pathlib import Path -sys.path.append(str(Path(__file__).absolute().parent) + "/../array") -from xy_array import XYArray +def RearRightTire(spec): + """Create the rear right tire as a non-steerable VehiclePart.""" - -class RearRightTire: - """ - Vehicle Rear Right Tire class - """ - - def __init__(self, spec): - """ - Constructor - spec: object of VehicleSpecification class - """ - - self.spec = spec - self.offset_x_m = -spec.r_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, elems): - """ - Function to plot vehicle's rear right tire'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, 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) + tire = RearLeftTire(spec) + tire.offset_y_m = -spec.axle_half_m + return tire