chapter 03 exercise partial solution

master
Tibor Bizjak 2023-07-26 14:17:04 +02:00
parent d40f20afb7
commit b660b8f153
5 changed files with 140 additions and 9 deletions

View File

@ -5,10 +5,12 @@
namespace Ad
{
namespace Constants
namespace Consts
{
constexpr std::int32_t EGO_VEHICLE_ID = -1;
constexpr std::size_t VEHICLES_PER_LANE = 2;
constexpr std::int8_t VISUALIZE_STEP = 20;
} // namespace Constants
} // namespace Ad

View File

@ -1,6 +1,8 @@
#include <cmath>
#include <cstdint>
#include <iostream>
#include <random>
#include <algorithm>
#include "AdConstants.h"
#include "AdFunctions.h"
@ -10,6 +12,7 @@ namespace Ad
namespace Utils
{
/* kph_to_mps: converts kmph to meters/second */
constexpr float kph_to_mps(const float kph)
{
return 0.2778 * kph;
@ -20,26 +23,131 @@ constexpr float kph_to_mps(const float kph)
namespace Data
{
/* init_ego_vehicle: initializes ego vehicle */
Types::Vehicle init_ego_vehicle()
{
return Types::Vehicle{
.id = Constants::EGO_VEHICLE_ID,
.id = Consts::EGO_VEHICLE_ID,
.lane = Types::Lane::Center,
.speed_mps = Utils::kph_to_mps(135),
.rel_distance_m = 0,
.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
{
void print_vehicle(const Types::Vehicle vehicle)
/* 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_distance_m << " m\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

View File

@ -11,12 +11,18 @@ namespace Utils
namespace Data
{
Types::Vehicle init_ego_vehicle();
Types::Vehicle init_ego_vehicle();
Types::NeighborVehicles init_vehicles();
} // namespace Ad::Data
namespace Visualize
{
void print_vehicle(const Types::Vehicle vehicle);
} // namespace Ad::Visualize
void print_vehicle(const Types::Vehicle &vehicle);
void print_neighbor_vehicles(const Types::NeighborVehicles &vehicles);
void print_scene(const Types::Vehicle &ego_vehicle,
const Types::NeighborVehicles &vehicles);
} // namespace Ad::Visualize
} // namespace Ad

View File

@ -1,6 +1,7 @@
#pragma once
#include <cstdint>
#include <array>
#include "AdConstants.h"
@ -22,7 +23,14 @@ struct Vehicle
std::int32_t id;
Lane lane;
double speed_mps;
double rel_distance_m;
double rel_dist_m;
};
struct NeighborVehicles
{
std::array<Vehicle, Consts::VEHICLES_PER_LANE> left_lane;
std::array<Vehicle, Consts::VEHICLES_PER_LANE> center_lane;
std::array<Vehicle, Consts::VEHICLES_PER_LANE> right_lane;
};
} // namespace Ad::Types

View File

@ -8,7 +8,14 @@
int main()
{
auto ego_vehicle = Ad::Data::init_ego_vehicle();
auto vehicles = Ad::Data::init_vehicles();
Ad::Visualize::print_vehicle(ego_vehicle);
std::cout << '\n';
Ad::Visualize::print_neighbor_vehicles(vehicles);
Ad::Visualize::print_scene(ego_vehicle, vehicles);
return 0;
}