Decision Class Reference

#include <DecisionStateMachine.hpp>

Collaboration diagram for Decision:

Public Member Functions
	Decision (rclcpp::Node *nodePtr)
void	set_velocity_reference (float velocityReference)
void	set_velocity_reference_parking (float velocityReference)
void	set_distance_after_switch_back_lane (float valueM)
void	set_critical_obstacle_distance_for_lane_changing (int valueCM)
void	start_driving ()
void	update (std::shared_ptr< PositionTimepoint > positionTimepoint, std::tuple< float, float, float, float > laneBlocked)
void	update_tof (float right, float rear)
void	update_stopline (float value)
void	update_starttargetline (float value)
void	set_parking_state (ParkingState state)
bool	get_indicator_left () const
bool	get_indicator_right () const
bool	get_chosen_lane () const
float	get_velocity_reference () const
bool	get_brakes_enable () const
SituationState	get_current_situationstate ()

Public Attributes
int	m_parkingTries = 0
bool	m_stopAtStartLine = false

Private Member Functions
void	update_lanes (DrivingState &newState, bool leftLaneBlocked, bool rightLaneBlocked)
void	set_indicators (DrivingState &newState)
void	switch_lane_if_possible (DrivingState &newState)

Private Attributes
std::atomic< bool >	m_startDriving
std::mutex	m_mtx
DrivingState	m_state
float	m_velocityReference = 1.5f
float	m_velocityAtParkingZone = 1.0f
float	m_distanceAfterSwitchBackToLaneMeter = 1.f
int	m_criticalObstacleDistanceForLaneChangingCM = 120
rclcpp::Node *	m_nodePtr = nullptr

Detailed Description

Definition at line 14 of file DecisionStateMachine.hpp.

Constructor & Destructor Documentation

Decision()

Decision::Decision

(

rclcpp::Node *

nodePtr

)

Definition at line 8 of file DecisionStateMachine.cpp.

                                      {
  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;
}

Member Function Documentation

get_brakes_enable()

bool Decision::get_brakes_enable

(

)

const

Definition at line 336 of file DecisionStateMachine.cpp.

                                       {
  return m_state.velocity == 0.f;
}

get_chosen_lane()

bool Decision::get_chosen_lane

(

)

const

Definition at line 330 of file DecisionStateMachine.cpp.

                                      {
  return m_state.drivingLane;
}

Referenced by AutoPilot::get_drive_information().

Here is the caller graph for this function:

get_current_situationstate()

SituationState Decision::get_current_situationstate

(

)

Definition at line 339 of file DecisionStateMachine.cpp.

                                                    {
  std::unique_lock<std::mutex> lock(m_mtx);
  return m_state.situationState;
}

Referenced by AutoPilotNode::debug_publish_loop(), and AutoPilot::get_drive_information().

Here is the caller graph for this function:

get_indicator_left()

bool Decision::get_indicator_left

(

)

const

Definition at line 324 of file DecisionStateMachine.cpp.

                                         {
  return m_state.indicatorLeft;
}

Referenced by AutoPilot::get_drive_information().

Here is the caller graph for this function:

get_indicator_right()

bool Decision::get_indicator_right

(

)

const

Definition at line 327 of file DecisionStateMachine.cpp.

                                          {
  return m_state.indicatorRight;
}

Referenced by AutoPilot::get_drive_information().

Here is the caller graph for this function:

get_velocity_reference()

float Decision::get_velocity_reference

(

)

const

Definition at line 333 of file DecisionStateMachine.cpp.

                                             {
  return m_state.velocity;
}

Referenced by AutoPilot::get_drive_information(), and AutoPilotNode::on_simulation_pose_callback().

Here is the caller graph for this function:

set_critical_obstacle_distance_for_lane_changing()

void Decision::set_critical_obstacle_distance_for_lane_changing

(

int

valueCM

)

Definition at line 36 of file DecisionStateMachine.cpp.

                                                                           {
  m_criticalObstacleDistanceForLaneChangingCM = valueCM;
}

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_distance_after_switch_back_lane()

void Decision::set_distance_after_switch_back_lane

(

float

valueM

)

Definition at line 33 of file DecisionStateMachine.cpp.

                                                               {
  m_distanceAfterSwitchBackToLaneMeter = valueM;
}

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_indicators()

void Decision::set_indicators ( DrivingState &  newState )

private

Definition at line 275 of file DecisionStateMachine.cpp.

                                                    {
  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;
    }
  }
}

References DrivingState::drivingLane, DrivingState::indicatorLeft, DrivingState::indicatorRight, DrivingState::timeIndicatorSet, and DrivingState::timestamp.

set_parking_state()

void Decision::set_parking_state

(

ParkingState

state

)

Definition at line 244 of file DecisionStateMachine.cpp.

                                                   {
  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);
  }
}

References Done, Idle, InParking, galaxis::utils::log_warning(), Parking, ParkingZone, and Searching.

Referenced by AutoPilotNode::on_parking_state_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

set_velocity_reference()

void Decision::set_velocity_reference

(

float

velocityReference

)

Definition at line 24 of file DecisionStateMachine.cpp.

                                                             {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_velocityReference = velocityReference;
  m_state.velocity = m_velocityReference;
}

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_velocity_reference_parking()

