DecisionStateMachine.cpp

Go to the documentation of this file.

#include "Decision/DecisionStateMachine.hpp"
 
#include "galaxis/AutopilotSettings.hpp"
 
#include "utils/logger.hpp"
using namespace galaxis::utils;
 
Decision::Decision(rclcpp::Node* nodePtr) {
  m_startDriving = false;
 
  // initial state
  m_state.drivingLane = true;
  m_state.indicatorLeft = false;
  m_state.indicatorRight = false;
  m_state.velocity = 0.f;
  m_state.velocityOfWheelFeedback = 0.f;
  m_state.timeLeftLaneBlocked = 0;
  m_state.timeRightLaneBlocked = 0;
  m_state.stopLineDistance = -1;
 
  m_nodePtr = nodePtr;
}
 
void Decision::set_velocity_reference(float velocityReference) {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_velocityReference = velocityReference;
  m_state.velocity = m_velocityReference;
}
void Decision::set_velocity_reference_parking(float velocityReference) {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_velocityAtParkingZone = velocityReference;
}
void Decision::set_distance_after_switch_back_lane(float valueM) {
  m_distanceAfterSwitchBackToLaneMeter = valueM;
}
void Decision::set_critical_obstacle_distance_for_lane_changing(int valueCM) {
  m_criticalObstacleDistanceForLaneChangingCM = valueCM;
}
// void Decision::set_default_driving_lane(bool value) { // false = left, true = right
//   if(value == false) log_warning("[Decision] default_driving_lane: This mode only works properly in free driving mode (without obstacles).");
//   m_defaultDrivingLane = value;
// }
void Decision::start_driving() {
  m_startDriving = true;
}
 
 
void Decision::update(std::shared_ptr<PositionTimepoint> positionTimepoint, std::tuple<float, float, float, float> laneBlocked) {
  std::unique_lock<std::mutex> lock(m_mtx);
 
  DrivingState newState = m_state;
  newState.timestamp = m_nodePtr->get_clock()->now().nanoseconds(); // warning: DO NOT use positionTimepoint timestamp
  newState.velocityOfWheelFeedback = positionTimepoint->speed;
 
  // transition functions
  auto toNormalDrive = [&]() {
    log_cout("[Decision] toNormalDrive");
    newState.situationState = SituationState::NormalDrive;
    newState.drivingLane = true; // right lane
    newState.velocity = m_velocityReference; // velocityReference
  };
  auto toObstacleEvasion = [&]() {
    log_cout("[Decision] toObstacleEvasion");
    newState.situationState = SituationState::ObstacleEvasion;
    newState.velocity = m_velocityReference * 0.75f;
  };
  auto toIntersection = [&]() {
    if((newState.timestamp - newState.intersectionLastExitTime) < 3e9) {
      newState.stopLineDistance = -1.f;
      return;
    }
 
    log_cout("[Decision] toIntersection");
    newState.situationState = SituationState::Intersection;
    newState.velocity = m_velocityReference;
    newState.timeStopLineDetected = newState.timestamp;
  };
  auto toParkingZone = [&]() {
    if((newState.timestamp - newState.timeStartTargetLineDetected) < 10e9 // prevent stopline detection 8 sec after last detected
      || (newState.timestamp -  newState.timeLastParkingDone) < 10e9 ) { // prevent 3sec from parking after parked
      newState.startTargetDistance = -1.f;
      return;
    }
    
    log_cout("[Decision] toParkingZone");
    newState.situationState = SituationState::ParkingZone;
    newState.startTargetDistance = -1.f;
  };
  auto toParking = [&]() {
    log_cout("[Decision] toParking");
    newState.situationState = SituationState::Parking;
    newState.velocity = 1.6f; // TODO: increase velocity in parking mode
  };
 
  bool cannotDriveOnLefLane = std::get<0>(laneBlocked) >= 0 && std::get<0>(laneBlocked) < m_criticalObstacleDistanceForLaneChangingCM;
  bool cannotDriveOnRigLane = std::get<1>(laneBlocked) >= 0 && std::get<1>(laneBlocked) < m_criticalObstacleDistanceForLaneChangingCM;
 
  this->update_lanes(newState, cannotDriveOnLefLane, cannotDriveOnRigLane);
 
 // driving state properties and transistions
  if(m_state.situationState == SituationState::WaitForStart) {
    log_debug("[Decision] WaitForStart");
    newState.velocity = 0.f;
    if(m_startDriving) {
      toNormalDrive();
    }
  }
  else if (m_state.situationState == SituationState::NormalDrive) {
    log_debug("[Decision] NormalDrive");
    if(cannotDriveOnRigLane) {
      toObstacleEvasion();
    }
    if(m_state.stopLineDistance >= 0 && m_state.stopLineDistance <= 1.f) { // stop line detected
      toIntersection();
    }
    if(m_state.startTargetDistance >= 0 && m_state.startTargetDistance <= 1.f) { // start stop line detected
      // if(m_state.parkingState == ParkingState::Idle) { // parking node is in idle // moved to parkingzone
        toParkingZone();
      // }
      // else {
        m_state.startTargetDistance = -1.f;
      // }
    }
  }
  else if (m_state.situationState == SituationState::ObstacleEvasion) {
    log_debug("[Decision] ObstacleEvasion");
    if(cannotDriveOnRigLane) { // right blocked -> drive on left
      newState.velocity = m_velocityReference * 0.8f;
      newState.drivingLane = false;
 
      if(cannotDriveOnLefLane) { // both lanes blocked -> drive very slow
        if(std::get<0>(laneBlocked) < std::get<1>(laneBlocked)) {
          newState.drivingLane = true;
        }
 
        // if(std::get<0>(laneBlocked) < 0.4f && std::get<1>(laneBlocked) < 0.4f) {
        //   // newState.velocity = 0.f;
        // }
        // else {
        //   newState.velocity = m_velocityReference * 0.75f;
        // }
        
      }
 
      this->switch_lane_if_possible(newState); // TODO maybe reenable
    } 
    // else if (m_state.tofRightValue > 0.f && m_state.tofRightValue < 0.35f && m_state.drivingLane == false) { // right lane blocked detected by tof (not 3d cam)
    //   newState.drivingLane = false; // stay on left
    // }
    else { // back to normal mode
      newState.drivingLane = true;
      this->switch_lane_if_possible(newState); // sets driving lane to false if switch not possible
      if(newState.drivingLane == true) {
        toNormalDrive();
      }
    }
  }
  else if (m_state.situationState == SituationState::Intersection) {
    log_debug("[Decision] Intersection");
    log_cout("[Decision] Intersection\n - RightLaneObjectDistance: ", std::get<1>(laneBlocked), "\n - IntersectionRightBeforeLeftDistance: ", std::get<2>(laneBlocked));
 
    newState.velocity = 0.f; // always brake at intersection
 
    if((std::get<2>(laneBlocked) >= 0.f && std::get<2>(laneBlocked) <= 85.f) 
        || (std::get<0>(laneBlocked) >= 0 && std::get<0>(laneBlocked) <= 0.6f)
        || (std::get<1>(laneBlocked) >= 0 && std::get<1>(laneBlocked) <= 0.6f)) {
      newState.velocity = 0.f; // brake
    }
    else if (std::get<2>(laneBlocked) < 0.f && !cannotDriveOnRigLane && (std::get<3>(laneBlocked) < 0.f || std::get<3>(laneBlocked) > 0.5f)) {
      if(newState.timestamp - m_state.timeInSituationState > 3e9) { // wait minimum 3s in state
        newState.stopLineDistance = -1;
        newState.intersectionLastExitTime = newState.timestamp;
        toNormalDrive(); // TODO maybe state for: only straight driving within intersection
      }
    }
    // if(newState.timestamp - newState.timeStopLineDetected > 3e9) { // timeout
    //   newState.stopLineDistance = -1;
    //   toNormalDrive();
    // }
  }
  else if (m_state.situationState == SituationState::ParkingZone) {
    log_debug("[Decision] ParkingZone");
    if((m_state.parkingState == ParkingState::Idle || m_state.parkingState == ParkingState::Searching) && get_velocity_reference() > m_velocityAtParkingZone) {
      newState.velocity = m_velocityAtParkingZone;
    }
    else {
      newState.velocity = get_velocity_reference();
    }
 
    if(m_stopAtStartLine && (newState.timestamp - m_state.timeInSituationState) < 1e9) {
      newState.velocity = 0.f;
    }
 
    if(newState.parkingState == ParkingState::InParking) {
      toParking();
      newState.startTargetDistance = -1.f;
    }
    if(newState.timestamp - m_state.timeInSituationState > 4e9) { // timeout
      log_cout(std::to_string(newState.timestamp - m_state.timeInSituationState));
      log_cout("[Decision] Parking: Searching TIMEOUT");
      newState.parkingState = ParkingState::Off;
      newState.startTargetDistance = -1.f;
      toNormalDrive();
    }
  }
  else if (m_state.situationState == SituationState::Parking) {
    log_debug("[Decision] Parking");
    if(newState.parkingState == ParkingState::Done) {
      newState.parkingState = ParkingState::Off;
      m_parkingTries++;
      toNormalDrive();
    }
    // TODO: reenable timeout if parking place not found
    if(newState.timestamp - m_state.timeInSituationState > 8e9) { // timeout
      log_cout(std::to_string(newState.timestamp - m_state.timeInSituationState));
      log_cout("[Decision] Parking: Parking TIMEOUT");
      newState.parkingState = ParkingState::Off;
      newState.timeLastParkingDone = newState.timestamp;
      toNormalDrive();
    }
  }
 
  // set indicators based on new state (and last)
  this->set_indicators(newState); // reference to state
 
  if(m_state.situationState != newState.situationState) {
    newState.timeInSituationState = m_nodePtr->get_clock()->now().nanoseconds();
  }
 
  // make persistent
  m_state = newState;
}
 
