Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
163 lines (143 sloc) 5.85 KB
//------------------------------------------------------------------------------
// Author: Lukasz Janyst <lukasz@jany.st>
// Date: 10.08.2017
//------------------------------------------------------------------------------
#include "behavior_planner.hh"
#include <stdexcept>
#include <limits>
#include <cmath>
#include <iostream>
#include <iomanip>
//------------------------------------------------------------------------------
// Constructor
//------------------------------------------------------------------------------
BehaviorPlanner::BehaviorPlanner(const Map &map,
PathPredictor &predictor,
double speed):
predictor_(predictor), map_(map), speed_(speed) {
sm_[Plan::kStartUp] [Plan::kStartUp] = [this]() { return SU2SU(); };
sm_[Plan::kStartUp] [Plan::kKeepLane] = [this]() { return SU2KL(); };
sm_[Plan::kKeepLane] [Plan::kKeepLane] = [this]() { return KL2KL(); };
sm_[Plan::kKeepLane] [Plan::kChangeLeft] = [this]() { return KL2CL(-1); };
sm_[Plan::kKeepLane] [Plan::kChangeRight] = [this]() { return KL2CL(1); };
sm_[Plan::kChangeLeft] [Plan::kKeepLane] = [this]() { return CL2KL(); };
sm_[Plan::kChangeRight][Plan::kKeepLane] = [this]() { return CL2KL(); };
}
//------------------------------------------------------------------------------
// Plan the behavior
//------------------------------------------------------------------------------
Plan BehaviorPlanner::ComputeBehavior(const Vehicle &v) {
//----------------------------------------------------------------------------
// Compute costs of changing the plan
//----------------------------------------------------------------------------
v_ = &v;
auto &transitions = sm_[current_plan_];
double min_cost = std::numeric_limits<double>::max();
Plan plan = current_plan_;
std::cout << "======================================" << std::endl;
std::cout << "Current plan: " << PlanToString(current_plan_) << std::endl;
for(auto &tr: transitions) {
double cost = tr.second();
std::cout << std::left << std::setw(19);
std::cout << PlanToString(tr.first) + " costs:" << cost << std::endl;
if(cost < min_cost) {
plan = tr.first;
min_cost = cost;
}
}
current_plan_ = plan;
return current_plan_;
}
//------------------------------------------------------------------------------
// Convert plan to string
//------------------------------------------------------------------------------
std::string BehaviorPlanner::PlanToString(Plan plan) {
switch(plan) {
case Plan::kStartUp: return "StartUp";
case Plan::kKeepLane: return "KeepLane";
case Plan::kChangeLeft: return "ChangeLeft";
case Plan::kChangeRight: return "ChangeRight";
}
throw std::runtime_error("Impossible happened - Unknown Plan!");
return "";
}
//------------------------------------------------------------------------------
// Start Up to Start Up
//------------------------------------------------------------------------------
double BehaviorPlanner::SU2SU() {
++start_up1_;
if(start_up1_ < 4)
return 0;
return 100;
}
//------------------------------------------------------------------------------
// Start Up to Keep Lane
//------------------------------------------------------------------------------
double BehaviorPlanner::SU2KL() {
++start_up2_;
if(start_up2_ < 4)
return 100;
return 0;
}
//------------------------------------------------------------------------------
// Keep Lane to Keep Lane
//------------------------------------------------------------------------------
double BehaviorPlanner::KL2KL() {
auto leader = predictor_.FindLeader(*v_);
double cost = 0;
double leader_distance = 250;
if(leader) {
if(leader->speed < speed_)
cost += 100*(speed_ - leader->speed);
leader_distance = leader->sd[0] - v_->sd[0];
}
if(leader_distance < 15)
cost += std::exp(15-leader_distance);
else
cost -= 3*leader_distance;
return cost;
}
//------------------------------------------------------------------------------
// Keep Lane to Change Lane
//------------------------------------------------------------------------------
double BehaviorPlanner::KL2CL(int direction) {
//----------------------------------------------------------------------------
// Sanity checks and stuff
//----------------------------------------------------------------------------
int target_lane = map_.Lane(*v_) + direction;
if(target_lane < 0 || target_lane >= map_.NumLanes())
return std::numeric_limits<double>::max();
auto leader = predictor_.FindLeader(*v_, target_lane);
auto follower = predictor_.FindFollower(*v_, target_lane);
double cost = 0;
//----------------------------------------------------------------------------
// Speed and distance from the leader
//----------------------------------------------------------------------------
double leader_distance = 250;
if(leader) {
if(leader->speed < speed_)
cost += 100*(speed_ - leader->speed);
leader_distance = leader->sd[0] - v_->sd[0];
}
if(leader_distance < 20)
cost += std::exp(20-leader_distance);
else
cost -= 3*leader_distance;
//----------------------------------------------------------------------------
// Distance from the follower
//----------------------------------------------------------------------------
if(follower) {
double follower_distance = v_->sd[0] - follower->sd[0];
if(follower_distance < 20) {
cost += std::exp(20-follower_distance);
cost += std::exp(5*(follower->speed - v_->v));
}
}
return cost;
}
//------------------------------------------------------------------------------
// Change Lane to Keep Lane
//------------------------------------------------------------------------------
double BehaviorPlanner::CL2KL() {
return 0;
}