From c4511749c93ad9b00b263605ecea2b4ab01c6726 Mon Sep 17 00:00:00 2001 From: Michael Krasa Date: Sun, 12 Apr 2026 15:42:04 -0700 Subject: [PATCH] Add interactive swarm simulator replay --- .gitignore | 3 + CMakeLists.txt | 41 ++ README.md | 123 +++- configs/default_mission.json | 7 + include/aerostack/control.hpp | 43 ++ include/aerostack/mission_manager.hpp | 22 + include/aerostack/planning.hpp | 25 + include/aerostack/simulation_engine.hpp | 47 ++ include/aerostack/telemetry.hpp | 39 ++ include/aerostack/types.hpp | 59 ++ include/aerostack/world.hpp | 30 + replay.html | 310 ++++++++++ src/app/main.cpp | 773 ++++++++++++++++++++++++ src/control/local_avoidance.cpp | 129 ++++ src/planning/astar_planner.cpp | 101 ++++ src/sim/mission_manager.cpp | 60 ++ src/sim/simulation_engine.cpp | 124 ++++ src/telemetry/telemetry.cpp | 40 ++ src/world/world.cpp | 23 + tests/test_core.cpp | 26 + 20 files changed, 2024 insertions(+), 1 deletion(-) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 configs/default_mission.json create mode 100644 include/aerostack/control.hpp create mode 100644 include/aerostack/mission_manager.hpp create mode 100644 include/aerostack/planning.hpp create mode 100644 include/aerostack/simulation_engine.hpp create mode 100644 include/aerostack/telemetry.hpp create mode 100644 include/aerostack/types.hpp create mode 100644 include/aerostack/world.hpp create mode 100644 replay.html create mode 100644 src/app/main.cpp create mode 100644 src/control/local_avoidance.cpp create mode 100644 src/planning/astar_planner.cpp create mode 100644 src/sim/mission_manager.cpp create mode 100644 src/sim/simulation_engine.cpp create mode 100644 src/telemetry/telemetry.cpp create mode 100644 src/world/world.cpp create mode 100644 tests/test_core.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9ae0ef6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +/build/ +*.log +*.jsonl diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..4020090 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.20) +project(aerostack VERSION 0.1.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +option(AEROSTACK_BUILD_TESTS "Build unit tests" ON) + +add_library(aerostack_lib + src/world/world.cpp + src/planning/astar_planner.cpp + src/control/local_avoidance.cpp + src/sim/mission_manager.cpp + src/sim/simulation_engine.cpp + src/telemetry/telemetry.cpp +) + +target_include_directories(aerostack_lib PUBLIC include) + +target_compile_options(aerostack_lib PRIVATE + $<$:-Wall -Wextra -Wpedantic> +) + +add_executable(aerostack_sim src/app/main.cpp) +target_link_libraries(aerostack_sim PRIVATE aerostack_lib) + +if(AEROSTACK_BUILD_TESTS) + include(CTest) + enable_testing() + + add_executable(aerostack_tests tests/test_core.cpp) + target_link_libraries(aerostack_tests PRIVATE aerostack_lib) + add_test(NAME aerostack_tests COMMAND aerostack_tests) + add_test(NAME aerostack_smoke COMMAND aerostack_sim --scenario smoke --quiet --fail-if-incomplete) + add_test(NAME aerostack_swarm COMMAND aerostack_sim --scenario swarm --quiet --fail-if-incomplete) + add_test(NAME aerostack_reject_negative_drones COMMAND aerostack_sim --drones -1 --quiet) + set_tests_properties(aerostack_reject_negative_drones PROPERTIES WILL_FAIL TRUE) + add_test(NAME aerostack_reject_junk_missions COMMAND aerostack_sim --missions 12x --quiet) + set_tests_properties(aerostack_reject_junk_missions PROPERTIES WILL_FAIL TRUE) +endif() diff --git a/README.md b/README.md index c8da246..db2f9f4 100644 --- a/README.md +++ b/README.md @@ -1 +1,122 @@ -# aerostack +# AeroStack: C++20 Drone Swarm Simulator + +A modular multi-drone autonomy simulator built with **modern C++20**. The project models a simplified autonomy stack for swarm mission execution: + +- world model + obstacles +- global planning (A*) +- local collision avoidance (predictive safety check) +- mission manager + dynamic replanning +- simulation engine using `std::jthread` and `std::stop_token` +- event telemetry using `std::variant` + +## Architecture + +```text +/include/aerostack + types.hpp + world.hpp + planning.hpp + control.hpp + mission_manager.hpp + simulation_engine.hpp + telemetry.hpp +/src + world/ + planning/ + control/ + sim/ + telemetry/ + app/main.cpp +/tests + test_core.cpp +``` + +### Core robotics-style components + +- **World**: continuous 2D space with circular obstacles and drone/mission state. +- **AStarPlanner**: global path planner on a discretized grid. +- **PredictiveAvoidance**: local collision avoidance by simulating near-term future trajectories. +- **MissionManager**: assigns random goals and replans if goals become unreachable. +- **SimulationEngine**: runs the control loop in a dedicated `std::jthread` with cooperative shutdown. +- **TelemetryRecorder**: records simulation events and can export JSONL logs. + +## Build + +```bash +cmake -S . -B build +cmake --build build -j +``` + +## Run simulator + +```bash +./build/aerostack_sim +``` + +The simulator is intended to be a terminal-first autonomy demo and browser replay generator. A run prints: + +- the scenario geometry, obstacles, drones, and deterministic mission goals +- the autonomy components being exercised: A* planning, predictive avoidance, mission management, and telemetry +- live progress every few seconds of simulated time, including drone positions, speed, mission distance, battery, collision count, and telemetry count +- a final result summary with simulated time, drone count, completed missions, collision count, and telemetry events + +It demonstrates the core shape of a robotics stack rather than a visual game/sandbox: missions are generated, drones plan global paths around obstacles, local control checks near-term safety, mission completion is detected, telemetry is recorded, and `replay.html` is regenerated for browser inspection. + +Useful simulator variants: + +```bash +# Fast deterministic smoke run: 2 drones, 2 short missions, exits nonzero if incomplete. +./build/aerostack_sim --scenario smoke --quiet --fail-if-incomplete + +# Swarm demo: deterministic lanes through an obstacle field, expected to complete. +./build/aerostack_sim --scenario swarm --quiet --fail-if-incomplete + +# Larger stress run with more work than drones; useful for finding congestion limits. +./build/aerostack_sim --scenario swarm --drones 20 --missions 40 --duration 120 + +# Terminal visualization without adding GUI dependencies. +./build/aerostack_sim --scenario demo --map --report-every 5 + +# Standalone interactive browser replay. +./build/aerostack_sim --scenario demo --quiet + +# Export telemetry for plotting or a later UI. +./build/aerostack_sim --scenario swarm --telemetry-out swarm.jsonl --quiet +``` + +Every simulator run writes a fresh standalone browser replay to `replay.html` by default. Use `--html-out ` only when you want a different output path. + +Supported options: + +- `--scenario `: choose a preset. +- `--drones `: set drone count. +- `--missions `: set mission count; missions can exceed drones and are assigned round-robin. +- `--duration `: set simulated runtime. +- `--report-every `: set progress interval. +- `--map`: print a coarse ASCII map where digits are drones, `*` marks active goals, and `#` marks obstacles. +- `--quiet`: only print the final summary. +- `--telemetry-out `: write simulation telemetry as JSONL. +- `--html-out `: set the standalone browser replay path; default is `replay.html`. +- `--fail-if-incomplete`: return a nonzero exit code unless all missions complete. + +## Run tests + +```bash +ctest --test-dir build --output-on-failure +``` + +## Metrics emitted by the simulator + +- `Missions completed`: how many assigned goals were reached within the simulated duration. +- `Detected collisions`: close-range drone/drone contacts observed during the run. The built-in smoke and swarm demos should stay at `0`. +- `Telemetry events`: control-loop tick and event records captured for replay/export. +- Per-drone progress lines: current position, speed, distance to the active goal, and battery level. +- `replay.html`: the latest interactive Canvas replay, overwritten on every simulator run unless `--html-out` points somewhere else. + +## Next upgrades + +- noisy GPS/IMU and Kalman filtering +- occupancy grid mapping +- battery-aware task allocation +- no-fly zones and wind disturbance model +- GUI frontend (raylib/SDL2/ImGui) diff --git a/configs/default_mission.json b/configs/default_mission.json new file mode 100644 index 0000000..e684142 --- /dev/null +++ b/configs/default_mission.json @@ -0,0 +1,7 @@ +{ + "world": {"width": 80, "height": 50}, + "drones": 6, + "seed": 42, + "step_ms": 50, + "duration_s": 10 +} diff --git a/include/aerostack/control.hpp b/include/aerostack/control.hpp new file mode 100644 index 0000000..fe8a392 --- /dev/null +++ b/include/aerostack/control.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +#include "aerostack/types.hpp" + +namespace aerostack { + +template +concept LocalAvoidance = requires(T avoidance, const DroneState& self, const std::vector& others, + const std::vector& obstacles, const Vec2& desired_velocity, double dt) { + { avoidance.safe_velocity(self, others, obstacles, desired_velocity, dt) } -> std::same_as; +}; + +class PredictiveAvoidance { + public: + PredictiveAvoidance(double horizon_seconds = 2.0, double min_separation = 1.5) + : horizon_seconds_(horizon_seconds), min_separation_(min_separation) {} + + Vec2 safe_velocity(const DroneState& self, + const std::vector& others, + const std::vector& obstacles, + const Vec2& desired_velocity, + double dt) const; + + private: + [[nodiscard]] bool unsafe(const DroneState& self, + const std::vector& others, + const std::vector& obstacles, + const Vec2& v, + double dt) const; + [[nodiscard]] Vec2 separation_velocity(const DroneState& self, const std::vector& others) const; + [[nodiscard]] bool obstacle_unsafe(const DroneState& self, + const std::vector& obstacles, + const Vec2& v, + double dt) const; + + double horizon_seconds_; + double min_separation_; +}; + +} // namespace aerostack diff --git a/include/aerostack/mission_manager.hpp b/include/aerostack/mission_manager.hpp new file mode 100644 index 0000000..ace23f5 --- /dev/null +++ b/include/aerostack/mission_manager.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "aerostack/planning.hpp" +#include "aerostack/telemetry.hpp" +#include "aerostack/world.hpp" + +namespace aerostack { + +class MissionManager { + public: + MissionManager(); + + void seed_random_missions(World& world, std::size_t mission_count); + void update(World& world, const AStarPlanner& planner, TelemetryRecorder& telemetry); + + private: + std::mt19937 rng_; +}; + +} // namespace aerostack diff --git a/include/aerostack/planning.hpp b/include/aerostack/planning.hpp new file mode 100644 index 0000000..a5c063a --- /dev/null +++ b/include/aerostack/planning.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include +#include + +#include "aerostack/types.hpp" + +namespace aerostack { + +template +concept GlobalPlanner = requires(T planner, const WorldSnapshot& world, const Vec2& start, const Vec2& goal) { + { planner.plan(world, start, goal) } -> std::same_as>>; +}; + +class AStarPlanner { + public: + explicit AStarPlanner(double grid_resolution = 1.0) : grid_resolution_(grid_resolution) {} + std::optional> plan(const WorldSnapshot& world, const Vec2& start, const Vec2& goal) const; + + private: + double grid_resolution_; +}; + +} // namespace aerostack diff --git a/include/aerostack/simulation_engine.hpp b/include/aerostack/simulation_engine.hpp new file mode 100644 index 0000000..b83b05a --- /dev/null +++ b/include/aerostack/simulation_engine.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include + +#include "aerostack/control.hpp" +#include "aerostack/mission_manager.hpp" + +namespace aerostack { + +struct DroneMetrics { + double path_length{0.0}; + double battery_used{0.0}; + std::size_t replans{0}; +}; + +class SimulationEngine { + public: + SimulationEngine(World world, AStarPlanner planner, PredictiveAvoidance avoidance, MissionManager mission_manager, + TelemetryRecorder telemetry); + + void run_for(std::chrono::seconds duration, std::chrono::milliseconds step); + void stop(); + void step(double dt); + + [[nodiscard]] WorldSnapshot snapshot() const { return world_.snapshot(); } + [[nodiscard]] const TelemetryRecorder& telemetry() const { return telemetry_; } + [[nodiscard]] std::size_t collisions() const { return collisions_.load(); } + + private: + void loop(std::stop_token st, std::chrono::milliseconds step); + + World world_; + AStarPlanner planner_; + PredictiveAvoidance avoidance_; + MissionManager mission_manager_; + TelemetryRecorder telemetry_; + std::atomic collisions_{0}; + std::unordered_map> planned_paths_; + std::unordered_map path_index_; + std::unordered_map metrics_; + std::jthread thread_; +}; + +} // namespace aerostack diff --git a/include/aerostack/telemetry.hpp b/include/aerostack/telemetry.hpp new file mode 100644 index 0000000..8282125 --- /dev/null +++ b/include/aerostack/telemetry.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include + +#include "aerostack/types.hpp" + +namespace aerostack { + +struct TickEvent { + double sim_time{}; + std::vector drones; +}; + +struct ReplanEvent { + std::size_t drone_id{}; + Vec2 new_goal; +}; + +struct CollisionEvent { + std::size_t a{}; + std::size_t b{}; + double sim_time{}; +}; + +using TelemetryEvent = std::variant; + +class TelemetryRecorder { + public: + void record(TelemetryEvent event); + [[nodiscard]] const std::vector& events() const { return events_; } + void write_jsonl(const std::filesystem::path& output) const; + + private: + std::vector events_; +}; + +} // namespace aerostack diff --git a/include/aerostack/types.hpp b/include/aerostack/types.hpp new file mode 100644 index 0000000..d6a886b --- /dev/null +++ b/include/aerostack/types.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include + +namespace aerostack { + +struct Vec2 { + double x{0.0}; + double y{0.0}; + + [[nodiscard]] double norm() const { return std::sqrt(x * x + y * y); } +}; + +inline Vec2 operator+(const Vec2& a, const Vec2& b) { return {a.x + b.x, a.y + b.y}; } +inline Vec2 operator-(const Vec2& a, const Vec2& b) { return {a.x - b.x, a.y - b.y}; } +inline Vec2 operator*(const Vec2& a, double s) { return {a.x * s, a.y * s}; } +inline Vec2 operator/(const Vec2& a, double s) { return {a.x / s, a.y / s}; } + +inline double distance(const Vec2& a, const Vec2& b) { return (a - b).norm(); } +inline Vec2 normalize(const Vec2& v) { + const auto n = v.norm(); + return n <= 1e-9 ? Vec2{} : v / n; +} + +struct Obstacle { + Vec2 center; + double radius{1.0}; +}; + +struct DroneState { + std::size_t id{0}; + Vec2 position; + Vec2 velocity; + double heading{0.0}; + double max_acceleration{2.0}; + double max_turn_rate{2.0}; + double battery{100.0}; + bool active{true}; +}; + +struct Mission { + std::size_t drone_id{0}; + Vec2 goal; + bool complete{false}; + std::size_t replans{0}; +}; + +struct WorldSnapshot { + double width{100.0}; + double height{100.0}; + std::vector obstacles; + std::vector drones; + std::vector missions; + double sim_time{0.0}; +}; + +} // namespace aerostack diff --git a/include/aerostack/world.hpp b/include/aerostack/world.hpp new file mode 100644 index 0000000..7c38d24 --- /dev/null +++ b/include/aerostack/world.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "aerostack/types.hpp" + +namespace aerostack { + +class World { + public: + World(double width, double height); + + [[nodiscard]] bool is_inside(const Vec2& p) const; + [[nodiscard]] bool is_collision(const Vec2& p, double clearance = 0.0) const; + [[nodiscard]] WorldSnapshot snapshot() const; + + std::vector& obstacles() { return obstacles_; } + std::vector& drones() { return drones_; } + std::vector& missions() { return missions_; } + + void advance_time(double dt) { sim_time_ += dt; } + + private: + double width_; + double height_; + double sim_time_{0.0}; + std::vector obstacles_; + std::vector drones_; + std::vector missions_; +}; + +} // namespace aerostack diff --git a/replay.html b/replay.html new file mode 100644 index 0000000..40b0e0c --- /dev/null +++ b/replay.html @@ -0,0 +1,310 @@ + + + + + +AeroStack Replay + + + +
+
+
+

AeroStack Replay

+

Interactive browser replay of a C++20 drone swarm simulation.

+
+

Scenario:

+
+
+
Time0.0s
+
Missions0/0
+
Drones0
+
Battery min0%
+
+
+
+ + + + +
+
+ drone + active goal + completed goal + obstacle +
+
+ + + diff --git a/src/app/main.cpp b/src/app/main.cpp new file mode 100644 index 0000000..d3c892c --- /dev/null +++ b/src/app/main.cpp @@ -0,0 +1,773 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "aerostack/simulation_engine.hpp" + +namespace { + +struct Options { + std::string scenario{"demo"}; + std::size_t drones{6}; + std::size_t missions{6}; + double duration_seconds{30.0}; + double step_seconds{0.05}; + double report_every_seconds{2.0}; + bool map{false}; + bool quiet{false}; + bool fail_if_incomplete{false}; + std::optional telemetry_out; + std::filesystem::path html_out{"replay.html"}; +}; + +void print_usage(const char* argv0) { + std::cout << "Usage: " << argv0 << " [options]\n\n"; + std::cout << "Options:\n"; + std::cout << " --scenario Preset world and defaults. Default: demo\n"; + std::cout << " --drones Number of drones. Overrides preset.\n"; + std::cout << " --missions Number of missions. Can exceed drones.\n"; + std::cout << " --duration Simulated seconds to run.\n"; + std::cout << " --report-every Progress interval. Default: 2\n"; + std::cout << " --map Print a coarse ASCII map at each progress report.\n"; + std::cout << " --quiet Only print final summary.\n"; + std::cout << " --telemetry-out Write telemetry JSONL after the run.\n"; + std::cout << " --html-out Browser replay path. Default: replay.html\n"; + std::cout << " --fail-if-incomplete Exit nonzero unless every mission completes.\n"; + std::cout << " --help Show this help.\n\n"; + std::cout << "Examples:\n"; + std::cout << " " << argv0 << " --scenario smoke --quiet --fail-if-incomplete\n"; + std::cout << " " << argv0 << " --scenario demo --quiet\n"; + std::cout << " " << argv0 << " --scenario swarm --drones 20 --missions 40 --map\n"; +} + +std::size_t parse_size(std::string_view raw, std::string_view name) { + if (raw.empty() || !std::all_of(raw.begin(), raw.end(), [](unsigned char c) { return std::isdigit(c); })) { + throw std::invalid_argument(std::string(name) + " must be an unsigned integer"); + } + + const auto value = std::stoul(std::string(raw)); + if (value == 0) { + throw std::invalid_argument(std::string(name) + " must be greater than zero"); + } + if (value > static_cast(std::numeric_limits::max())) { + throw std::invalid_argument(std::string(name) + " must fit in an int"); + } + return value; +} + +double parse_positive_double(std::string_view raw, std::string_view name) { + const auto value = std::stod(std::string(raw)); + if (value <= 0.0) { + throw std::invalid_argument(std::string(name) + " must be greater than zero"); + } + return value; +} + +Options options_for_scenario(std::string scenario) { + Options options; + options.scenario = std::move(scenario); + if (options.scenario == "smoke") { + options.drones = 2; + options.missions = 2; + options.duration_seconds = 10.0; + options.report_every_seconds = 5.0; + } else if (options.scenario == "swarm") { + options.drones = 12; + options.missions = 12; + options.duration_seconds = 60.0; + options.report_every_seconds = 5.0; + } else if (options.scenario != "demo") { + throw std::invalid_argument("unknown scenario: " + options.scenario); + } + return options; +} + +Options parse_options(int argc, char** argv) { + Options options; + for (int i = 1; i < argc; ++i) { + const std::string_view arg = argv[i]; + const auto require_value = [&](std::string_view name) -> std::string_view { + if (i + 1 >= argc) { + throw std::invalid_argument(std::string(name) + " requires a value"); + } + return argv[++i]; + }; + + if (arg == "--help" || arg == "-h") { + print_usage(argv[0]); + std::exit(0); + } + if (arg == "--scenario") { + const bool keep_map = options.map; + const bool keep_quiet = options.quiet; + const bool keep_fail_if_incomplete = options.fail_if_incomplete; + const auto keep_telemetry_out = options.telemetry_out; + const auto keep_html_out = options.html_out; + options = options_for_scenario(std::string(require_value(arg))); + options.map = keep_map; + options.quiet = keep_quiet; + options.fail_if_incomplete = keep_fail_if_incomplete; + options.telemetry_out = keep_telemetry_out; + options.html_out = keep_html_out; + } else if (arg == "--drones") { + options.drones = parse_size(require_value(arg), arg); + } else if (arg == "--missions") { + options.missions = parse_size(require_value(arg), arg); + } else if (arg == "--duration") { + options.duration_seconds = parse_positive_double(require_value(arg), arg); + } else if (arg == "--report-every") { + options.report_every_seconds = parse_positive_double(require_value(arg), arg); + } else if (arg == "--map") { + options.map = true; + } else if (arg == "--quiet") { + options.quiet = true; + } else if (arg == "--telemetry-out") { + options.telemetry_out = std::filesystem::path(require_value(arg)); + } else if (arg == "--html-out") { + options.html_out = std::filesystem::path(require_value(arg)); + } else if (arg == "--fail-if-incomplete") { + options.fail_if_incomplete = true; + } else { + throw std::invalid_argument("unknown argument: " + std::string(arg)); + } + } + return options; +} + +const aerostack::Mission* mission_for(const aerostack::WorldSnapshot& snapshot, std::size_t drone_id) { + const auto it = std::find_if(snapshot.missions.begin(), snapshot.missions.end(), + [drone_id](const aerostack::Mission& mission) { + return mission.drone_id == drone_id && !mission.complete; + }); + return it == snapshot.missions.end() ? nullptr : &*it; +} + +std::size_t completed_missions(const aerostack::WorldSnapshot& snapshot) { + return static_cast(std::count_if(snapshot.missions.begin(), snapshot.missions.end(), + [](const aerostack::Mission& mission) { return mission.complete; })); +} + +double nearest_goal_distance(const aerostack::WorldSnapshot& snapshot) { + double nearest = 0.0; + bool found = false; + for (const auto& drone : snapshot.drones) { + const auto* mission = mission_for(snapshot, drone.id); + if (!mission) { + continue; + } + const auto goal_distance = aerostack::distance(drone.position, mission->goal); + nearest = found ? std::min(nearest, goal_distance) : goal_distance; + found = true; + } + return nearest; +} + +aerostack::World make_world(const Options& options) { + const double width = options.scenario == "swarm" ? 120.0 : 80.0; + const double height = options.scenario == "swarm" ? 80.0 : 50.0; + aerostack::World world(width, height); + + if (options.scenario == "smoke") { + world.obstacles() = {}; + } else if (options.scenario == "swarm") { + world.obstacles() = { + {{24.0, 24.0}, 4.5}, {{42.0, 50.0}, 5.5}, {{66.0, 32.0}, 5.5}, + {{88.0, 58.0}, 4.5}, {{100.0, 24.0}, 4.0}, + }; + } else { + world.obstacles() = { + {{20.0, 20.0}, 4.0}, + {{38.0, 25.0}, 5.5}, + {{58.0, 30.0}, 4.0}, + }; + } + + const double spacing = options.scenario == "swarm" ? 3.0 : 2.0; + const std::size_t columns = static_cast(std::max(1.0, std::floor((width - 8.0) / spacing))); + for (std::size_t i = 0; i < options.drones; ++i) { + const double x = 4.0 + static_cast(i % columns) * spacing; + const double y = 4.0 + static_cast(i / columns) * spacing; + world.drones().push_back({i, {x, y}, {0.0, 0.0}, 0.0, 3.0, 2.5, 100.0, true}); + } + + return world; +} + +void seed_missions(const Options& options, aerostack::World& world, aerostack::MissionManager& mission_manager) { + if (options.scenario == "demo") { + mission_manager.seed_random_missions(world, options.missions); + return; + } + + auto& missions = world.missions(); + missions.clear(); + if (options.scenario == "swarm") { + const auto snapshot = world.snapshot(); + const std::vector goal_bank = { + {16.0, 12.0}, {16.0, 68.0}, {34.0, 12.0}, {34.0, 68.0}, {54.0, 14.0}, {54.0, 66.0}, + {76.0, 14.0}, {76.0, 66.0}, {110.0, 12.0}, {110.0, 40.0}, {110.0, 68.0}, {92.0, 12.0}, + }; + for (std::size_t i = 0; i < options.missions; ++i) { + const auto drone_id = i % world.drones().size(); + const auto lap = i / world.drones().size(); + auto goal = goal_bank[(drone_id + lap * 5) % goal_bank.size()]; + if (lap % 2 == 1) { + goal.x = snapshot.width - goal.x; + } + missions.push_back({drone_id, goal, false, 0}); + } + return; + } + + for (std::size_t i = 0; i < options.missions; ++i) { + const auto drone_id = i % world.drones().size(); + const auto lap = i / world.drones().size(); + const double x = 12.0 + static_cast(lap) * 4.0; + const double y = 6.0 + static_cast(drone_id) * 4.0; + missions.push_back({drone_id, {x, y}, false, 0}); + } +} + +void print_scenario(const aerostack::WorldSnapshot& snapshot, const Options& options) { + std::cout << "=== AeroStack Drone Swarm Simulator ===\n\n"; + std::cout << "What this repo demonstrates:\n"; + std::cout << " - A small robotics autonomy stack in C++20, not a game loop or visualizer.\n"; + std::cout << " - A* global path planning around circular obstacles.\n"; + std::cout << " - Predictive local avoidance so drones slow, stop, or sidestep unsafe near-term motion.\n"; + std::cout << " - Mission management, replanning, and telemetry events captured during each control tick.\n\n"; + + std::cout << "Scenario: " << options.scenario << "\n"; + std::cout << " World: " << snapshot.width << " x " << snapshot.height << " meters\n"; + std::cout << " Drones: " << snapshot.drones.size() << "\n"; + std::cout << " Missions: " << snapshot.missions.size(); + if (options.scenario == "smoke") { + std::cout << " deterministic short-hop goals\n"; + } else if (options.scenario == "swarm") { + std::cout << " deterministic lane goals\n"; + } else { + std::cout << " random goals from deterministic seed 42\n"; + } + std::cout << " Obstacles: " << snapshot.obstacles.size() << "\n"; + for (const auto& obstacle : snapshot.obstacles) { + std::cout << " obstacle center=(" << obstacle.center.x << ", " << obstacle.center.y << "), radius=" + << obstacle.radius << "\n"; + } + for (const auto& mission : snapshot.missions) { + std::cout << " mission drone " << mission.drone_id << " -> goal=(" << std::fixed << std::setprecision(1) + << mission.goal.x << ", " << mission.goal.y << ")\n"; + } + std::cout << "\nRunning " << std::fixed << std::setprecision(1) << options.duration_seconds + << "s of simulated time at " << std::setprecision(2) << options.step_seconds << "s control ticks"; + if (options.map) { + std::cout << " with ASCII map snapshots"; + } + std::cout << " and browser replay capture"; + std::cout << ".\n\n"; +} + +void print_map(const aerostack::WorldSnapshot& snapshot) { + constexpr std::size_t map_width = 60; + constexpr std::size_t map_height = 20; + std::vector rows(map_height, std::string(map_width, '.')); + + const auto to_col = [&](double x) { + return std::clamp(static_cast((x / snapshot.width) * static_cast(map_width)), std::size_t{0}, + map_width - 1); + }; + const auto to_row = [&](double y) { + return std::clamp(static_cast((y / snapshot.height) * static_cast(map_height)), std::size_t{0}, + map_height - 1); + }; + + for (std::size_t row = 0; row < map_height; ++row) { + for (std::size_t col = 0; col < map_width; ++col) { + const aerostack::Vec2 p{(static_cast(col) + 0.5) * snapshot.width / static_cast(map_width), + (static_cast(row) + 0.5) * snapshot.height / static_cast(map_height)}; + for (const auto& obstacle : snapshot.obstacles) { + if (aerostack::distance(p, obstacle.center) <= obstacle.radius) { + rows[row][col] = '#'; + break; + } + } + } + } + + for (const auto& mission : snapshot.missions) { + if (!mission.complete) { + rows[to_row(mission.goal.y)][to_col(mission.goal.x)] = '*'; + } + } + for (const auto& drone : snapshot.drones) { + const char marker = static_cast('0' + (drone.id % 10)); + rows[to_row(drone.position.y)][to_col(drone.position.x)] = marker; + } + + std::cout << " map legend: digit=drone, *=active goal, #=obstacle\n"; + for (const auto& row : rows) { + std::cout << " " << row << "\n"; + } +} + +void print_progress(const aerostack::WorldSnapshot& snapshot, + const Options& options, + std::size_t collisions, + std::size_t telemetry_events) { + std::cout << "[t=" << std::fixed << std::setprecision(1) << std::setw(4) << snapshot.sim_time << "s] " + << "missions " << completed_missions(snapshot) << "/" << snapshot.missions.size() << ", nearest active goal " + << std::setprecision(2) << nearest_goal_distance(snapshot) << "m, collisions " << collisions + << ", telemetry events " << telemetry_events << "\n"; + + for (const auto& drone : snapshot.drones) { + const auto* mission = mission_for(snapshot, drone.id); + const std::string status = mission ? "active" : "done "; + const double goal_distance = mission ? aerostack::distance(drone.position, mission->goal) : 0.0; + std::cout << " drone " << drone.id << " " << status << " pos=(" << std::setprecision(1) << drone.position.x << ", " + << drone.position.y << ") speed=" << std::setprecision(2) << drone.velocity.norm() + << "m/s goal_dist=" << goal_distance << "m battery=" << std::setprecision(1) << drone.battery << "%\n"; + } + if (options.map) { + print_map(snapshot); + } + std::cout << "\n"; +} + +void print_summary(const aerostack::WorldSnapshot& snapshot, const aerostack::SimulationEngine& engine) { + std::cout << "Final result:\n"; + std::cout << " Sim time: " << std::fixed << std::setprecision(2) << snapshot.sim_time << " s\n"; + std::cout << " Drones: " << snapshot.drones.size() << "\n"; + std::cout << " Missions completed: " << completed_missions(snapshot) << "/" << snapshot.missions.size() << "\n"; + std::cout << " Detected collisions: " << engine.collisions() << "\n"; + std::cout << " Telemetry events: " << engine.telemetry().events().size() << "\n"; +} + +void write_vec2_json(std::ostream& out, const aerostack::Vec2& p) { out << "{\"x\":" << p.x << ",\"y\":" << p.y << "}"; } + +void write_replay_html(const std::filesystem::path& output, + const Options& options, + const std::vector& frames) { + if (frames.empty()) { + throw std::runtime_error("cannot write replay with no frames"); + } + + std::ofstream out(output); + if (!out) { + throw std::runtime_error("failed to open replay output: " + output.string()); + } + + const auto& initial = frames.front(); + out << std::setprecision(6); + out << R"HTML( + + + + +AeroStack Replay + + + +
+
+
+

AeroStack Replay

+

Interactive browser replay of a C++20 drone swarm simulation.

+
+

Scenario:

+
+
+
Time0.0s
+
Missions0/0
+
Drones0
+
Battery min0%
+
+
+
+ + + + +
+
+ drone + active goal + completed goal + obstacle +
+
+ + + +)HTML"; +} + +} // namespace + +int main(int argc, char** argv) { + Options options; + try { + options = parse_options(argc, argv); + } catch (const std::exception& error) { + std::cerr << "error: " << error.what() << "\n\n"; + print_usage(argv[0]); + return 2; + } + + auto world = make_world(options); + aerostack::MissionManager mission_manager; + seed_missions(options, world, mission_manager); + + aerostack::SimulationEngine engine(std::move(world), aerostack::AStarPlanner(1.0), aerostack::PredictiveAvoidance(2.0, 1.4), + std::move(mission_manager), aerostack::TelemetryRecorder{}); + + if (!options.quiet) { + print_scenario(engine.snapshot(), options); + } + + double next_report = 0.0; + std::vector replay_frames; + replay_frames.push_back(engine.snapshot()); + const int steps = static_cast(std::ceil(options.duration_seconds / options.step_seconds)); + for (int i = 0; i < steps; ++i) { + engine.step(options.step_seconds); + const auto snapshot = engine.snapshot(); + replay_frames.push_back(snapshot); + if (!options.quiet && (snapshot.sim_time + 1e-9 >= next_report || i == steps - 1)) { + print_progress(snapshot, options, engine.collisions(), engine.telemetry().events().size()); + next_report += options.report_every_seconds; + } + } + + print_summary(engine.snapshot(), engine); + if (options.telemetry_out.has_value()) { + engine.telemetry().write_jsonl(*options.telemetry_out); + std::cout << "Telemetry written to: " << options.telemetry_out->string() << "\n"; + } + write_replay_html(options.html_out, options, replay_frames); + std::cout << "Browser replay written to: " << options.html_out.string() << "\n"; + + const auto snapshot = engine.snapshot(); + if (options.fail_if_incomplete && completed_missions(snapshot) != snapshot.missions.size()) { + return 1; + } + return 0; +} diff --git a/src/control/local_avoidance.cpp b/src/control/local_avoidance.cpp new file mode 100644 index 0000000..ab4ab44 --- /dev/null +++ b/src/control/local_avoidance.cpp @@ -0,0 +1,129 @@ +#include "aerostack/control.hpp" + +#include +#include + +namespace aerostack { +namespace { +constexpr double kHardObstacleClearance = 0.4; + +Vec2 rotate(const Vec2& v, double radians) { + const auto c = std::cos(radians); + const auto s = std::sin(radians); + return {v.x * c - v.y * s, v.x * s + v.y * c}; +} + +} // namespace + +Vec2 PredictiveAvoidance::safe_velocity(const DroneState& self, + const std::vector& others, + const std::vector& obstacles, + const Vec2& desired_velocity, + double dt) const { + const Vec2 separation = separation_velocity(self, others); + const Vec2 blended_desired = desired_velocity + separation; + + if (!unsafe(self, others, obstacles, blended_desired, dt)) { + return blended_desired; + } + + if (separation.norm() > 0.0 && !obstacle_unsafe(self, obstacles, separation, dt)) { + return separation; + } + + const Vec2 base = normalize(blended_desired.norm() > 0.0 ? blended_desired : desired_velocity); + const double speed = std::max(0.8, desired_velocity.norm()); + const std::vector candidates = { + blended_desired * 0.75, + rotate(base, 0.55) * speed, + rotate(base, -0.55) * speed, + rotate(base, 1.1) * (speed * 0.8), + rotate(base, -1.1) * (speed * 0.8), + Vec2{-base.y, base.x} * (speed * 0.65), + Vec2{base.y, -base.x} * (speed * 0.65), + desired_velocity * 0.35 + separation, + {0.0, 0.0}, + }; + + for (const auto& candidate : candidates) { + if (!unsafe(self, others, obstacles, candidate, dt)) { + return candidate; + } + } + + return {0.0, 0.0}; +} + +Vec2 PredictiveAvoidance::separation_velocity(const DroneState& self, const std::vector& others) const { + Vec2 separation{}; + for (const auto& other : others) { + if (other.id == self.id || !other.active) { + continue; + } + const Vec2 away = self.position - other.position; + const double d = away.norm(); + if (d < 1e-6 || d >= min_separation_ * 2.0) { + continue; + } + const double strength = (min_separation_ * 2.0 - d) / (min_separation_ * 2.0); + separation = separation + normalize(away) * (strength * 2.2); + } + return separation; +} + +bool PredictiveAvoidance::obstacle_unsafe(const DroneState& self, + const std::vector& obstacles, + const Vec2& v, + double dt) const { + const int steps = static_cast(horizon_seconds_ / dt); + Vec2 predicted = self.position; + + for (int i = 0; i < steps; ++i) { + predicted = predicted + v * dt; + for (const auto& obstacle : obstacles) { + if (distance(predicted, obstacle.center) < obstacle.radius + kHardObstacleClearance) { + return true; + } + } + } + + return false; +} + +bool PredictiveAvoidance::unsafe(const DroneState& self, + const std::vector& others, + const std::vector& obstacles, + const Vec2& v, + double dt) const { + const int steps = static_cast(horizon_seconds_ / dt); + Vec2 predicted = self.position; + + for (int i = 0; i < steps; ++i) { + predicted = predicted + v * dt; + + for (const auto& obstacle : obstacles) { + const double predicted_clearance = distance(predicted, obstacle.center) - obstacle.radius; + if (predicted_clearance < kHardObstacleClearance) { + return true; + } + const double current_clearance = distance(self.position, obstacle.center) - obstacle.radius; + if (predicted_clearance < min_separation_ && predicted_clearance < current_clearance - 1e-3) { + return true; + } + } + + for (const auto& other : others) { + if (other.id == self.id || !other.active) { + continue; + } + const Vec2 other_future = other.position + other.velocity * (dt * static_cast(i + 1)); + if (distance(predicted, other_future) < min_separation_) { + return true; + } + } + } + + return false; +} + +} // namespace aerostack diff --git a/src/planning/astar_planner.cpp b/src/planning/astar_planner.cpp new file mode 100644 index 0000000..3fcde93 --- /dev/null +++ b/src/planning/astar_planner.cpp @@ -0,0 +1,101 @@ +#include "aerostack/planning.hpp" + +#include +#include +#include +#include +#include + +namespace aerostack { +namespace { +constexpr double kPlannerObstacleClearance = 0.8; + +struct Cell { + int x; + int y; + bool operator==(const Cell& other) const { return x == other.x && y == other.y; } +}; + +struct CellHash { + std::size_t operator()(const Cell& c) const { return (static_cast(c.x) << 32U) ^ static_cast(c.y); } +}; + +} // namespace + +std::optional> AStarPlanner::plan(const WorldSnapshot& world, const Vec2& start, const Vec2& goal) const { + const auto to_cell = [&](const Vec2& p) -> Cell { + return {static_cast(std::round(p.x / grid_resolution_)), static_cast(std::round(p.y / grid_resolution_))}; + }; + const auto to_point = [&](const Cell& c) -> Vec2 { return {c.x * grid_resolution_, c.y * grid_resolution_}; }; + const auto blocked = [&](const Cell& c) { + const auto p = to_point(c); + if (p.x < 0 || p.y < 0 || p.x > world.width || p.y > world.height) { + return true; + } + for (const auto& obstacle : world.obstacles) { + if (distance(p, obstacle.center) <= obstacle.radius + kPlannerObstacleClearance) { + return true; + } + } + return false; + }; + + const Cell start_cell = to_cell(start); + const Cell goal_cell = to_cell(goal); + + if (blocked(start_cell) || blocked(goal_cell)) { + return std::nullopt; + } + + struct Node { + Cell cell; + double f{}; + bool operator<(const Node& other) const { return f > other.f; } + }; + + std::priority_queue open; + std::unordered_map g_score; + std::unordered_map came_from; + + g_score[start_cell] = 0.0; + open.push({start_cell, distance(start, goal)}); + + constexpr Cell dirs[] = {{1, 0}, {-1, 0}, {0, 1}, {0, -1}, {1, 1}, {1, -1}, {-1, 1}, {-1, -1}}; + + while (!open.empty()) { + const Cell current = open.top().cell; + open.pop(); + + if (current == goal_cell) { + std::vector path; + Cell at = current; + path.push_back(to_point(at)); + while (!(at == start_cell)) { + at = came_from.at(at); + path.push_back(to_point(at)); + } + std::reverse(path.begin(), path.end()); + return path; + } + + const double current_g = g_score[current]; + for (const auto& dir : dirs) { + Cell neighbor{current.x + dir.x, current.y + dir.y}; + if (blocked(neighbor)) { + continue; + } + const double step_cost = (dir.x != 0 && dir.y != 0) ? std::sqrt(2.0) : 1.0; + const double tentative = current_g + step_cost; + if (!g_score.contains(neighbor) || tentative < g_score[neighbor]) { + came_from[neighbor] = current; + g_score[neighbor] = tentative; + const auto h = distance(to_point(neighbor), to_point(goal_cell)); + open.push({neighbor, tentative + h}); + } + } + } + + return std::nullopt; +} + +} // namespace aerostack diff --git a/src/sim/mission_manager.cpp b/src/sim/mission_manager.cpp new file mode 100644 index 0000000..fdf0405 --- /dev/null +++ b/src/sim/mission_manager.cpp @@ -0,0 +1,60 @@ +#include "aerostack/mission_manager.hpp" + +#include + +namespace aerostack { + +MissionManager::MissionManager() : rng_(42) {} + +void MissionManager::seed_random_missions(World& world, std::size_t mission_count) { + auto snapshot = world.snapshot(); + std::uniform_real_distribution x_dist(2.0, snapshot.width - 2.0); + std::uniform_real_distribution y_dist(2.0, snapshot.height - 2.0); + + auto& missions = world.missions(); + missions.clear(); + if (world.drones().empty()) { + return; + } + + for (std::size_t i = 0; i < mission_count; ++i) { + missions.push_back({i % world.drones().size(), {x_dist(rng_), y_dist(rng_)}, false, 0}); + } +} + +void MissionManager::update(World& world, const AStarPlanner& planner, TelemetryRecorder& telemetry) { + auto snapshot = world.snapshot(); + const bool should_check_reachability = std::abs(snapshot.sim_time - std::round(snapshot.sim_time)) < 1e-6; + + for (auto& mission : world.missions()) { + if (mission.complete) { + continue; + } + + auto* drone = static_cast(nullptr); + for (auto& d : world.drones()) { + if (d.id == mission.drone_id) { + drone = &d; + break; + } + } + if (!drone) { + continue; + } + + if (distance(drone->position, mission.goal) < 1.5) { + mission.complete = true; + continue; + } + + if (should_check_reachability && !planner.plan(snapshot, drone->position, mission.goal).has_value()) { + std::uniform_real_distribution x_dist(2.0, snapshot.width - 2.0); + std::uniform_real_distribution y_dist(2.0, snapshot.height - 2.0); + mission.goal = {x_dist(rng_), y_dist(rng_)}; + mission.replans++; + telemetry.record(ReplanEvent{mission.drone_id, mission.goal}); + } + } +} + +} // namespace aerostack diff --git a/src/sim/simulation_engine.cpp b/src/sim/simulation_engine.cpp new file mode 100644 index 0000000..444494f --- /dev/null +++ b/src/sim/simulation_engine.cpp @@ -0,0 +1,124 @@ +#include "aerostack/simulation_engine.hpp" + +#include +#include +#include + +namespace aerostack { + +SimulationEngine::SimulationEngine(World world, + AStarPlanner planner, + PredictiveAvoidance avoidance, + MissionManager mission_manager, + TelemetryRecorder telemetry) + : world_(std::move(world)), + planner_(std::move(planner)), + avoidance_(std::move(avoidance)), + mission_manager_(std::move(mission_manager)), + telemetry_(std::move(telemetry)) {} + +void SimulationEngine::run_for(std::chrono::seconds duration, std::chrono::milliseconds step) { + thread_ = std::jthread([this, step](std::stop_token st) { loop(st, step); }); + std::this_thread::sleep_for(duration); + stop(); +} + +void SimulationEngine::stop() { + if (thread_.joinable()) { + thread_.request_stop(); + thread_.join(); + } +} + +void SimulationEngine::loop(std::stop_token st, std::chrono::milliseconds step) { + const auto dt = static_cast(step.count()) / 1000.0; + while (!st.stop_requested()) { + this->step(dt); + std::this_thread::sleep_for(step); + } +} + +void SimulationEngine::step(double dt) { + mission_manager_.update(world_, planner_, telemetry_); + + auto snapshot = world_.snapshot(); + + for (auto& drone : world_.drones()) { + if (!drone.active || drone.battery <= 0.0) { + drone.active = false; + continue; + } + + Mission* mission = nullptr; + for (auto& m : world_.missions()) { + if (m.drone_id == drone.id && !m.complete) { + mission = &m; + break; + } + } + if (!mission) { + drone.velocity = {0.0, 0.0}; + drone.active = false; + continue; + } + + if (planned_paths_.contains(drone.id) && !planned_paths_[drone.id].empty() && + distance(planned_paths_[drone.id].back(), mission->goal) > 0.8) { + planned_paths_.erase(drone.id); + path_index_.erase(drone.id); + } + + if (!planned_paths_.contains(drone.id) || path_index_[drone.id] >= planned_paths_[drone.id].size()) { + if (auto path = planner_.plan(snapshot, drone.position, mission->goal); path.has_value()) { + planned_paths_[drone.id] = *path; + path_index_[drone.id] = 1; + } + } + + Vec2 target = mission->goal; + if (planned_paths_.contains(drone.id) && path_index_[drone.id] < planned_paths_[drone.id].size()) { + target = planned_paths_[drone.id][path_index_[drone.id]]; + if (distance(drone.position, target) < 0.8) { + path_index_[drone.id] = std::min(path_index_[drone.id] + 1, planned_paths_[drone.id].size()); + } + } + + const Vec2 desired_direction = normalize(target - drone.position); + const Vec2 desired_velocity = desired_direction * 3.0; + const Vec2 safe_v = avoidance_.safe_velocity(drone, snapshot.drones, snapshot.obstacles, desired_velocity, dt); + + const Vec2 accel = (safe_v - drone.velocity); + const double accel_norm = accel.norm(); + const double accel_scale = accel_norm > drone.max_acceleration ? drone.max_acceleration / accel_norm : 1.0; + drone.velocity = drone.velocity + accel * accel_scale * dt; + const Vec2 new_position = drone.position + drone.velocity * dt; + + if (!world_.is_collision(new_position, 0.4)) { + metrics_[drone.id].path_length += distance(drone.position, new_position); + drone.position = new_position; + } + + drone.heading = std::atan2(drone.velocity.y, drone.velocity.x); + const double consumed = 0.005 + 0.01 * drone.velocity.norm(); + drone.battery = std::max(0.0, drone.battery - consumed); + metrics_[drone.id].battery_used += consumed; + } + + world_.advance_time(dt); + auto post = world_.snapshot(); + for (std::size_t i = 0; i < post.drones.size(); ++i) { + for (std::size_t j = i + 1; j < post.drones.size(); ++j) { + if (!post.drones[i].active || !post.drones[j].active) { + continue; + } + if (distance(post.drones[i].position, post.drones[j].position) < 1.0) { + collisions_.fetch_add(1); + telemetry_.record(CollisionEvent{post.drones[i].id, post.drones[j].id, post.sim_time}); + } + } + } + + telemetry_.record(TickEvent{post.sim_time, post.drones}); +} + +} // namespace aerostack diff --git a/src/telemetry/telemetry.cpp b/src/telemetry/telemetry.cpp new file mode 100644 index 0000000..173fc63 --- /dev/null +++ b/src/telemetry/telemetry.cpp @@ -0,0 +1,40 @@ +#include "aerostack/telemetry.hpp" + +#include +#include + +namespace aerostack { + +void TelemetryRecorder::record(TelemetryEvent event) { events_.push_back(std::move(event)); } + +void TelemetryRecorder::write_jsonl(const std::filesystem::path& output) const { + std::ofstream out(output); + for (const auto& event : events_) { + std::visit( + [&out](const auto& e) { + using T = std::decay_t; + if constexpr (std::is_same_v) { + out << "{\"type\":\"tick\",\"sim_time\":" << e.sim_time << ",\"drones\":["; + for (std::size_t i = 0; i < e.drones.size(); ++i) { + const auto& drone = e.drones[i]; + if (i != 0) { + out << ","; + } + out << "{\"id\":" << drone.id << ",\"position\":[" << drone.position.x << "," << drone.position.y + << "],\"velocity\":[" << drone.velocity.x << "," << drone.velocity.y << "],\"battery\":" + << drone.battery << ",\"active\":" << (drone.active ? "true" : "false") << "}"; + } + out << "]}\n"; + } else if constexpr (std::is_same_v) { + out << "{\"type\":\"replan\",\"drone_id\":" << e.drone_id << ",\"goal\":[" << e.new_goal.x << "," + << e.new_goal.y << "]}\n"; + } else if constexpr (std::is_same_v) { + out << "{\"type\":\"collision\",\"a\":" << e.a << ",\"b\":" << e.b << ",\"sim_time\":" << e.sim_time + << "}\n"; + } + }, + event); + } +} + +} // namespace aerostack diff --git a/src/world/world.cpp b/src/world/world.cpp new file mode 100644 index 0000000..3ef145c --- /dev/null +++ b/src/world/world.cpp @@ -0,0 +1,23 @@ +#include "aerostack/world.hpp" + +namespace aerostack { + +World::World(double width, double height) : width_(width), height_(height) {} + +bool World::is_inside(const Vec2& p) const { return p.x >= 0 && p.y >= 0 && p.x <= width_ && p.y <= height_; } + +bool World::is_collision(const Vec2& p, double clearance) const { + if (!is_inside(p)) { + return true; + } + for (const auto& obstacle : obstacles_) { + if (distance(p, obstacle.center) <= obstacle.radius + clearance) { + return true; + } + } + return false; +} + +WorldSnapshot World::snapshot() const { return {width_, height_, obstacles_, drones_, missions_, sim_time_}; } + +} // namespace aerostack diff --git a/tests/test_core.cpp b/tests/test_core.cpp new file mode 100644 index 0000000..9a3801d --- /dev/null +++ b/tests/test_core.cpp @@ -0,0 +1,26 @@ +#include +#include + +#include "aerostack/control.hpp" +#include "aerostack/planning.hpp" +#include "aerostack/world.hpp" + +int main() { + aerostack::World world(20.0, 20.0); + world.obstacles().push_back({{10.0, 10.0}, 2.0}); + + const auto snap = world.snapshot(); + aerostack::AStarPlanner planner(1.0); + const auto path = planner.plan(snap, {1.0, 1.0}, {18.0, 18.0}); + assert(path.has_value()); + assert(path->size() > 5); + + aerostack::PredictiveAvoidance avoidance; + aerostack::DroneState self{0, {5.0, 5.0}, {0.0, 0.0}}; + std::vector others = {{1, {7.0, 5.0}, {-1.0, 0.0}}}; + const auto safe = avoidance.safe_velocity(self, others, world.obstacles(), {1.0, 0.0}, 0.1); + assert(!(safe.x == 1.0 && safe.y == 0.0)); + + std::cout << "All core tests passed\n"; + return 0; +}