Branch data Line data Source code
1 : : // Copyright 2020 the Autoware Foundation
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : : #include <common/types.hpp>
17 : : #include <helper_functions/float_comparisons.hpp>
18 : :
19 : : #include <time_utils/time_utils.hpp>
20 : :
21 : : #include <cmath>
22 : : #include <limits>
23 : :
24 : : #include "vehicle_interface/safety_state_machine.hpp"
25 : :
26 : : #include "autoware_auto_vehicle_msgs/msg/headlights_command.hpp"
27 : : #include "autoware_auto_vehicle_msgs/msg/headlights_report.hpp"
28 : : #include "autoware_auto_vehicle_msgs/msg/horn_command.hpp"
29 : : #include "autoware_auto_vehicle_msgs/msg/horn_report.hpp"
30 : : #include "autoware_auto_vehicle_msgs/msg/wipers_command.hpp"
31 : : #include "autoware_auto_vehicle_msgs/msg/wipers_report.hpp"
32 : :
33 : : using autoware::common::types::bool8_t;
34 : : namespace comp = autoware::common::helper_functions::comparisons;
35 : :
36 : : namespace autoware
37 : : {
38 : : namespace drivers
39 : : {
40 : : namespace vehicle_interface
41 : : {
42 : : namespace
43 : : {
44 : : using VSC = autoware_auto_vehicle_msgs::msg::VehicleStateCommand;
45 : : using VSR = StateReport;
46 : : using autoware_auto_vehicle_msgs::msg::HeadlightsCommand;
47 : : using autoware_auto_vehicle_msgs::msg::HeadlightsReport;
48 : : using autoware_auto_vehicle_msgs::msg::HornCommand;
49 : : using autoware_auto_vehicle_msgs::msg::HornReport;
50 : : using autoware_auto_vehicle_msgs::msg::WipersCommand;
51 : : using autoware_auto_vehicle_msgs::msg::WipersReport;
52 : : static_assert(VSC::BLINKER_OFF == VSR::BLINKER_OFF, "BLINKER_OFF!=");
53 : : static_assert(VSC::BLINKER_LEFT == VSR::BLINKER_LEFT, "BLINKER_LEFT !=");
54 : : static_assert(VSC::BLINKER_RIGHT == VSR::BLINKER_RIGHT, "BLINKER_RIGHT !=");
55 : : static_assert(VSC::BLINKER_HAZARD == VSR::BLINKER_HAZARD, "BLINKER_HAZARD !=");
56 : : static_assert(HeadlightsCommand::DISABLE == VSR::HEADLIGHT_OFF, "HEADLIGHT_OFF !=");
57 : : static_assert(HeadlightsCommand::ENABLE_LOW == VSR::HEADLIGHT_ON, "HEADLIGHT_ON !=");
58 : : static_assert(HeadlightsCommand::ENABLE_HIGH == VSR::HEADLIGHT_HIGH, "HEADLIGHT_HIGH !=");
59 : : static_assert(WipersCommand::DISABLE == WipersReport::DISABLE, "DISABLE !=");
60 : : static_assert(WipersCommand::ENABLE_LOW == WipersReport::ENABLE_LOW, "ENABLE_LOW !=");
61 : : static_assert(WipersCommand::ENABLE_HIGH == WipersReport::ENABLE_HIGH, "ENABLE_HIGH !=");
62 : : static_assert(WipersCommand::ENABLE_CLEAN == WipersReport::ENABLE_CLEAN, "ENABLE_CLEAN !=");
63 : : static_assert(VSC::GEAR_DRIVE == VSR::GEAR_DRIVE, "GEAR_DRIVE !=");
64 : : static_assert(VSC::GEAR_REVERSE == VSR::GEAR_REVERSE, "GEAR_REVERSE !=");
65 : : static_assert(VSC::GEAR_PARK == VSR::GEAR_PARK, "GEAR_PARK !=");
66 : : static_assert(VSC::GEAR_LOW == VSR::GEAR_LOW, "GEAR_LOW !=");
67 : : static_assert(VSC::GEAR_NEUTRAL == VSR::GEAR_NEUTRAL, "GEAR_NEUTRAL !=");
68 : : static_assert(VSC::MODE_AUTONOMOUS == VSR::MODE_AUTONOMOUS, "MODE_AUTONOMOUS !=");
69 : : static_assert(VSC::MODE_MANUAL == VSR::MODE_MANUAL, "MODE_MANUAL !=");
70 : : } // namespace
71 : :
72 : : using VCC = autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
73 : :
74 : : ////////////////////////////////////////////////////////////////////////////////
75 : 141 : StateMachineConfig::StateMachineConfig(
76 : : const VelocityT gear_shift_velocity_threshold,
77 : : const AccelLimits & accel_limits,
78 : : const FrontWheelLimits & front_wheel_limits,
79 : : const std::chrono::nanoseconds time_step,
80 : : const AccelT timeout_accel_mps2,
81 : : const std::chrono::nanoseconds state_transition_timeout,
82 : 141 : const AccelT auto_gear_shift_accel_deadzone_mps2)
83 : : : m_gear_shift_velocity_threshold{gear_shift_velocity_threshold},
84 : : m_accel_limits{accel_limits},
85 : : m_front_wheel_limits{front_wheel_limits},
86 : : m_time_step{time_step},
87 : : m_timeout_acceleration{timeout_accel_mps2},
88 : : m_state_transition_timeout{state_transition_timeout},
89 : 141 : m_auto_gear_shift_accel_deadzone{auto_gear_shift_accel_deadzone_mps2}
90 : : {
91 [ - + ]: 141 : if (VelocityT{} >= gear_shift_velocity_threshold) {
92 [ # # ]: 0 : throw std::domain_error{"Gear shift velocity threshold must be positive"};
93 : : }
94 [ - + ]: 141 : if (decltype(time_step)::zero() >= time_step) {
95 [ # # ]: 0 : throw std::domain_error{"Time step must be positive"};
96 : : }
97 [ - + ]: 141 : if (AccelT{} >= timeout_accel_mps2) {
98 [ # # ]: 0 : throw std::domain_error{"Timeout acceleration must be > 0"};
99 : : }
100 [ + - - + ]: 141 : if ((timeout_accel_mps2 < accel_limits.min()) || (timeout_accel_mps2 > accel_limits.max())) {
101 [ # # ]: 0 : throw std::domain_error{"Timeout acceleration outside of acceleration limits"};
102 : : }
103 [ - + ]: 141 : if (state_transition_timeout < time_step) {
104 [ # # ]: 0 : throw std::domain_error{"State transition timeout must be > time_step"};
105 : : }
106 [ - + ]: 141 : if (AccelT{} >= auto_gear_shift_accel_deadzone_mps2) {
107 [ # # ]: 0 : throw std::domain_error{"Gear shift acceleration deadzone must be > 0"};
108 : : }
109 : 141 : }
110 : :
111 : 119 : StateMachineConfig::VelocityT StateMachineConfig::gear_shift_velocity_threshold() const noexcept
112 : : {
113 : 119 : return m_gear_shift_velocity_threshold;
114 : : }
115 : :
116 : 141 : const StateMachineConfig::AccelLimits & StateMachineConfig::accel_limits() const noexcept
117 : : {
118 : 141 : return m_accel_limits;
119 : : }
120 : :
121 : 136 : const StateMachineConfig::FrontWheelLimits & StateMachineConfig::front_wheel_limits() const noexcept
122 : : {
123 : 136 : return m_front_wheel_limits;
124 : : }
125 : :
126 : 47 : std::chrono::nanoseconds StateMachineConfig::time_step() const noexcept {return m_time_step;}
127 : :
128 : 5 : StateMachineConfig::AccelT StateMachineConfig::timeout_acceleration() const noexcept
129 : : {
130 : 5 : return m_timeout_acceleration;
131 : : }
132 : :
133 : 1289 : std::chrono::nanoseconds StateMachineConfig::state_transition_timeout() const noexcept
134 : : {
135 : 1289 : return m_state_transition_timeout;
136 : : }
137 : :
138 : 149 : StateMachineConfig::AccelT StateMachineConfig::auto_gear_shift_accel_deadzone() const noexcept
139 : : {
140 : 149 : return m_auto_gear_shift_accel_deadzone;
141 : : }
142 : :
143 : : ////////////////////////////////////////////////////////////////////////////////
144 : : ////////////////////////////////////////////////////////////////////////////////
145 : 139 : SafetyStateMachine::SafetyStateMachine(const StateMachineConfig & config)
146 [ + - ]: 139 : : m_config{config}
147 : : {
148 [ + - ]: 139 : m_reports.reserve(10U); // Number of enums
149 : : { // initialize state to something sensible
150 : 139 : m_state.gear = VSR::GEAR_DRIVE;
151 : 139 : m_state.blinker = VSR::BLINKER_OFF;
152 : 139 : m_state.wiper = VSR::WIPER_OFF;
153 : 139 : m_state.headlight = VSR::HEADLIGHT_OFF;
154 : 139 : m_state.mode = VSR::MODE_NOT_READY;
155 : 139 : m_state.horn = false;
156 : 139 : m_state.hand_brake = false;
157 : : }
158 : 139 : }
159 : :
160 : : ////////////////////////////////////////////////////////////////////////////////
161 : 136 : Command SafetyStateMachine::compute_safe_commands(const Command & command)
162 : : {
163 [ - + ]: 136 : m_reports.clear();
164 : 136 : const auto control = clamp(command.control());
165 : : MaybeStateCommand state{command.state()};
166 [ + + ]: 136 : if (command.state()) {
167 : : // Sanitize state command
168 [ + - ]: 226 : state = sanitize(command.state().value());
169 : : // Apply headlight logic
170 : : const auto new_headlight = headlights_on_if_wipers_on(state.value());
171 [ + + ]: 113 : if (new_headlight) {
172 : 10 : state->headlight = new_headlight.value();
173 : 10 : m_reports.emplace_back(StateMachineReport::WIPERS_ON_HEADLIGHTS_ON);
174 : : }
175 : : // Apply automatic gear shift logic: no command if already in that state
176 : : {
177 : 113 : const auto requested_gear = automatic_gear_shift(control, state.value());
178 [ + + + - ]: 161 : state->gear = (requested_gear == m_state.gear) ? VSC::GEAR_NO_COMMAND : requested_gear;
179 : : }
180 : : // Apply gear check logic
181 [ + + ]: 113 : if (bad_gear_shift(state.value())) {
182 : 7 : state->gear = VSC::GEAR_NO_COMMAND;
183 : 7 : m_reports.emplace_back(StateMachineReport::REMOVE_GEAR_COMMAND);
184 : : }
185 : : // Add requests to queue
186 : 113 : cache_state_change_request(state.value());
187 : : } else {
188 : : // Check if you need to do gear shifting
189 : : //lint -e{1793} NOLINT nonconst member on temp ok: named parameter idiom
190 : 23 : const auto dummy_state = VSC{}.set__gear(m_state.gear);
191 : 23 : const auto requested_gear = automatic_gear_shift(control, dummy_state);
192 [ + + ]: 23 : if (requested_gear != m_state.gear) {
193 : : state = VSC{};
194 : 4 : state->stamp = control.stamp;
195 : 4 : state->gear = requested_gear;
196 : : }
197 : : }
198 : 136 : return Command{control, state};
199 : : }
200 : :
201 : : ////////////////////////////////////////////////////////////////////////////////
202 [ + + ]: 1195 : void SafetyStateMachine::update(const Odometry & odom, const StateReport & state)
203 : : {
204 : : m_reports.clear();
205 : : // TODO(c.ho) sanitize StateReport
206 : 1195 : m_odometry = odom;
207 : 1195 : m_state = state;
208 : : // Check if state update has been satisfied
209 : 1195 : check_state_change(state);
210 : 1195 : }
211 : :
212 : : ////////////////////////////////////////////////////////////////////////////////
213 : 5 : Command SafetyStateMachine::timeout_commands() const noexcept
214 : : {
215 : : //lint -e{1793} NOLINT temporary ok: named parameter idiom
216 : : const auto state = VSC{}.set__blinker(VSC::BLINKER_HAZARD);
217 : : using Accel = decltype(BasicControlCommand::long_accel_mps2);
218 : 10 : auto acceleration = -m_odometry.velocity_mps /
219 : 5 : std::chrono::duration_cast<std::chrono::duration<Accel>>(m_config.time_step()).count();
220 : 5 : (void)m_config.accel_limits().clamp_warn(acceleration);
221 : : //lint -e{1793} NOLINT temporary ok: named parameter idiom
222 : : const auto control = BasicControlCommand{}.set__long_accel_mps2(acceleration);
223 : 5 : return Command{control, state};
224 : : }
225 : :
226 : : ////////////////////////////////////////////////////////////////////////////////
227 : 1165 : const SafetyStateMachine::Reports & SafetyStateMachine::reports() const noexcept
228 : : {
229 : 1165 : return m_reports;
230 : : }
231 : :
232 : : ////////////////////////////////////////////////////////////////////////////////
233 : 0 : const StateMachineConfig & SafetyStateMachine::get_config() const noexcept
234 : : {
235 : 0 : return m_config;
236 : : }
237 : :
238 : : ////////////////////////////////////////////////////////////////////////////////
239 : 0 : SafetyStateMachine::MaybeEnum SafetyStateMachine::headlights_on_if_wipers_on(const VSC & in)
240 : : {
241 : 0 : MaybeEnum ret{};
242 [ - - + + ]: 113 : switch (in.wiper) {
243 : 14 : case WipersCommand::ENABLE_LOW:
244 : : case WipersCommand::ENABLE_HIGH:
245 [ - - + + ]: 14 : switch (in.headlight) {
246 : : case HeadlightsCommand::DISABLE:
247 : : case HeadlightsCommand::NO_COMMAND:
248 : : ret = MaybeEnum{HeadlightsCommand::ENABLE_LOW};
249 : 10 : break;
250 : : case HeadlightsCommand::ENABLE_LOW:
251 : : case HeadlightsCommand::ENABLE_HIGH:
252 : : default: // throw on other cases?
253 : : break;
254 : : }
255 : : break;
256 : : case WipersCommand::DISABLE:
257 : : case WipersCommand::ENABLE_CLEAN:
258 : : case WipersCommand::NO_COMMAND:
259 : : default: // Throw on other cases?
260 : : break;
261 : : }
262 : 0 : return ret;
263 : : }
264 : :
265 : : ////////////////////////////////////////////////////////////////////////////////
266 : 113 : bool8_t SafetyStateMachine::bad_gear_shift(const VSC & in) const
267 : : {
268 : : // Doing nothing -> no check needed
269 [ + + ]: 113 : if (VSC::GEAR_NO_COMMAND == in.gear) {
270 : : return false;
271 : : }
272 : : // Low velocity -> no check needed
273 : : const auto velocity_over_threshold =
274 [ + + ]: 35 : (m_odometry.velocity_mps > m_config.gear_shift_velocity_threshold()) ||
275 [ + + ]: 31 : (m_odometry.velocity_mps < -m_config.gear_shift_velocity_threshold());
276 : : if (!velocity_over_threshold) {
277 : : return false;
278 : : }
279 : : // If start and end start are not both forward -> bad
280 : 7 : const auto going_forward =
281 : 7 : (StateReport::GEAR_DRIVE == m_state.gear) || (StateReport::GEAR_LOW == m_state.gear);
282 : 7 : const auto target_forward = (VSC::GEAR_DRIVE == in.gear) || (VSC::GEAR_LOW == in.gear);
283 [ + + ]: 7 : if (going_forward != target_forward) {
284 : : return true;
285 : : }
286 : : // Otherwise if start and end state (park or reverse) are not the same -> bad
287 : : static_assert(VSC::GEAR_REVERSE == StateReport::GEAR_REVERSE, "VSC::reverse != VSR::reverse");
288 : : static_assert(VSC::GEAR_PARK == StateReport::GEAR_PARK, "VSC::park != VSR::park");
289 : 1 : return m_state.gear != in.gear;
290 : : }
291 : :
292 : : ////////////////////////////////////////////////////////////////////////////////
293 : 136 : uint8_t SafetyStateMachine::automatic_gear_shift(
294 : : const BasicControlCommand control,
295 : : const VSC & state) const
296 : : {
297 : : // If you're specifically asking for a park, I'll respect that and let other logic check
298 [ + + ]: 136 : if (VSC::GEAR_PARK == state.gear) {
299 : : return state.gear;
300 : : }
301 : : // Small acceleration -> no check; let other logic handle
302 : : using Real = decltype(control.long_accel_mps2);
303 [ + + ]: 127 : if (std::fabs(control.long_accel_mps2) < m_config.auto_gear_shift_accel_deadzone()) {
304 : 106 : return state.gear;
305 : : }
306 : : // High velocity -> no check needed: let other logic handle
307 : 21 : const auto velocity = m_odometry.velocity_mps;
308 [ + + ]: 21 : const auto velocity_over_threshold = (velocity > m_config.gear_shift_velocity_threshold()) ||
309 [ + + ]: 18 : (velocity < -m_config.gear_shift_velocity_threshold());
310 : : if (velocity_over_threshold) {
311 : 6 : return state.gear;
312 : : }
313 : : // If velocity and acceleration are in the same direction -> no check; continue if 0 though
314 [ - + ]: 15 : if (Real{} < (control.long_accel_mps2 * velocity)) {
315 : 0 : return state.gear;
316 : : }
317 : : // Check if commanded acceleration will switch your sign
318 : : const auto dv =
319 : : control.long_accel_mps2 *
320 : 15 : std::chrono::duration_cast<std::chrono::duration<Real>>(m_config.time_step()).count();
321 : 15 : const auto v_next = dv + velocity;
322 [ + + ]: 15 : const auto accel_switches_sign = Real{} > (velocity * v_next);
323 : : // Nonzero velocity and acceleration doesn't switch sign -> stay as is
324 : : constexpr auto EPS = std::numeric_limits<Real>::epsilon();
325 [ + + + + ]: 15 : if (!comp::abs_eq_zero(velocity, EPS) && (!accel_switches_sign)) {
326 : 2 : return state.gear;
327 : : }
328 : : // Zero velocity -> any acceleration should be a gear shift
329 : : // Return drive or reverse according to acceleration sign
330 [ + + ]: 13 : return (control.long_accel_mps2 > Real{}) ? VSC::GEAR_DRIVE : VSC::GEAR_REVERSE;
331 : : }
332 : :
333 : : ////////////////////////////////////////////////////////////////////////////////
334 : 136 : BasicControlCommand SafetyStateMachine::clamp(BasicControlCommand in) const
335 : : {
336 : : auto warn = false;
337 [ + + ]: 272 : warn = m_config.accel_limits().clamp_warn(in.long_accel_mps2) || warn;
338 [ + + + + ]: 272 : warn = m_config.front_wheel_limits().clamp_warn(in.front_wheel_angle_rad) || warn;
339 : : if (warn) {
340 : 5 : m_reports.emplace_back(StateMachineReport::CLAMP_PAST_THRESHOLD);
341 : : }
342 : 136 : return in;
343 : : }
344 : :
345 : : ////////////////////////////////////////////////////////////////////////////////
346 : 113 : void SafetyStateMachine::cache_state_change_request(const VSC & in)
347 : : {
348 : 113 : const auto stamp = ::time_utils::from_message(in.stamp);
349 : : // States
350 : : const auto update_request = [stamp](auto no_command, auto & command, auto state, auto & request) {
351 [ + - + - : 111 : if ((no_command != command) && (command != state)) {
+ - + + -
- ]
352 : 109 : request = {command, stamp};
353 : : }
354 : 113 : };
355 [ + + ]: 113 : update_request(WipersCommand::NO_COMMAND, in.wiper, m_state.wiper, m_requests.wiper);
356 [ + + ]: 113 : update_request(VSC::BLINKER_NO_COMMAND, in.blinker, m_state.blinker, m_requests.blinker);
357 [ + + ]: 113 : update_request(VSC::GEAR_NO_COMMAND, in.gear, m_state.gear, m_requests.gear);
358 : : update_request(
359 [ + + ]: 113 : HeadlightsCommand::NO_COMMAND, in.headlight, m_state.headlight,
360 : : m_requests.headlight);
361 [ - + ]: 113 : update_request(VSC::MODE_NO_COMMAND, in.mode, m_state.mode, m_requests.mode);
362 : : // Flags
363 : : const auto update_flag_request = [stamp](auto command, auto state, auto & request) -> void {
364 : 226 : if (command != state) {
365 : 0 : request = {command, stamp};
366 : : }
367 : : };
368 [ - + ]: 113 : update_flag_request(in.horn, m_state.horn, m_requests.horn);
369 [ - + ]: 113 : update_flag_request(in.hand_brake, m_state.hand_brake, m_requests.hand_brake);
370 : 113 : }
371 : :
372 : : ////////////////////////////////////////////////////////////////////////////////
373 : 1195 : void SafetyStateMachine::check_state_change(const StateReport & in)
374 : : {
375 : 1195 : auto timeout = false;
376 : 1195 : const auto stamp = ::time_utils::from_message(in.stamp) - m_config.state_transition_timeout();
377 : : // Check states
378 : : const auto check_state = [&timeout, stamp](auto & request, auto state, auto no_command) -> void {
379 : 5975 : if (no_command == request.value) {
380 : : return;
381 : : }
382 [ - + - + : 56 : if (request.value == state) {
- + - + -
- ]
383 : 0 : request = {no_command, decltype(stamp)::min()};
384 : : }
385 [ + - + - : 56 : if (stamp > request.stamp) {
+ - + - -
- ]
386 : 56 : timeout = true;
387 : : }
388 : : };
389 [ + + ]: 1195 : check_state(m_requests.gear, in.gear, VSC::GEAR_NO_COMMAND);
390 [ + + ]: 1195 : check_state(m_requests.blinker, in.blinker, VSC::BLINKER_NO_COMMAND);
391 [ + + ]: 1195 : check_state(m_requests.wiper, in.wiper, WipersCommand::NO_COMMAND);
392 [ + + ]: 1195 : check_state(m_requests.headlight, in.headlight, HeadlightsCommand::NO_COMMAND);
393 [ - + ]: 1195 : check_state(m_requests.mode, in.mode, VSC::MODE_NO_COMMAND);
394 : : // Check flags
395 : : const auto check_flag = [&timeout, stamp](auto request, auto state) -> void {
396 : 2390 : if (request.value == state) {
397 : : return;
398 : : }
399 [ # # # # ]: 0 : if (stamp > request.stamp) {
400 : 0 : timeout = true;
401 : : }
402 : : };
403 : : //lint -save -e523 NOLINT false positive: lambda affects timeout which is reference captured
404 [ - + ]: 1195 : check_flag(m_requests.horn, in.horn);
405 [ - + ]: 1195 : check_flag(m_requests.hand_brake, in.hand_brake);
406 : : //lint -restore NOLINT
407 : : // Warn on timeout
408 [ + + ]: 1195 : if (timeout) {
409 : 47 : m_reports.emplace_back(StateMachineReport::STATE_TRANSITION_TIMEOUT);
410 : : }
411 : 1195 : }
412 : :
413 : : ////////////////////////////////////////////////////////////////////////////////
414 : 113 : SafetyStateMachine::VSC SafetyStateMachine::sanitize(const VSC & msg) const
415 : : {
416 : : auto did_sanitize = false;
417 : 113 : VSC ret{msg};
418 : : // Headlights
419 [ + + ]: 113 : switch (msg.headlight) {
420 : : case HeadlightsCommand::NO_COMMAND:
421 : : case HeadlightsCommand::DISABLE:
422 : : case HeadlightsCommand::ENABLE_LOW:
423 : : case HeadlightsCommand::ENABLE_HIGH:
424 : : break;
425 : 2 : default:
426 : : did_sanitize = true;
427 : : ret.headlight = HeadlightsCommand::NO_COMMAND;
428 : 2 : break;
429 : : }
430 : : // Blinker
431 [ + + ]: 113 : switch (msg.blinker) {
432 : : case VSC::BLINKER_NO_COMMAND:
433 : : case VSC::BLINKER_OFF:
434 : : case VSC::BLINKER_LEFT:
435 : : case VSC::BLINKER_RIGHT:
436 : : case VSC::BLINKER_HAZARD:
437 : : break;
438 : 2 : default:
439 : : did_sanitize = true;
440 : : ret.blinker = VSC::BLINKER_NO_COMMAND;
441 : 2 : break;
442 : : }
443 : : // Wipers
444 [ + + ]: 113 : switch (msg.wiper) {
445 : : case WipersCommand::ENABLE_LOW:
446 : : case WipersCommand::ENABLE_HIGH:
447 : : case WipersCommand::DISABLE:
448 : : case WipersCommand::ENABLE_CLEAN:
449 : : case WipersCommand::NO_COMMAND:
450 : : break;
451 : 2 : default:
452 : : did_sanitize = true;
453 : : ret.wiper = WipersCommand::NO_COMMAND;
454 : 2 : break;
455 : : }
456 : : // Gear
457 [ + + ]: 113 : switch (msg.gear) {
458 : : case VSC::GEAR_NO_COMMAND:
459 : : case VSC::GEAR_DRIVE:
460 : : case VSC::GEAR_PARK:
461 : : case VSC::GEAR_REVERSE:
462 : : case VSC::GEAR_NEUTRAL:
463 : : case VSC::GEAR_LOW:
464 : : break;
465 : 2 : default:
466 : : did_sanitize = true;
467 : : ret.gear = VSC::GEAR_NO_COMMAND;
468 : 2 : break;
469 : : }
470 : : // Mode
471 [ + + ]: 113 : switch (msg.mode) {
472 : : case VSC::MODE_NO_COMMAND:
473 : : case VSC::MODE_AUTONOMOUS:
474 : : case VSC::MODE_MANUAL:
475 : : break;
476 : : default:
477 : : did_sanitize = true;
478 : : ret.mode = VSC::MODE_NO_COMMAND;
479 : : break;
480 : : }
481 : :
482 [ + + ]: 111 : if (did_sanitize) {
483 : 6 : m_reports.emplace_back(StateMachineReport::BAD_STATE);
484 : : }
485 : :
486 : 113 : return ret;
487 : : }
488 : :
489 : : } // namespace vehicle_interface
490 : : } // namespace drivers
491 : : } // namespace autoware
|
