safety_state_machine.hpp

Go to the documentation of this file.

// Copyright 2020 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#ifndef VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_
#define VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_

#include <common/types.hpp>
#include <autoware_auto_msgs/msg/vehicle_control_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_odometry.hpp>
#include <autoware_auto_msgs/msg/vehicle_state_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_state_report.hpp>
#include <vehicle_interface/visibility_control.hpp>

#include <experimental/optional>
#include <algorithm>
#include <chrono>
#include <vector>

using autoware::common::types::bool8_t;

namespace autoware
{
namespace drivers
{
namespace vehicle_interface
{
using MaybeStateCommand = std::experimental::optional<autoware_auto_msgs::msg::VehicleStateCommand>;
using BasicControlCommand = autoware_auto_msgs::msg::VehicleControlCommand;
using Odometry = autoware_auto_msgs::msg::VehicleOdometry;
using StateReport = autoware_auto_msgs::msg::VehicleStateReport;
class VEHICLE_INTERFACE_PUBLIC Command
{
public:
  Command(
    const BasicControlCommand & control = BasicControlCommand{},
    const MaybeStateCommand & state = MaybeStateCommand{})
  : m_control{control}, m_state{state} {}

  const BasicControlCommand & control() const noexcept {return m_control;}
  const MaybeStateCommand & state() const noexcept {return m_state;}

private:
  BasicControlCommand m_control;
  MaybeStateCommand m_state;
};  // class Command

enum class StateMachineReport : uint8_t
{
  CLAMP_PAST_THRESHOLD,  
  BAD_STATE,  
  WIPERS_ON_HEADLIGHTS_ON,  
  REMOVE_GEAR_COMMAND,  
  HIGH_FREQUENCY_ACCELERATION_COMMAND,  
  HIGH_FREQUENCY_STEER_COMMAND,  
  HIGH_FREQUENCY_VELOCITY_REPORT,  
  HIGH_FREQUENCY_STEER_REPORT,  
  STATE_TRANSITION_TIMEOUT  
};  // enum class StateMachineWarning

// TODO(c.ho) concept comparable
template<typename T>
class VEHICLE_INTERFACE_PUBLIC Limits
{
public:
  Limits(T min, T max, T threshold)
  : m_min{min}, m_max{max}, m_threshold{threshold}
  {
    if (min >= max) {
      throw std::domain_error{"min >= max"};
    }
  }

  T min() const noexcept {return m_min;}
  T max() const noexcept {return m_max;}
  T threshold() const noexcept {return m_threshold;}
  bool8_t clamp_warn(T & value) const noexcept
  {
    const auto value_raw = value;
    value = std::min(std::max(m_min, value_raw), m_max);
    return std::abs(value - value_raw) >= m_threshold;
  }

private:
  T m_min;
  T m_max;
  T m_threshold;
};

class VEHICLE_INTERFACE_PUBLIC StateMachineConfig
{
public:
  using VelocityT = decltype(Odometry::velocity_mps);
  using AccelT = decltype(BasicControlCommand::long_accel_mps2);
  using AccelLimits = Limits<AccelT>;
  using FrontWheelLimits = Limits<decltype(BasicControlCommand::front_wheel_angle_rad)>;

  StateMachineConfig(
    const VelocityT gear_shift_velocity_threshold,
    const AccelLimits & accel_limits,
    const FrontWheelLimits & front_wheel_limits,
    const std::chrono::nanoseconds time_step,
    const AccelT timeout_accel_mps2,
    const std::chrono::nanoseconds state_transition_timeout,
    const AccelT auto_gear_shift_accel_deadzone_mps2
  );

  VelocityT gear_shift_velocity_threshold() const noexcept;
  const AccelLimits & accel_limits() const noexcept;
  const FrontWheelLimits & front_wheel_limits() const noexcept;
  std::chrono::nanoseconds time_step() const noexcept;
  AccelT timeout_acceleration() const noexcept;
  std::chrono::nanoseconds state_transition_timeout() const noexcept;
  AccelT auto_gear_shift_accel_deadzone() const noexcept;

private:
  VelocityT m_gear_shift_velocity_threshold;
  AccelLimits m_accel_limits;
  FrontWheelLimits m_front_wheel_limits;
  std::chrono::nanoseconds m_time_step;
  AccelT m_timeout_acceleration;
  std::chrono::nanoseconds m_state_transition_timeout;
  AccelT m_auto_gear_shift_accel_deadzone;
};  // class StateMachineConfig

class VEHICLE_INTERFACE_PUBLIC SafetyStateMachine
{
public:
  using Reports = std::vector<StateMachineReport>;  // TODO(c.ho) maybe a set instead
  explicit SafetyStateMachine(const StateMachineConfig & config);
  Command compute_safe_commands(const Command & command);
  void update(const Odometry & odom, const StateReport & state);
  Command timeout_commands() const noexcept;
  const Reports & reports() const noexcept;
  const StateMachineConfig & get_config() const noexcept;

private:
  using VSC = autoware_auto_msgs::msg::VehicleStateCommand;
  using MaybeEnum = std::experimental::optional<decltype(VSC::blinker)>;
  //lint -save -e9150 NOLINT Pure aggregate and only used internally; no constraints on state
  template<typename T>
  struct ValueStamp
  {
    T value;
    std::chrono::system_clock::time_point stamp{std::chrono::system_clock::time_point::min()};
  };
  struct StateChangeRequests
  {
    ValueStamp<decltype(VSC::gear)> gear;
    ValueStamp<decltype(VSC::blinker)> blinker;
    ValueStamp<decltype(VSC::wiper)> wiper;
    ValueStamp<decltype(VSC::headlight)> headlight;
    ValueStamp<decltype(VSC::mode)> mode;
    ValueStamp<decltype(VSC::hand_brake)> hand_brake;
    ValueStamp<decltype(VSC::horn)> horn;
  };  // struct StateChangeRequest
  //lint -restore NOLINT
  // Make sure uint members are within range--otherwise set to NO_COMMAND
  VEHICLE_INTERFACE_LOCAL VSC sanitize(const VSC & msg) const;
  // Turn on headlights if the wipers are being turned on
  VEHICLE_INTERFACE_LOCAL static MaybeEnum headlights_on_if_wipers_on(const VSC & in);
  // Apply gear shift if velocity is small and commanded acceleration is big enough
  VEHICLE_INTERFACE_LOCAL uint8_t automatic_gear_shift(
    const BasicControlCommand control,
    const VSC & state) const;
  // Remove "big" gear shifts if velocity is too large in magnitude
  VEHICLE_INTERFACE_LOCAL bool8_t bad_gear_shift(const VSC & in) const;
  // Clamp control values; warn if wildly out of range
  VEHICLE_INTERFACE_LOCAL BasicControlCommand clamp(BasicControlCommand in) const;
  // Add/overwrite any new state change requests
  VEHICLE_INTERFACE_LOCAL void cache_state_change_request(const VSC & in);
  // Clear any state changes that have happened, or warn on timeout
  VEHICLE_INTERFACE_LOCAL void check_state_change(const StateReport & in);

  StateMachineConfig m_config;
  Odometry m_odometry{};
  StateReport m_state{};
  StateChangeRequests m_requests{};
  mutable Reports m_reports{};
};  // class SafetyStateMachine
}  // namespace vehicle_interface
}  // namespace drivers
}  // namespace autoware

#endif  // VEHICLE_INTERFACE__SAFETY_STATE_MACHINE_HPP_