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                 :            : /// \file
      17                 :            : /// \brief Input validation state machine
      18                 :            : #ifndef VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_
      19                 :            : #define VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_
      20                 :            : 
      21                 :            : #include <common/types.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>
      30                 :            : 
      31                 :            : #include <experimental/optional>
      32                 :            : #include <algorithm>
      33                 :            : #include <chrono>
      34                 :            : #include <vector>
      35                 :            : 
      36                 :            : using autoware::common::types::bool8_t;
      37                 :            : 
      38                 :            : namespace autoware
      39                 :            : {
      40                 :            : namespace drivers
      41                 :            : {
      42                 :            : namespace vehicle_interface
      43                 :            : {
      44                 :            : using autoware_auto_vehicle_msgs::msg::HeadlightsCommand;
      45                 :            : using autoware_auto_vehicle_msgs::msg::HornCommand;
      46                 :            : using autoware_auto_vehicle_msgs::msg::WipersCommand;
      47                 :            : using MaybeStateCommand =
      48                 :            :   std::experimental::optional<autoware_auto_vehicle_msgs::msg::VehicleStateCommand>;
      49                 :            : using BasicControlCommand = autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
      50                 :            : using Odometry = autoware_auto_vehicle_msgs::msg::VehicleOdometry;
      51                 :            : using StateReport = autoware_auto_vehicle_msgs::msg::VehicleStateReport;
      52                 :            : /// Simple wrapper for control command and state command together
      53                 :            : class VEHICLE_INTERFACE_PUBLIC Command
      54                 :            : {
      55                 :            : public:
      56                 :            :   /// Constructor
      57                 :            :   Command(
      58                 :            :     const BasicControlCommand & control = BasicControlCommand{},
      59                 :            :     const MaybeStateCommand & state = MaybeStateCommand{})
      60         [ +  + ]:        260 :   : m_control{control}, m_state{state} {}
      61                 :            : 
      62                 :            :   /// Getter
      63                 :        136 :   const BasicControlCommand & control() const noexcept {return m_control;}
      64                 :            :   /// Getter
      65                 :            :   const MaybeStateCommand & state() const noexcept {return m_state;}
      66                 :            : 
      67                 :            : private:
      68                 :            :   BasicControlCommand m_control;
      69                 :            :   MaybeStateCommand m_state;
      70                 :            : };  // class Command
      71                 :            : 
      72                 :            : /// Denotes one of the behaviors that the state machine has done; either a notification that it has
      73                 :            : /// done something (Info), or a warning that something bad has been detected
      74                 :            : enum class StateMachineReport : uint8_t
      75                 :            : {
      76                 :            :   CLAMP_PAST_THRESHOLD,  ///< Clamped a command that was way out of bounds
      77                 :            :   BAD_STATE,  ///< A state command was not one of the specified constants
      78                 :            :   WIPERS_ON_HEADLIGHTS_ON,  ///< Turn on headlights because you turned on wipers
      79                 :            :   REMOVE_GEAR_COMMAND,  ///< Converted gear command to GEAR_NO_COMMAND because you're going too fast
      80                 :            :   HIGH_FREQUENCY_ACCELERATION_COMMAND,  ///< Acceleration has high frequency components
      81                 :            :   HIGH_FREQUENCY_STEER_COMMAND,  ///< Steering has high frequency components
      82                 :            :   HIGH_FREQUENCY_VELOCITY_REPORT,  ///< Reported velocity has high frequency components
      83                 :            :   HIGH_FREQUENCY_STEER_REPORT,  ///< Reported steering has high frequency components
      84                 :            :   STATE_TRANSITION_TIMEOUT  ///< Commanded state transition didn't happen
      85                 :            : };  // enum class StateMachineWarning
      86                 :            : 
      87                 :            : // TODO(c.ho) concept comparable
      88                 :            : /// A simple class representing 1D limits and a threshold value for when something is considered
      89                 :            : /// warning worthy.
      90                 :            : template<typename T>
      91                 :            : class VEHICLE_INTERFACE_PUBLIC Limits
      92                 :            : {
      93                 :            : public:
      94                 :            :   /// Constructor
      95                 :            :   /// \throw std::domain_error if min >= max
      96                 :        282 :   Limits(T min, T max, T threshold)
      97                 :        282 :   : m_min{min}, m_max{max}, m_threshold{threshold}
      98                 :            :   {
      99         [ -  + ]:        282 :     if (min >= max) {
     100         [ #  # ]:          0 :       throw std::domain_error{"min >= max"};
     101                 :            :     }
     102                 :        282 :   }
     103                 :            : 
     104                 :            :   T min() const noexcept {return m_min;}
     105                 :            :   T max() const noexcept {return m_max;}
     106                 :            :   T threshold() const noexcept {return m_threshold;}
     107                 :            :   ///  Clamps value to max/min range; return true if value is threshold past limits
     108                 :            :   bool8_t clamp_warn(T & value) const noexcept
     109                 :            :   {
     110                 :        277 :     const auto value_raw = value;
     111   [ +  +  +  +  :        277 :     value = std::min(std::max(m_min, value_raw), m_max);
                   +  + ]
     112   [ +  +  +  + ]:        272 :     return std::abs(value - value_raw) >= m_threshold;
     113                 :            :   }
     114                 :            : 
     115                 :            : private:
     116                 :            :   T m_min;
     117                 :            :   T m_max;
     118                 :            :   T m_threshold;
     119                 :            : };
     120                 :            : 
     121                 :            : /// Configuration class for SafetyStateMachine
     122                 :            : class VEHICLE_INTERFACE_PUBLIC StateMachineConfig
     123                 :            : {
     124                 :            : public:
     125                 :            :   using VelocityT = decltype(Odometry::velocity_mps);
     126                 :            :   using AccelT = decltype(BasicControlCommand::long_accel_mps2);
     127                 :            :   using AccelLimits = Limits<AccelT>;
     128                 :            :   using FrontWheelLimits = Limits<decltype(BasicControlCommand::front_wheel_angle_rad)>;
     129                 :            : 
     130                 :            :   /// Constructor
     131                 :            :   /// \param[in] gear_shift_velocity_threshold Gear shifts between directions (i.e. park->drive,
     132                 :            :   /// low->reverse are allowed only if the magnitude of velocity is below this threshold. Must be
     133                 :            :   /// positive
     134                 :            :   /// \param[in] accel_limits Max/min value for command acceleration
     135                 :            :   /// \param[in] front_wheel_limits Max/min value for commanded wheel angles
     136                 :            :   /// \param[in] time_step Characteristic time step of the system: used for computing timeout
     137                 :            :   /// acceleration commands and when to do autonomous gear shifting. Must be postiive
     138                 :            :   /// \param[in] timeout_accel_mps2 Magnitude of acceleration to use to bring the vehicle to a stop
     139                 :            :   /// when the vehicle interface has timed out. Must be positive
     140                 :            :   /// \param[in] state_transition_timeout If a state component has not changed within this
     141                 :            :   /// period, then a warning is raised. Must be positive and bigger than time_step.
     142                 :            :   /// \param[in] auto_gear_shift_accel_deadzone_mps2 Acceleration must be bigger than this magnitude
     143                 :            :   /// to induce an automatic gear shift. Must be positive.
     144                 :            :   /// \throw std::domain_error If gear_shift_velocity_threshold is negative
     145                 :            :   /// \throw std::domain_error If time_step is non-positive
     146                 :            :   /// \throw std::domain_error If timeout_accel_mps2 is non-positive, or outside of accel_limits
     147                 :            :   /// \throw std::domain_error If state_transition_timeout is smaller than time_step
     148                 :            :   /// \throw std::domain_error If auto_gear_shift_accel_deadzone_mps2 is non-positive
     149                 :            :   StateMachineConfig(
     150                 :            :     const VelocityT gear_shift_velocity_threshold,
     151                 :            :     const AccelLimits & accel_limits,
     152                 :            :     const FrontWheelLimits & front_wheel_limits,
     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
     157                 :            :   );
     158                 :            : 
     159                 :            :   VelocityT gear_shift_velocity_threshold() const noexcept;
     160                 :            :   const AccelLimits & accel_limits() const noexcept;
     161                 :            :   const FrontWheelLimits & front_wheel_limits() 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;
     166                 :            : 
     167                 :            : private:
     168                 :            :   VelocityT m_gear_shift_velocity_threshold;
     169                 :            :   AccelLimits m_accel_limits;
     170                 :            :   FrontWheelLimits m_front_wheel_limits;
     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;
     175                 :            : };  // class StateMachineConfig
     176                 :            : 
     177                 :            : /// Implements stateful behavior for VehicleInterfaceNode
     178                 :            : /// Primary function is to ensure vehicle platform does not get
     179                 :            : /// conflicting commands
     180                 :            : class VEHICLE_INTERFACE_PUBLIC SafetyStateMachine
     181                 :            : {
     182                 :            : public:
     183                 :            :   using Reports = std::vector<StateMachineReport>;  // TODO(c.ho) maybe a set instead
     184                 :            :   /// Constructor
     185                 :            :   explicit SafetyStateMachine(const StateMachineConfig & config);
     186                 :            :   /// Ensure given commands are consistent and not dangerous. See design document for
     187                 :            :   /// full list of behaviors.
     188                 :            :   /// \param[in] command The raw input commands
     189                 :            :   /// \return Commands which are consistent and safe (within the scope of the vehicle interface)
     190                 :            :   Command compute_safe_commands(const Command & command);
     191                 :            :   /// Update the state of the state machine
     192                 :            :   /// \param[in] odom The odometry of the vehicle
     193                 :            :   /// \param[in] state The state of the vehicle
     194                 :            :   void update(const Odometry & odom, const StateReport & state);
     195                 :            :   /// Compute safety fallback commands that should be executed on a timeout
     196                 :            :   /// \return The safety fallback command for the current state
     197                 :            :   Command timeout_commands() const noexcept;
     198                 :            :   /// Get list of warnings state machine saw and adjusted
     199                 :            :   const Reports & reports() const noexcept;
     200                 :            :   /// Get config object
     201                 :            :   const StateMachineConfig & get_config() const noexcept;
     202                 :            : 
     203                 :            : private:
     204                 :            :   using VSC = autoware_auto_vehicle_msgs::msg::VehicleStateCommand;
     205                 :            :   using MaybeEnum = std::experimental::optional<decltype(VSC::blinker)>;
     206                 :            :   //lint -save -e9150 NOLINT Pure aggregate and only used internally; no constraints on state
     207                 :            :   template<typename T>
     208                 :            :   struct ValueStamp
     209                 :            :   {
     210                 :            :     T value;
     211                 :            :     std::chrono::system_clock::time_point stamp{std::chrono::system_clock::time_point::min()};
     212                 :            :   };
     213                 :            :   struct StateChangeRequests
     214                 :            :   {
     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;
     222                 :            :   };  // struct StateChangeRequest
     223                 :            :   //lint -restore NOLINT
     224                 :            :   // Make sure uint members are within range--otherwise set to NO_COMMAND
     225                 :            :   VEHICLE_INTERFACE_LOCAL VSC sanitize(const VSC & msg) const;
     226                 :            :   // Turn on headlights if the wipers are being turned on
     227                 :            :   VEHICLE_INTERFACE_LOCAL static MaybeEnum headlights_on_if_wipers_on(const VSC & in);
     228                 :            :   // Apply gear shift if velocity is small and commanded acceleration is big enough
     229                 :            :   VEHICLE_INTERFACE_LOCAL uint8_t automatic_gear_shift(
     230                 :            :     const BasicControlCommand control,
     231                 :            :     const VSC & state) const;
     232                 :            :   // Remove "big" gear shifts if velocity is too large in magnitude
     233                 :            :   VEHICLE_INTERFACE_LOCAL bool8_t bad_gear_shift(const VSC & in) const;
     234                 :            :   // Clamp control values; warn if wildly out of range
     235                 :            :   VEHICLE_INTERFACE_LOCAL BasicControlCommand clamp(BasicControlCommand in) const;
     236                 :            :   // Add/overwrite any new state change requests
     237                 :            :   VEHICLE_INTERFACE_LOCAL void cache_state_change_request(const VSC & in);
     238                 :            :   // Clear any state changes that have happened, or warn on timeout
     239                 :            :   VEHICLE_INTERFACE_LOCAL void check_state_change(const StateReport & in);
     240                 :            : 
     241                 :            :   StateMachineConfig m_config;
     242                 :            :   Odometry m_odometry{};
     243                 :            :   StateReport m_state{};
     244                 :            :   StateChangeRequests m_requests{};
     245                 :            :   mutable Reports m_reports{};
     246                 :            : };  // class SafetyStateMachine
     247                 :            : }  // namespace vehicle_interface
     248                 :            : }  // namespace drivers
     249                 :            : }  // namespace autoware
     250                 :            : 
     251                 :            : #endif  // VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_