1- from dataclasses import dataclass
2- from ...utils import settings ,serialization
3- from ...state import VehicleState , ObjectPose , ObjectFrameEnum
4- from ...knowledge .vehicle .geometry import front2steer ,steer2front ,heading_rate
5- from ...knowledge .vehicle .dynamics import pedal_positions_to_acceleration , acceleration_to_pedal_positions
6- from typing import List ,Optional ,Callable
7-
8- @dataclass
9- @serialization .register
10- class GEMVehicleReading :
11- """All items that the vehicle reports directly from its internal sensors."""
12- speed : float = 0 # in m/s
13- gear : int = 0 # 0 neutral, -1 reverse, -2 park, > 0 forward
14- accelerator_pedal_position : float = 0 # in range [0,1]
15- brake_pedal_position : float = 0 # in range [0,1]
16- steering_wheel_angle : float = 0 # in radians
17- left_turn_signal : bool = False
18- right_turn_signal : bool = False
19- headlights_on : bool = False
20- horn_on : bool = False
21- wiper_level : int = 0
22- battery_level : Optional [float ] = None # in range [0,1]
23- fuel_level : Optional [float ] = None # in liters
24- driving_range : Optional [float ] = None # remaining range left, in km
25-
26- def from_state (self , state : VehicleState ) -> None :
27- """Sets the readings that would be approximately sensed at the given
28- VehicleState.
29-
30- Does not change the battery_level, fuel_level, or driving_range values.
31- """
32- self .speed = state .v
33- self .steering_wheel_angle = state .steering_wheel_angle
34- pitch = state .pose .pitch if state .pose .pitch is not None else 0.0
35-
36- #acc_pos,brake_pos,gear = acceleration_to_pedal_positions(state.acceleration, state.v, pitch, state.gear)
37- self .accelerator_pedal_position = state .accelerator_pedal_position
38- self .brake_pedal_position = state .brake_pedal_position
39- self .gear = state .gear
40- self .left_turn_signal = state .left_turn_indicator
41- self .right_turn_signal = state .right_turn_indicator
42- self .horn_on = state .horn_on
43- self .wiper_level = state .wiper_level
44- self .headlights_on = state .headlights_on
45-
46- def to_state (self , pose : ObjectPose = None ) -> VehicleState :
47- """Returns a VehicleState representing the vehicle's current state given
48- these readings.
49-
50- Note: the acceleration attribute is totally bogus, and should be ignored
51- until the dynamics are calibrated better.
52- """
53- if pose is None :
54- pose = ObjectPose (frame = ObjectFrameEnum .CURRENT ,t = 0 ,x = 0 ,y = 0 ,yaw = 0 )
55- pitch = pose .pitch if pose .pitch is not None else 0.0
56- wheel_base = settings .get ('vehicle.geometry.wheelbase' )
57- front_wheel_angle = steer2front (self .steering_wheel_angle )
58- turn_rate = heading_rate (front_wheel_angle ,self .speed ,wheel_base )
59- acc = pedal_positions_to_acceleration (self .accelerator_pedal_position , self .brake_pedal_position , self .speed , pitch , self .gear )
60- return VehicleState (pose ,v = self .speed ,accelerator_pedal_position = self .accelerator_pedal_position ,brake_pedal_position = self .brake_pedal_position ,
61- acceleration = acc ,gear = self .gear ,steering_wheel_angle = self .steering_wheel_angle ,
62- front_wheel_angle = front_wheel_angle ,heading_rate = turn_rate ,
63- left_turn_indicator = self .left_turn_signal ,right_turn_indicator = self .right_turn_signal ,
64- horn_on = self .horn_on ,wiper_level = self .wiper_level ,headlights_on = self .headlights_on )
65-
66-
67- @dataclass
68- @serialization .register
69- class GEMVehicleCommand :
70- """All items that can be directly commanded to the vehicle's actuators."""
71- gear : int #follows convention in state.vehicle.VehicleState. -2: park, -1 reverse: 0: neutral, 1..n: forward
72- accelerator_pedal_position : float
73- accelerator_pedal_speed : float
74- brake_pedal_position : float
75- brake_pedal_speed : float
76- steering_wheel_angle : float
77- steering_wheel_speed : float
78- left_turn_signal : bool = False
79- right_turn_signal : bool = False
80- headlights_on : bool = False
81- horn_on : bool = False
82- wiper_level : int = 0
83-
84-
85- @dataclass
86- class GNSSReading :
87- pose : ObjectPose
88- speed : float
89- status : str
90-
91-
92- class GEMInterface :
93- """Base class for simulated / physical GEM vehicle.
94- """
95- def __init__ (self ):
96- self .last_command = None # type: GEMVehicleCommand
97- self .last_reading = None # type: GEMVehicleReading
98-
99- def start (self ):
100- pass
101-
102- def stop (self ):
103- pass
104-
105- def time (self ) -> float :
106- """Returns the current time"""
107- raise NotImplementedError ()
108-
109- def get_reading (self ) -> GEMVehicleReading :
110- """Returns current read state of the vehicle"""
111- raise NotImplementedError ()
112-
113- def send_command (self , cmd : GEMVehicleCommand ):
114- """Sends a command to the vehicle"""
115- raise NotImplementedError ()
116-
117- def sensors (self ) -> List [str ]:
118- """Returns all available sensors"""
119- return ['gnss' ,'imu' ,'top_lidar' ,'front_camera' ,'front_depth' ,'front_radar' ]
120-
121- def subscribe_sensor (self , name : str , callback : Callable , type = None ) -> None :
122- """Subscribes to a sensor with a given callback.
123-
124- If type is not None, it should be the expected type of the message produced
125- by the sensor callback.
126- """
127- raise NotImplementedError ()
128-
129- def hardware_faults (self ) -> List [str ]:
130- """Returns a list of hardware faults, naming the failed component.
131-
132- Can be any sensor, actuator, or other component.
133- """
134- raise NotImplementedError ()
135-
136- def simple_command (self , acceleration_mps2 : float , steering_wheel_angle : float , state : VehicleState = None ) -> GEMVehicleCommand :
137- """"
138- Returns a command according to a desired acceleration and steering angle
139-
140- Args:
141- acceleration_mps2: acceleration in m/s^2
142- steering_wheel_angle: steering angle in radians
143- state: current vehicle state
144- """
145- pitch = state .pose .pitch if state is not None and state .pose .pitch is not None else 0.0
146- v = state .v if state is not None else 0.0
147- gear = state .gear if state is not None else 1
148- acc_pos ,brake_pos ,gear = acceleration_to_pedal_positions (acceleration_mps2 , v , pitch , gear )
149-
150- cmd = GEMVehicleCommand (gear = gear ,
151- accelerator_pedal_position = acc_pos ,
152- brake_pedal_position = brake_pos ,
153- steering_wheel_angle = steering_wheel_angle ,
154- accelerator_pedal_speed = settings .get ('vehicle.control_defaults.accelerator_pedal_speed' ),
155- brake_pedal_speed = settings .get ('vehicle.control_defaults.brake_pedal_speed' ),
156- steering_wheel_speed = settings .get ('vehicle.control_defaults.steering_wheel_speed' ))
157- if state is not None :
158- #preserve indicators
159- cmd .left_turn_signal = state .left_turn_indicator
160- cmd .right_turn_signal = state .right_turn_indicator
161- cmd .headlights_on = state .headlights_on
162- cmd .horn_on = state .horn_on
163- cmd .wiper_level = state .wiper_level
164-
165- return cmd
166-
167- def command_from_reading (self , reading : GEMVehicleReading = None ) -> GEMVehicleCommand :
168- """Returns a command that maintains all the current elements in the
169- provided vehicle reading. If reading=None, then the last reading
170- is used.
171- """
172- if reading is None :
173- reading = self .last_reading
174- if reading is None :
175- raise RuntimeError ("Can't get command from reading, no reading available" )
176- return GEMVehicleCommand (gear = reading .gear ,
177- accelerator_pedal_position = reading .accelerator_pedal_position ,
178- brake_pedal_position = reading .brake_pedal_position ,
179- steering_wheel_angle = reading .steering_wheel_angle ,
180- accelerator_pedal_speed = settings .get ('vehicle.control_defaults.accelerator_pedal_speed' ),
181- brake_pedal_speed = settings .get ('vehicle.control_defaults.brake_pedal_speed' ),
182- steering_wheel_speed = settings .get ('vehicle.control_defaults.steering_wheel_speed' ),
183- left_turn_signal = reading .left_turn_signal ,
184- right_turn_signal = reading .right_turn_signal ,
185- headlights_on = reading .headlights_on ,
186- horn_on = reading .horn_on ,
1+ from dataclasses import dataclass
2+ from ...utils import settings ,serialization
3+ from ...state import VehicleState , ObjectPose , ObjectFrameEnum
4+ from ...knowledge .vehicle .geometry import front2steer ,steer2front ,heading_rate
5+ from ...knowledge .vehicle .dynamics import pedal_positions_to_acceleration , acceleration_to_pedal_positions
6+ from typing import List ,Optional ,Callable
7+
8+ @dataclass
9+ @serialization .register
10+ class GEMVehicleReading :
11+ """All items that the vehicle reports directly from its internal sensors."""
12+ speed : float = 0 # in m/s
13+ gear : int = 0 # 0 neutral, -1 reverse, -2 park, > 0 forward
14+ accelerator_pedal_position : float = 0 # in range [0,1]
15+ brake_pedal_position : float = 0 # in range [0,1]
16+ steering_wheel_angle : float = 0 # in radians
17+ left_turn_signal : bool = False
18+ right_turn_signal : bool = False
19+ headlights_on : bool = False
20+ horn_on : bool = False
21+ wiper_level : int = 0
22+ battery_level : Optional [float ] = None # in range [0,1]
23+ fuel_level : Optional [float ] = None # in liters
24+ driving_range : Optional [float ] = None # remaining range left, in km
25+
26+ def from_state (self , state : VehicleState ) -> None :
27+ """Sets the readings that would be approximately sensed at the given
28+ VehicleState.
29+
30+ Does not change the battery_level, fuel_level, or driving_range values.
31+ """
32+ self .speed = state .v
33+ self .steering_wheel_angle = state .steering_wheel_angle
34+ pitch = state .pose .pitch if state .pose .pitch is not None else 0.0
35+
36+ #acc_pos,brake_pos,gear = acceleration_to_pedal_positions(state.acceleration, state.v, pitch, state.gear)
37+ self .accelerator_pedal_position = state .accelerator_pedal_position
38+ self .brake_pedal_position = state .brake_pedal_position
39+ self .gear = state .gear
40+ self .left_turn_signal = state .left_turn_indicator
41+ self .right_turn_signal = state .right_turn_indicator
42+ self .horn_on = state .horn_on
43+ self .wiper_level = state .wiper_level
44+ self .headlights_on = state .headlights_on
45+
46+ def to_state (self , pose : ObjectPose = None ) -> VehicleState :
47+ """Returns a VehicleState representing the vehicle's current state given
48+ these readings.
49+
50+ Note: the acceleration attribute is totally bogus, and should be ignored
51+ until the dynamics are calibrated better.
52+ """
53+ if pose is None :
54+ pose = ObjectPose (frame = ObjectFrameEnum .CURRENT ,t = 0 ,x = 0 ,y = 0 ,yaw = 0 )
55+ pitch = pose .pitch if pose .pitch is not None else 0.0
56+ wheel_base = settings .get ('vehicle.geometry.wheelbase' )
57+ front_wheel_angle = steer2front (self .steering_wheel_angle )
58+ turn_rate = heading_rate (front_wheel_angle ,self .speed ,wheel_base )
59+ acc = pedal_positions_to_acceleration (self .accelerator_pedal_position , self .brake_pedal_position , self .speed , pitch , self .gear )
60+ return VehicleState (pose ,v = self .speed ,accelerator_pedal_position = self .accelerator_pedal_position ,brake_pedal_position = self .brake_pedal_position ,
61+ acceleration = acc ,gear = self .gear ,steering_wheel_angle = self .steering_wheel_angle ,
62+ front_wheel_angle = front_wheel_angle ,heading_rate = turn_rate ,
63+ left_turn_indicator = self .left_turn_signal ,right_turn_indicator = self .right_turn_signal ,
64+ horn_on = self .horn_on ,wiper_level = self .wiper_level ,headlights_on = self .headlights_on )
65+
66+
67+ @dataclass
68+ @serialization .register
69+ class GEMVehicleCommand :
70+ """All items that can be directly commanded to the vehicle's actuators."""
71+ gear : int #follows convention in state.vehicle.VehicleState. -2: park, -1 reverse: 0: neutral, 1..n: forward
72+ accelerator_pedal_position : float
73+ accelerator_pedal_speed : float
74+ brake_pedal_position : float
75+ brake_pedal_speed : float
76+ steering_wheel_angle : float
77+ steering_wheel_speed : float
78+ left_turn_signal : bool = False
79+ right_turn_signal : bool = False
80+ headlights_on : bool = False
81+ horn_on : bool = False
82+ wiper_level : int = 0
83+
84+
85+ @dataclass
86+ class GNSSReading :
87+ pose : ObjectPose
88+ speed : float
89+ status : str
90+
91+
92+ class GEMInterface :
93+ """Base class for simulated / physical GEM vehicle.
94+ """
95+ def __init__ (self ):
96+ self .last_command = None # type: GEMVehicleCommand
97+ self .last_reading = None # type: GEMVehicleReading
98+
99+ def start (self ):
100+ pass
101+
102+ def stop (self ):
103+ pass
104+
105+ def time (self ) -> float :
106+ """Returns the current time"""
107+ raise NotImplementedError ()
108+
109+ def get_reading (self ) -> GEMVehicleReading :
110+ """Returns current read state of the vehicle"""
111+ raise NotImplementedError ()
112+
113+ def send_command (self , cmd : GEMVehicleCommand ):
114+ """Sends a command to the vehicle"""
115+ raise NotImplementedError ()
116+
117+ def sensors (self ) -> List [str ]:
118+ """Returns all available sensors"""
119+ return ['gnss' ,'imu' ,'top_lidar' ,'front_camera' ,'front_depth' ,'front_radar' ]
120+
121+ def subscribe_sensor (self , name : str , callback : Callable , type = None ) -> None :
122+ """Subscribes to a sensor with a given callback.
123+
124+ If type is not None, it should be the expected type of the message produced
125+ by the sensor callback.
126+ """
127+ raise NotImplementedError ()
128+
129+ def hardware_faults (self ) -> List [str ]:
130+ """Returns a list of hardware faults, naming the failed component.
131+
132+ Can be any sensor, actuator, or other component.
133+ """
134+ raise NotImplementedError ()
135+
136+ def simple_command (self , acceleration_mps2 : float , steering_wheel_angle : float , state : VehicleState = None ) -> GEMVehicleCommand :
137+ """"
138+ Returns a command according to a desired acceleration and steering angle
139+
140+ Args:
141+ acceleration_mps2: acceleration in m/s^2
142+ steering_wheel_angle: steering angle in radians
143+ state: current vehicle state
144+ """
145+ pitch = state .pose .pitch if state is not None and state .pose .pitch is not None else 0.0
146+ v = state .v if state is not None else 0.0
147+ gear = state .gear if state is not None else 1
148+ acc_pos ,brake_pos ,gear = acceleration_to_pedal_positions (acceleration_mps2 , v , pitch , gear )
149+
150+ cmd = GEMVehicleCommand (gear = gear ,
151+ accelerator_pedal_position = acc_pos ,
152+ brake_pedal_position = brake_pos ,
153+ steering_wheel_angle = steering_wheel_angle ,
154+ accelerator_pedal_speed = settings .get ('vehicle.control_defaults.accelerator_pedal_speed' ),
155+ brake_pedal_speed = settings .get ('vehicle.control_defaults.brake_pedal_speed' ),
156+ steering_wheel_speed = settings .get ('vehicle.control_defaults.steering_wheel_speed' ))
157+ if state is not None :
158+ #preserve indicators
159+ cmd .left_turn_signal = state .left_turn_indicator
160+ cmd .right_turn_signal = state .right_turn_indicator
161+ cmd .headlights_on = state .headlights_on
162+ cmd .horn_on = state .horn_on
163+ cmd .wiper_level = state .wiper_level
164+
165+ return cmd
166+
167+ def command_from_reading (self , reading : GEMVehicleReading = None ) -> GEMVehicleCommand :
168+ """Returns a command that maintains all the current elements in the
169+ provided vehicle reading. If reading=None, then the last reading
170+ is used.
171+ """
172+ if reading is None :
173+ reading = self .last_reading
174+ if reading is None :
175+ raise RuntimeError ("Can't get command from reading, no reading available" )
176+ return GEMVehicleCommand (gear = reading .gear ,
177+ accelerator_pedal_position = reading .accelerator_pedal_position ,
178+ brake_pedal_position = reading .brake_pedal_position ,
179+ steering_wheel_angle = reading .steering_wheel_angle ,
180+ accelerator_pedal_speed = settings .get ('vehicle.control_defaults.accelerator_pedal_speed' ),
181+ brake_pedal_speed = settings .get ('vehicle.control_defaults.brake_pedal_speed' ),
182+ steering_wheel_speed = settings .get ('vehicle.control_defaults.steering_wheel_speed' ),
183+ left_turn_signal = reading .left_turn_signal ,
184+ right_turn_signal = reading .right_turn_signal ,
185+ headlights_on = reading .headlights_on ,
186+ horn_on = reading .horn_on ,
187187 wiper_level = reading .wiper_level )
0 commit comments