void Decision::update_stopline(float value) {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_state.stopLineDistance = value;
}
 
void Decision::update_starttargetline(float value) {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_state.startTargetDistance = value;
}
 
void Decision::set_parking_state(ParkingState state) {
  std::unique_lock<std::mutex> lock(m_mtx);
  if(state == ParkingState::Searching && m_state.situationState == SituationState::ParkingZone && m_state.parkingState == ParkingState::Idle) {
    m_state.parkingState = ParkingState::Searching;
  }
  else if(state == ParkingState::InParking && m_state.situationState == SituationState::ParkingZone && m_state.parkingState == ParkingState::Searching) {
    m_state.parkingState = ParkingState::InParking;
  }
  else if(state == ParkingState::Done && m_state.situationState == SituationState::Parking) {
    m_state.parkingState = state;
  }
  else if(state == ParkingState::Idle && m_state.situationState != SituationState::Parking) {
    m_state.parkingState = state;
  }
  else {
    // other states are handled via update function
    log_warning("[Decision] Can NOT set this parking state: ", state);
  }
}
 
void Decision::update_tof(float right, float rear) {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_state.tofRightValue = right;
  m_state.tofRearValue = rear;
}
 
void Decision::update_lanes(DrivingState& newState, bool leftLaneBlocked, bool rightLaneBlocked) {
  if(leftLaneBlocked) newState.timeLeftLaneBlocked = newState.timestamp;
  if(rightLaneBlocked) newState.timeRightLaneBlocked = newState.timestamp;
}
 
