156 lines
4.6 KiB
C++
156 lines
4.6 KiB
C++
#include <cmath>
|
|
#include <cstdint>
|
|
#include <iostream>
|
|
#include <random>
|
|
#include <algorithm>
|
|
|
|
#include "AdConstants.h"
|
|
#include "AdFunctions.h"
|
|
|
|
namespace Ad
|
|
{
|
|
namespace Utils
|
|
{
|
|
|
|
/* kph_to_mps: converts kmph to meters/second */
|
|
constexpr float kph_to_mps(const float kph)
|
|
{
|
|
return 0.2778 * kph;
|
|
}
|
|
|
|
} // namespace Ad::Utils
|
|
|
|
namespace Data
|
|
{
|
|
|
|
/* init_ego_vehicle: initializes ego vehicle */
|
|
Types::Vehicle init_ego_vehicle()
|
|
{
|
|
return Types::Vehicle{
|
|
.id = Consts::EGO_VEHICLE_ID,
|
|
.lane = Types::Lane::Center,
|
|
.speed_mps = Utils::kph_to_mps(135),
|
|
.rel_dist_m = 0,
|
|
};
|
|
}
|
|
|
|
/* init_vehicle_array: initialize an array of vehicles */
|
|
template <std::size_t N>
|
|
static void init_vehicle_array(std::array<Types::Vehicle, N> &arr, int32_t start_id, Types::Lane lane)
|
|
{
|
|
/* init random generators */
|
|
std::mt19937 rng(std::random_device{}());
|
|
std::uniform_real_distribution<double> speed(-150, 150);
|
|
std::uniform_real_distribution<double> dist_gen(0, Consts::VISUALIZE_STEP);
|
|
|
|
double dist = static_cast<double> (-Consts::VISUALIZE_STEP) * (arr.size() / 2 + 1);
|
|
|
|
for (int32_t i = 0; i < arr.size(); i++)
|
|
arr[i] = Types::Vehicle{
|
|
.id = start_id++,
|
|
.lane = lane,
|
|
.speed_mps = Utils::kph_to_mps(speed(rng)),
|
|
.rel_dist_m = (dist += Consts::VISUALIZE_STEP + dist_gen(rng))
|
|
};
|
|
}
|
|
|
|
/* init_vehicles: initializes vehicles */
|
|
Types::NeighborVehicles init_vehicles()
|
|
{
|
|
auto nv = Types::NeighborVehicles{};
|
|
|
|
int32_t id = 0;
|
|
init_vehicle_array(nv.left_lane, id, Types::Lane::Left);
|
|
init_vehicle_array(nv.center_lane, (id += nv.left_lane.size()), Types::Lane::Center);
|
|
init_vehicle_array(nv.right_lane, (id += nv.right_lane.size()), Types::Lane::Right);
|
|
|
|
return nv;
|
|
}
|
|
|
|
|
|
|
|
} // namespace Ad::Data
|
|
|
|
namespace Visualize
|
|
{
|
|
|
|
/* print_lane: prints lane, helper function for print vehicle */
|
|
void print_lane(const Types::Lane lane)
|
|
{
|
|
#define if_lane(LANE) if (lane == Types::Lane::LANE) std::cout << #LANE; else
|
|
if_lane(Left)
|
|
if_lane(Center)
|
|
if_lane(Right)
|
|
/* else */
|
|
std::cout << "Unknown";
|
|
}
|
|
|
|
/* print_vehicle: prints vehicle */
|
|
void print_vehicle(const Types::Vehicle &vehicle)
|
|
{
|
|
std::cout << "Vehicle " << vehicle.id << '\n';
|
|
std::cout << "Lane : ";
|
|
print_lane(vehicle.lane);
|
|
std::cout << '\n';
|
|
std::cout << "speed : " << vehicle.speed_mps << " m/s\n";
|
|
std::cout << "distance : " << vehicle.rel_dist_m << " m\n";
|
|
}
|
|
|
|
/* print_vehicle_array: helper function for print_neghbor_vehicles */
|
|
template <size_t N>
|
|
static void print_vehicle_array(const std::array<Types::Vehicle, N> &arr)
|
|
{
|
|
for (int32_t i = 0; i < arr.size(); i++) {
|
|
print_vehicle(arr[i]);
|
|
std::cout << '\n';
|
|
}
|
|
}
|
|
|
|
/* print_neghbor_vehicles: print neghbor vehicles information */
|
|
void print_neighbor_vehicles(const Types::NeighborVehicles &vehicles)
|
|
{
|
|
print_vehicle_array(vehicles.left_lane);
|
|
print_vehicle_array(vehicles.center_lane);
|
|
print_vehicle_array(vehicles.right_lane);
|
|
}
|
|
|
|
/* print_scene: prints scene
|
|
* very rough implementation */
|
|
void print_scene(const Types::Vehicle &ego_vehicle,
|
|
const Types::NeighborVehicles &vehicles)
|
|
{
|
|
/* TODO : sort NeighborVehicles arrays */
|
|
double min_dist_d = std::min({vehicles.left_lane[0].rel_dist_m,
|
|
vehicles.center_lane[0].rel_dist_m,
|
|
vehicles.right_lane[0].rel_dist_m});
|
|
|
|
double max_dist_d = std::max({vehicles.left_lane.back().rel_dist_m,
|
|
vehicles.center_lane.back().rel_dist_m,
|
|
vehicles.right_lane.back().rel_dist_m});
|
|
|
|
int32_t min_dist = std::floor(min_dist_d / Consts::VISUALIZE_STEP) * Consts::VISUALIZE_STEP;
|
|
int32_t max_dist = std::ceil(max_dist_d / Consts::VISUALIZE_STEP) * Consts::VISUALIZE_STEP;
|
|
|
|
int32_t li, ci, ri, count;
|
|
li = ci = ri = Consts::VEHICLES_PER_LANE - 1;
|
|
std::cout << " L C R\n";
|
|
for (int32_t dist = max_dist; dist >= min_dist; dist -= Consts::VISUALIZE_STEP) {
|
|
printf("%8d| ", dist);
|
|
char c;
|
|
#define HANDLE_LANE(lane, li) \
|
|
for (count = 0; li >= 0 && vehicles.lane[li].rel_dist_m >= dist-Consts::VISUALIZE_STEP; count++, li--) \
|
|
; \
|
|
c = !count ? ' ' : (count > 1) ? '0' + count : (vehicles.lane[li+1].speed_mps >= 0) ? 'v' : '^'; \
|
|
std::cout << c << " | "
|
|
HANDLE_LANE(left_lane, li);
|
|
HANDLE_LANE(center_lane, ci);
|
|
HANDLE_LANE(right_lane, ri);
|
|
std::cout << '\n';
|
|
}
|
|
|
|
}
|
|
|
|
} // namespace Ad::Visualize
|
|
|
|
} // namespace Ad
|