From f140b55a63582bd68aa3df4ee2f9d3f9cc4d2193 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Tue, 3 Mar 2026 18:53:19 -0800 Subject: [PATCH] obstacle respected now --- aerpaw/controller.coderprj | 85 +++++++++++-------- aerpaw/guidance_step.m | 2 +- aerpaw/results/plotGpsCsvs.m | 13 ++- .../yqgj8VLvTZ-pn09SsztajkpKW1Ed.xml | 2 + .../yqgj8VLvTZ-pn09SsztajkpKW1Ep.xml | 2 + .../68s6SYUbIV2lonVX55ZsTcaPe_4d.xml | 2 + .../68s6SYUbIV2lonVX55ZsTcaPe_4p.xml | 2 + 7 files changed, 69 insertions(+), 39 deletions(-) create mode 100644 resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ed.xml create mode 100644 resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ep.xml create mode 100644 resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4d.xml create mode 100644 resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4p.xml diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj index f0b9274..cf8f232 100644 --- a/aerpaw/controller.coderprj +++ b/aerpaw/controller.coderprj @@ -128,6 +128,11 @@ + + int32 + + + @@ -873,221 +878,227 @@ - /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp GENERATED_SOURCE - /home/kdee/matlab/R2025a/extern/include/tmwtypes.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp + /home/kdee/matlab/R2025a/extern/include/tmwtypes.h GENERATED_SOURCE + + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp + + GENERATED_SOURCE + + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h @@ -1095,7 +1106,7 @@ true - 2026-03-03T15:54:20 + 2026-03-03T18:47:15 diff --git a/aerpaw/guidance_step.m b/aerpaw/guidance_step.m index c01a794..bd8df89 100644 --- a/aerpaw/guidance_step.m +++ b/aerpaw/guidance_step.m @@ -135,7 +135,7 @@ if isInit % --- Build obstacle list from flat arrays --- coder.varsize('obstacleList', [8, 1], [1, 0]); - obstacleList = cell(int32(0), 1); + obstacleList = repmat({rectangularPrism}, numObstacles, 1); for ii = 1:numObstacles obs = rectangularPrism; obs = obs.initialize([obstacleMin(ii, :); obstacleMax(ii, :)], ... diff --git a/aerpaw/results/plotGpsCsvs.m b/aerpaw/results/plotGpsCsvs.m index d040570..e6581a8 100644 --- a/aerpaw/results/plotGpsCsvs.m +++ b/aerpaw/results/plotGpsCsvs.m @@ -20,7 +20,7 @@ fclose(fID); lla0 = [str2double(yaml((strfind(yaml, 'lat:') + 4):(strfind(yaml, 'lon:') - 1))), str2double(yaml((strfind(yaml, 'lon:') + 4):(strfind(yaml, 'alt:') - 1))), seaToGroundLevel]; % Paths to logs -gpsCsvs = dir(fullfile("sandbox", "test10", "*.csv")); +gpsCsvs = dir(fullfile("sandbox", "test12", "*.csv")); G = cell(size(gpsCsvs)); for ii = 1:size(gpsCsvs, 1) @@ -64,5 +64,16 @@ objectivePos = [params.objectivePos, 0]; llaObj = enu2lla(objectivePos, lla0, 'flat'); geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), domain(2, 3)], 'LineWidth', 3, "Color", 'y'); +% Plot obstacles +for ii = 1:params.numObstacles + obstacle = enu2lla([params.obstacleMin((1 + (ii - 1) * 3):(ii * 3)); params.obstacleMax((1 + (ii - 1) * 3):(ii * 3))], lla0, 'flat'); + geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r'); + geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r'); + geoplot3(gf, [obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r'); + geoplot3(gf, [obstacle(2, 1), obstacle(2, 1)], [obstacle(1, 2), obstacle(1, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r'); + geoplot3(gf, [obstacle(1, 1), obstacle(1, 1)], [obstacle(2, 2), obstacle(2, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r'); + geoplot3(gf, [obstacle(2, 1), obstacle(2, 1)], [obstacle(2, 2), obstacle(2, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r'); +end + % finish hold(gf, "off"); \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ed.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ed.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ed.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ep.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ep.xml new file mode 100644 index 0000000..49b762c --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ep.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4d.xml b/resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4d.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4p.xml b/resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4p.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file