void Decision::set_indicators(DrivingState& newState) {
  if(m_state.drivingLane == false && newState.drivingLane == true) { // switch from left lane to right -> enable left indicator
    newState.indicatorRight = true;
    newState.timeIndicatorSet = newState.timestamp;
 
    newState.indicatorLeft = false;
  }
  if(m_state.drivingLane == true && newState.drivingLane == false) { // switch from left lane to right -> enable left indicator
    newState.indicatorLeft = true;
    newState.timeIndicatorSet = newState.timestamp;
 
    newState.indicatorRight = false;
  }
 
  // disable indicators
  // rclcpp::Duration keepIndicatorOnDuration(1, 500000000); // 1.5 sec
  if(m_state.indicatorLeft == true || m_state.indicatorRight == true) {
    if((newState.timestamp - newState.timeIndicatorSet) > 1500000000) { // 2.5sec
      newState.indicatorLeft = false;
      newState.indicatorRight = false;
    }
  }
}
 
void Decision::switch_lane_if_possible(DrivingState& newState) {
  if(newState.drivingLane != m_state.drivingLane) {
    if(newState.drivingLane == false) { // new lane is left
      // log_cout("## LEFT");
      // log_cout("Time [ns]",(newState.timestamp - m_state.timeLeftLaneBlocked));
      // log_cout("Time [ns] / 1000: ", ((newState.timestamp - m_state.timeLeftLaneBlocked) / 1000000000.f));
      // log_cout("CurrVelo [m/s]: ", (m_state.velocityOfWheelFeedback / 1000.f));
      // log_cout("FINAL: ", ((newState.timestamp - m_state.timeLeftLaneBlocked) / 1000000000.f) * (m_state.velocityOfWheelFeedback / 1000.f));
      if(((newState.timestamp - m_state.timeLeftLaneBlocked) / 1000000000.f) * (m_state.velocityOfWheelFeedback / 1000.f) < m_distanceAfterSwitchBackToLaneMeter) { // ns -> s (1e9) mm-> m (1000)
        newState.drivingLane = m_state.drivingLane; // stay on right
      }
    }
    else { // new lane is right
      // log_cout("## RIGHT");
      // log_cout("Time [ns]",(newState.timestamp - m_state.timeRightLaneBlocked));
      // log_cout("Time [ns] / 1000: ", ((newState.timestamp - m_state.timeRightLaneBlocked) / 1000000000.f));
      // log_cout("CurrVelo [m/s]: ", (m_state.velocityOfWheelFeedback / 1000.f));
      // log_cout("FINAL: ", ((newState.timestamp - m_state.timeRightLaneBlocked) / 1000000000.f) * (m_state.velocityOfWheelFeedback / 1000.f));
      if(((newState.timestamp - m_state.timeRightLaneBlocked) / 1000000000.f) * (m_state.velocityOfWheelFeedback / 1000.f) < m_distanceAfterSwitchBackToLaneMeter) { // ns -> s (1e9) mm-> m (1000)
        newState.drivingLane = m_state.drivingLane; // stay on left
      }
    }
  }
}
 
bool  Decision::get_indicator_left() const {
  return m_state.indicatorLeft;
}
bool  Decision::get_indicator_right() const {
  return m_state.indicatorRight;
}
bool  Decision::get_chosen_lane() const {
  return m_state.drivingLane;
}
float Decision::get_velocity_reference() const {
  return m_state.velocity;
}
bool Decision::get_brakes_enable() const {
  return m_state.velocity == 0.f;
}
SituationState Decision::get_current_situationstate() {
  std::unique_lock<std::mutex> lock(m_mtx);
  return m_state.situationState;
}