void Decision::set_velocity_reference_parking

(

float

velocityReference

)

Definition at line 29 of file DecisionStateMachine.cpp.

                                                                     {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_velocityAtParkingZone = velocityReference;
}

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

start_driving()

void Decision::start_driving

(

)

Definition at line 43 of file DecisionStateMachine.cpp.

                             {
  m_startDriving = true;
}

Referenced by AutoPilotNode::AutoPilotNode(), and AutoPilotNode::on_qr_code_callback().

Here is the caller graph for this function:

switch_lane_if_possible()

void Decision::switch_lane_if_possible ( DrivingState &  newState )

private

Definition at line 299 of file DecisionStateMachine.cpp.

                                                             {
  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
      }
    }
  }
}

References DrivingState::drivingLane, and DrivingState::timestamp.

update()

void Decision::update	(	std::shared_ptr< PositionTimepoint >	positionTimepoint,
		std::tuple< float, float, float, float >	laneBlocked
	)

Definition at line 48 of file DecisionStateMachine.cpp.

                                                                                                                          {
  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;
}

References Done, DrivingState::drivingLane, Idle, InParking, Intersection, DrivingState::intersectionLastExitTime, galaxis::utils::log_cout(), galaxis::utils::log_debug(), NormalDrive, ObstacleEvasion, Off, Parking, DrivingState::parkingState, ParkingZone, Searching, DrivingState::situationState, DrivingState::startTargetDistance, DrivingState::stopLineDistance, DrivingState::timeInSituationState, DrivingState::timeLastParkingDone, DrivingState::timestamp, DrivingState::timeStartTargetLineDetected, DrivingState::timeStopLineDetected, DrivingState::velocity, DrivingState::velocityOfWheelFeedback, and WaitForStart.

Referenced by AutoPilot::get_drive_information().

Here is the call graph for this function:

Here is the caller graph for this function:

update_lanes()

void Decision::update_lanes ( DrivingState &  newState, bool  leftLaneBlocked, bool  rightLaneBlocked  )

private

Definition at line 270 of file DecisionStateMachine.cpp.

                                                                                               {
  if(leftLaneBlocked) newState.timeLeftLaneBlocked = newState.timestamp;
  if(rightLaneBlocked) newState.timeRightLaneBlocked = newState.timestamp;
}

References DrivingState::timeLeftLaneBlocked, DrivingState::timeRightLaneBlocked, and DrivingState::timestamp.

update_starttargetline()

void Decision::update_starttargetline

(

float

value

)

Definition at line 239 of file DecisionStateMachine.cpp.

                                                 {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_state.startTargetDistance = value;
}

Referenced by AutoPilotNode::on_stop_line_callback().

Here is the caller graph for this function:

update_stopline()

void Decision::update_stopline

(

float

value

)

Definition at line 234 of file DecisionStateMachine.cpp.

                                          {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_state.stopLineDistance = value;
}

Referenced by AutoPilotNode::on_stop_line_callback().

Here is the caller graph for this function:

update_tof()

void Decision::update_tof	(	float	right,
		float	rear
	)

Definition at line 264 of file DecisionStateMachine.cpp.

                                                 {
  std::unique_lock<std::mutex> lock(m_mtx);
  m_state.tofRightValue = right;
  m_state.tofRearValue = rear;
}

Referenced by AutoPilotNode::on_ecu_measurement_callback().

Here is the caller graph for this function:

Member Data Documentation

m_criticalObstacleDistanceForLaneChangingCM

int Decision::m_criticalObstacleDistanceForLaneChangingCM = 120

private

Definition at line 25 of file DecisionStateMachine.hpp.

m_distanceAfterSwitchBackToLaneMeter

float Decision::m_distanceAfterSwitchBackToLaneMeter = 1.f

private

Definition at line 24 of file DecisionStateMachine.hpp.

m_mtx

std::mutex Decision::m_mtx

private

Definition at line 17 of file DecisionStateMachine.hpp.

m_nodePtr

rclcpp::Node* Decision::m_nodePtr = nullptr

private

Definition at line 30 of file DecisionStateMachine.hpp.

m_parkingTries

int Decision::m_parkingTries = 0

Definition at line 27 of file DecisionStateMachine.hpp.

m_startDriving

std::atomic<bool> Decision::m_startDriving

private

Definition at line 16 of file DecisionStateMachine.hpp.

m_state

DrivingState Decision::m_state

private

Definition at line 19 of file DecisionStateMachine.hpp.

m_stopAtStartLine

bool Decision::m_stopAtStartLine = false

Definition at line 33 of file DecisionStateMachine.hpp.

Referenced by AutoPilotNode::AutoPilotNode().

m_velocityAtParkingZone

float Decision::m_velocityAtParkingZone = 1.0f

private

Definition at line 23 of file DecisionStateMachine.hpp.

m_velocityReference

float Decision::m_velocityReference = 1.5f

private

Definition at line 22 of file DecisionStateMachine.hpp.

---

The documentation for this class was generated from the following files:

• nodes/Autopilot/include/Decision/DecisionStateMachine.hpp
• nodes/Autopilot/src/Decision/DecisionStateMachine.cpp