Go to the documentation of this file.
18 #ifndef VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_
19 #define VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_
22 #include <autoware_auto_vehicle_msgs/msg/wipers_command.hpp>
23 #include <autoware_auto_vehicle_msgs/msg/headlights_command.hpp>
24 #include <autoware_auto_vehicle_msgs/msg/horn_command.hpp>
25 #include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
26 #include <autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp>
27 #include <autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp>
28 #include <autoware_auto_vehicle_msgs/msg/vehicle_state_report.hpp>
29 #include <vehicle_interface/visibility_control.hpp>
31 #include <experimental/optional>
42 namespace vehicle_interface
44 using autoware_auto_vehicle_msgs::msg::HeadlightsCommand;
45 using autoware_auto_vehicle_msgs::msg::HornCommand;
46 using autoware_auto_vehicle_msgs::msg::WipersCommand;
48 std::experimental::optional<autoware_auto_vehicle_msgs::msg::VehicleStateCommand>;
50 using Odometry = autoware_auto_vehicle_msgs::msg::VehicleOdometry;
51 using StateReport = autoware_auto_vehicle_msgs::msg::VehicleStateReport;
60 : m_control{control}, m_state{state} {}
97 : m_min{min}, m_max{max}, m_threshold{threshold}
100 throw std::domain_error{
"min >= max"};
104 T
min() const noexcept {
return m_min;}
105 T
max() const noexcept {
return m_max;}
110 const auto value_raw = value;
111 value = std::min(std::max(m_min, value_raw), m_max);
112 return std::abs(value - value_raw) >= m_threshold;
126 using AccelT = decltype(BasicControlCommand::long_accel_mps2);
150 const VelocityT gear_shift_velocity_threshold,
153 const std::chrono::nanoseconds time_step,
154 const AccelT timeout_accel_mps2,
155 const std::chrono::nanoseconds state_transition_timeout,
156 const AccelT auto_gear_shift_accel_deadzone_mps2
159 VelocityT gear_shift_velocity_threshold() const noexcept;
162 std::chrono::nanoseconds time_step() const noexcept;
163 AccelT timeout_acceleration() const noexcept;
164 std::chrono::nanoseconds state_transition_timeout() const noexcept;
165 AccelT auto_gear_shift_accel_deadzone() const noexcept;
168 VelocityT m_gear_shift_velocity_threshold;
171 std::chrono::nanoseconds m_time_step;
172 AccelT m_timeout_acceleration;
173 std::chrono::nanoseconds m_state_transition_timeout;
174 AccelT m_auto_gear_shift_accel_deadzone;
183 using Reports = std::vector<StateMachineReport>;
197 Command timeout_commands() const noexcept;
199 const
Reports & reports() const noexcept;
204 using VSC = autoware_auto_vehicle_msgs::msg::VehicleStateCommand;
205 using MaybeEnum = std::experimental::optional<decltype(VSC::blinker)>;
211 std::chrono::system_clock::time_point stamp{std::chrono::system_clock::time_point::min()};
213 struct StateChangeRequests
215 ValueStamp<decltype(VSC::gear)> gear;
216 ValueStamp<decltype(VSC::blinker)> blinker;
217 ValueStamp<decltype(WipersCommand::command)> wiper;
218 ValueStamp<decltype(HeadlightsCommand::command)> headlight;
219 ValueStamp<decltype(VSC::mode)> mode;
220 ValueStamp<decltype(VSC::hand_brake)> hand_brake;
221 ValueStamp<decltype(HornCommand::active)> horn;
231 const VSC & state)
const;
241 StateMachineConfig m_config;
244 StateChangeRequests m_requests{};
245 mutable Reports m_reports{};
251 #endif // VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_
@ STATE_TRANSITION_TIMEOUT
Commanded state transition didn't happen.
T threshold() const noexcept
Definition: safety_state_machine.hpp:106
autoware_auto_vehicle_msgs::msg::VehicleControlCommand VehicleControlCommand
Definition: pure_pursuit.hpp:41
This file includes common type definition.
StateMachineReport
Definition: safety_state_machine.hpp:74
T clamp(T val, T min, T max)
Standard clamp implementation.
Definition: motion_common.hpp:131
std::experimental::optional< autoware_auto_vehicle_msgs::msg::VehicleStateCommand > MaybeStateCommand
Definition: safety_state_machine.hpp:48
Command(const BasicControlCommand &control=BasicControlCommand{}, const MaybeStateCommand &state=MaybeStateCommand{})
Constructor.
Definition: safety_state_machine.hpp:57
T min() const noexcept
Definition: safety_state_machine.hpp:104
Configuration class for SafetyStateMachine.
Definition: safety_state_machine.hpp:122
@ BAD_STATE
A state command was not one of the specified constants.
@ CLAMP_PAST_THRESHOLD
Clamped a command that was way out of bounds.
autoware_auto_vehicle_msgs::msg::VehicleStateCommand VSC
Definition: lgsvl_interface.hpp:88
const BasicControlCommand & control() const noexcept
Getter.
Definition: safety_state_machine.hpp:63
bool bool8_t
Definition: types.hpp:39
Definition: safety_state_machine.hpp:180
bool8_t clamp_warn(T &value) const noexcept
Clamps value to max/min range; return true if value is threshold past limits.
Definition: safety_state_machine.hpp:108
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
#define VEHICLE_INTERFACE_LOCAL
Definition: drivers/vehicle_interface/include/vehicle_interface/visibility_control.hpp:45
Definition: safety_state_machine.hpp:91
Limits(T min, T max, T threshold)
Definition: safety_state_machine.hpp:96
const MaybeStateCommand & state() const noexcept
Getter.
Definition: safety_state_machine.hpp:65
@ WIPERS_ON_HEADLIGHTS_ON
Turn on headlights because you turned on wipers.
@ HIGH_FREQUENCY_STEER_REPORT
Reported steering has high frequency components.
@ HIGH_FREQUENCY_VELOCITY_REPORT
Reported velocity has high frequency components.
@ REMOVE_GEAR_COMMAND
Converted gear command to GEAR_NO_COMMAND because you're going too fast.
decltype(BasicControlCommand::long_accel_mps2) AccelT
Definition: safety_state_machine.hpp:126
autoware_auto_vehicle_msgs::msg::VehicleOdometry Odometry
Definition: safety_state_machine.hpp:50
@ HIGH_FREQUENCY_ACCELERATION_COMMAND
Acceleration has high frequency components.
autoware_auto_vehicle_msgs::msg::VehicleControlCommand BasicControlCommand
Definition: safety_state_machine.hpp:49
@ HIGH_FREQUENCY_STEER_COMMAND
Steering has high frequency components.
decltype(Odometry::velocity_mps) VelocityT
Definition: safety_state_machine.hpp:125
std::vector< StateMachineReport > Reports
Definition: safety_state_machine.hpp:183
Simple wrapper for control command and state command together.
Definition: safety_state_machine.hpp:53
T max() const noexcept
Definition: safety_state_machine.hpp:105
#define VEHICLE_INTERFACE_PUBLIC
Definition: drivers/vehicle_interface/include/vehicle_interface/visibility_control.hpp:44
autoware_auto_vehicle_msgs::msg::VehicleStateReport StateReport
Definition: safety_state_machine.hpp:51