diff --git a/example_scripts/pysimulation_quick_demo_version_w_python_interface.py b/example_scripts/pysimulation_quick_demo_version_w_python_interface.py new file mode 100644 index 000000000..9517791ce --- /dev/null +++ b/example_scripts/pysimulation_quick_demo_version_w_python_interface.py @@ -0,0 +1,79 @@ +import os +import pyhelios +import time +import numpy as np +from pyhelios.survey import Survey +from pyhelios.primitives import SimulationCycleCallback +cycleMeasurementsCount = 0 +cp1 = [] +cpn = [0, 0, 0] + + +# --- C A L L B A C K --- # +# ------------------------- # +def callback(output=None): + with pyhelios.PYHELIOS_SIMULATION_BUILD_CONDITION_VARIABLE: + global cycleMeasurementsCount + global cp1 + global cpn + measurements = output[0] + + # Set 1st cycle point + if cycleMeasurementsCount == 0 and len(measurements) > 0: + pos = measurements[0].position + cp1.append(pos[0]) + cp1.append(pos[1]) + cp1.append(pos[2]) + + # Update cycle measurement count + cycleMeasurementsCount += len(measurements) + + # Update last cycle point + if len(measurements) > 0: + pos = measurements[len(measurements)-1].position + cpn[0] = pos[0] + cpn[1] = pos[1] + cpn[2] = pos[2] + + # Notify for conditional variable + pyhelios.PYHELIOS_SIMULATION_BUILD_CONDITION_VARIABLE.notify() + + +# --- M A I N --- # +# ----------------- # +if __name__ == '__main__': + # Configure simulation context + pyhelios.logging_verbose() + pyhelios.default_rand_generator_seed("123") + + # Build a simulation + survey = Survey.from_xml('data/surveys/toyblocks/als_toyblocks.xml') + survey.las_output = False + survey.zip_output = False + survey.final_output = True + survey.write_waveform = True + survey.calc_echowidth = True + survey.fullwavenoise = False + survey.is_platform_noise_disabled = True + + + simulation_callback = SimulationCycleCallback() + survey.callback = simulation_callback + survey.output_path = 'output/' + survey.run(num_threads=0, callback_frequency=10, export_to_file=False) + # survey.resume() + output = survey.join_output() + + measurements = output[0] + trajectories = output[1] + + p1Pos = measurements[0].position + pnPos = measurements[len(measurements)-1].position + + # PyHelios Tools + npMeasurements, npTrajectories = pyhelios.outputToNumpy(output) + + coords = npMeasurements[:, 0:3] + spher = pyhelios.cartesianToSpherical(coords) + cart = pyhelios.sphericalToCartesian(spher) + diff --git a/pytests/test_pyhelios.py b/pytests/test_pyhelios.py index eb3112622..8941a5eb0 100644 --- a/pytests/test_pyhelios.py +++ b/pytests/test_pyhelios.py @@ -300,7 +300,7 @@ def test_material(test_sim): assert mat0.specularity == 0.0 assert mat0.specular_exponent == 0.0 assert mat0.classification == 0 - assert np.isclose(mat0.kd[0], 0.20, atol=1e-2) + assert np.isclose(mat0.diffuse_components[0], 0.20, atol=1e-2) def test_scanner(test_sim): diff --git a/python/helios/helios_python.cpp b/python/helios/helios_python.cpp old mode 100755 new mode 100644 index 902e4288a..ccf5f2910 --- a/python/helios/helios_python.cpp +++ b/python/helios/helios_python.cpp @@ -16,11 +16,18 @@ #include #include #include +#include +#include +#include +#include +#include #include #include +#include #include #include + bool logging::LOGGING_SHOW_TRACE, logging::LOGGING_SHOW_DEBUG, logging::LOGGING_SHOW_INFO, logging::LOGGING_SHOW_TIME, logging::LOGGING_SHOW_WARN, logging::LOGGING_SHOW_ERR; @@ -30,13 +37,16 @@ namespace py = pybind11; using VectorString = std::vector; -// Declare opaque types first -PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); #include +#include +#include +#include +#include +#include #include #include #include @@ -50,6 +60,9 @@ PYBIND11_MAKE_OPAQUE(std::vector); #include #include #include +#include +#include +#include #include #include @@ -58,12 +71,42 @@ PYBIND11_MAKE_OPAQUE(std::vector); #include #include #include +#include #include +#include +#include #include #include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + #include +#include #include +#include #include #include #include @@ -74,8 +117,12 @@ PYBIND11_MAKE_OPAQUE(std::vector); #include #include #include +#include +#include +#include +#include - +using helios::filems::FMSFacadeFactory; namespace pyhelios{ @@ -83,9 +130,9 @@ namespace pyhelios{ m.doc() = "Helios python bindings"; py::bind_vector>(m, "StringVector"); - py::bind_vector>(m, "MeasurementVector"); - py::bind_vector>(m, "TrajectoryVector"); - py::bind_vector>(m, "DoubleVector"); + py::bind_vector>(m, "MeasurementList"); + py::bind_vector>(m, "TrajectoryList"); + py::implicitly_convertible(); logging::makeQuiet(); @@ -95,7 +142,9 @@ namespace pyhelios{ // Enable GDAL (Load its drivers) GDALAllRegister(); - + if (GDALGetDriverCount() == 0) { + throw std::runtime_error("GDAL failed to initialize properly."); + } // Definitions m.def("logging_quiet", &logging::makeQuiet, "Set the logging verbosity level to quiet"); @@ -107,13 +156,12 @@ namespace pyhelios{ m.def("default_rand_generator_seed", &setDefaultRandomnessGeneratorSeed, "Set the seed for the default randomness generator"); - py::class_> aabb(m, "AABB"); - aabb - .def_static("create", []() { return std::make_shared(); }, py::return_value_policy::take_ownership) - - .def_property_readonly("min_vertex", [](AABB &aabb) { return &(aabb.vertices[0]); }, py::return_value_policy::reference) - .def_property_readonly("max_vertex", [](AABB &aabb) { return &(aabb.vertices[1]); }, py::return_value_policy::reference) - .def("__str__", &AABB::toString); + + py::class_> asset(m, "Asset"); + asset + .def_readwrite("id", &Asset::id) + .def_readwrite("name", &Asset::name); + py::class_> abstract_beam_deflector(m, "AbstractBeamDeflector"); abstract_beam_deflector @@ -131,20 +179,59 @@ namespace pyhelios{ .def_readwrite("cached_angle_between_pulses", &AbstractBeamDeflector::cached_angleBetweenPulses_rad) .def_property_readonly("emitter_relative_attitude", &AbstractBeamDeflector::getEmitterRelativeAttitudeByReference) - .def_property_readonly("optics_type", &AbstractBeamDeflector::getOpticsType); + .def_property_readonly("optics_type", &AbstractBeamDeflector::getOpticsType) + .def("clone", &AbstractBeamDeflector::clone); + py::class_> oscillating_mirror_beam_deflector(m, "OscillatingMirrorBeamDeflector"); + oscillating_mirror_beam_deflector + .def(py::init(), + py::arg("scanAngleMax_rad"), py::arg("scanFreqMax_Hz"), py::arg("scanFreqMin_Hz"), py::arg("scanProduct")) + + .def_readwrite("scan_product", &OscillatingMirrorBeamDeflector::cfg_device_scanProduct) + .def("clone", &OscillatingMirrorBeamDeflector::clone); + + py::class_> conic_beam_deflector(m, "ConicBeamDeflector"); + conic_beam_deflector + .def(py::init(), + py::arg("scanAngleMax_rad"), py::arg("scanFreqMax_Hz"), py::arg("scanFreqMin_Hz")) + .def("clone", &ConicBeamDeflector::clone); + + py::class_> fiber_array_beam_deflector(m, "FiberArrayBeamDeflector"); + fiber_array_beam_deflector + .def(py::init(), + py::arg("scanAngleMax_rad"), py::arg("scanFreqMax_Hz"), py::arg("scanFreqMin_Hz"), py::arg("numFibers")) + .def_property("num_fibers", &FiberArrayBeamDeflector::getNumFibers, &FiberArrayBeamDeflector::setNumFibers) + .def("clone", &FiberArrayBeamDeflector::clone); + + + py::class_> polygon_mirror_beam_deflector(m, "PolygonMirrorBeamDeflector"); + polygon_mirror_beam_deflector + .def(py::init(), + py::arg("scanAngleMax_rad"), py::arg("scanFreqMax_Hz"), py::arg("scanFreqMin_Hz"), py::arg("ScanAngleEffectiveMax_rad")) + .def_property_readonly("scan_angle_effective_max", &PolygonMirrorBeamDeflector::getScanAngleEffectiveMax_rad) + .def("clone", &PolygonMirrorBeamDeflector::clone); + + + py::class_> risley_beam_deflector(m, "RisleyBeamDeflector"); + risley_beam_deflector + .def(py::init(), + py::arg("scanAngleMax_rad"), py::arg("rotorFreq_1_Hz"), py::arg("rotorFreq_2_Hz")) + .def_readwrite("rotor_speed_rad_1", &RisleyBeamDeflector::rotorSpeed_rad_1) + .def_readwrite("rotor_speed_rad_2", &RisleyBeamDeflector::rotorSpeed_rad_2) + .def("clone", &RisleyBeamDeflector::clone); + + py::class_> primitive(m, "Primitive"); primitive .def(py::init<>()) - .def_property_readonly("scene_part", [](Primitive &prim) { - return prim.part.get(); }) - .def_property_readonly("material", [](Primitive &prim) { - return prim.material.get();}) - .def_property_readonly("AABB", &Primitive::getAABB) - .def_property_readonly("centroid", &Primitive::getCentroid) - .def_property_readonly("num_vertices", &Primitive::getNumVertices) - .def_property_readonly("vertices", [](Primitive &prim, size_t index) { - return &prim.getVertices()[index]; + + .def_property("scene_part", [](Primitive &prim) { + return prim.part.get();}, [](Primitive &prim, std::shared_ptr part) { + prim.part = part; + }) + .def_property("material", [](Primitive &prim) { + return prim.material.get();}, [](Primitive &prim, std::shared_ptr material) { + prim.material = material; }) .def("incidence_angle", @@ -158,7 +245,6 @@ namespace pyhelios{ .def("ray_intersection_distance", [](Primitive &prim, const glm::dvec3& rayOrigin, const glm::dvec3& rayDir) { return prim.getRayIntersectionDistance(rayOrigin, rayDir); }) - .def("update", &Primitive::update) .def("is_triangle", [](Primitive &prim) { return dynamic_cast(&prim) != nullptr; @@ -174,6 +260,38 @@ namespace pyhelios{ }); + py::class_> aabb(m, "AABB"); + aabb + + .def(py::init<>()) // Default constructor + .def(py::init(), "Construct AABB from min and max vertices") + .def_property("vertices", + [](const AABB& aabb) { + return std::vector(aabb.vertices, aabb.vertices + 2); + }, + [](AABB& aabb, const std::vector& vertices) { + if (vertices.size() != 2) { + throw std::runtime_error("Vertices array must have exactly 2 elements."); + } + std::copy(vertices.begin(), vertices.end(), aabb.vertices); + }, + "Get and set the vertices of the AABB") + .def_property("bounds", + [](const AABB& aabb) { + return std::vector(aabb.bounds, aabb.bounds + 2); + }, + [](AABB& aabb, const std::vector& bounds) { + if (bounds.size() != 2) { + throw std::runtime_error("Bounds array must have exactly 2 elements."); + } + std::copy(bounds.begin(), bounds.end(), aabb.bounds); + }, + "Get and set the cached bounds of the AABB") + .def_property_readonly("min_vertex", [](AABB &aabb) { return &(aabb.vertices[0]); }, py::return_value_policy::reference) + .def_property_readonly("max_vertex", [](AABB &aabb) { return &(aabb.vertices[1]); }, py::return_value_policy::reference) + .def("__str__", &AABB::toString); + + py::class_, Primitive> detailed_voxel(m, "DetailedVoxel"); detailed_voxel .def(py::init<>()) @@ -208,25 +326,62 @@ namespace pyhelios{ .def_property("las_scale", [](AbstractDetector &self) { return self.getFMS()->write.getMeasurementWriterLasScale();}, [](AbstractDetector &self, double lasScale) { - self.getFMS()->write.setMeasurementWriterLasScale(lasScale);}); + self.getFMS()->write.setMeasurementWriterLasScale(lasScale);}) + + .def("clone", &AbstractDetector::clone); + + + py::class_> full_waveform_pulse_detector(m, "FullWaveformPulseDetector"); + full_waveform_pulse_detector + .def(py::init< + std::shared_ptr, + double, + double, + double + >(), + py::arg("scanner"), + py::arg("accuracy_m"), + py::arg("rangeMin_m"), + py::arg("rangeMax_m") = std::numeric_limits::max()) + + .def("clone", &FullWaveformPulseDetector::clone); + py::class_, Primitive> triangle(m, "Triangle"); triangle .def(py::init(), py::arg("v0"), py::arg("v1"), py::arg("v2")) .def("__str__", &Triangle::toString) .def("ray_intersection", &Triangle::getRayIntersection) + .def_property("vertices", + [](const Triangle& tri) -> std::vector { + return {tri.verts[0], tri.verts[1], tri.verts[2]}; + }, + [](Triangle& tri, const std::vector& vertices) { + if (vertices.size() != 3) { + throw std::runtime_error("Vertices array must have exactly 3 elements."); + } + std::copy(vertices.begin(), vertices.end(), tri.verts); + }, + "Get and set the vertices of the triangle") + .def_property_readonly("face_normal", &Triangle::getFaceNormal); - py::class_ vertex(m, "Vertex"); + + py::class_> vertex(m, "Vertex"); vertex .def(py::init<>()) .def(py::init(), py::arg("x"), py::arg("y"), py::arg("z")) - .def_property_readonly("position", [](const Vertex &v) { return glm::dvec3(v.pos); }) - .def_property_readonly("normal", [](const Vertex &v) { return glm::dvec3(v.normal); }); + + .def_readwrite("position", &Vertex::pos) + .def_readwrite("normal", &Vertex::normal) + .def_readwrite("tex_coords", &Vertex::texcoords) + .def("clone", &Vertex::copy); + py::class_(m, "PyHeliosException") .def(py::init(), py::arg("msg")=""); + py::class_> trajectory(m, "Trajectory"); trajectory .def(py::init<>()) @@ -237,6 +392,15 @@ namespace pyhelios{ .def_readwrite("pitch", &Trajectory::pitch) .def_readwrite("yaw", &Trajectory::yaw); + + py::class_> trajectory_settings(m, "TrajectorySettings"); + trajectory_settings + .def(py::init<>()) + .def_readwrite("start_time", &TrajectorySettings::tStart) + .def_readwrite("end_time", &TrajectorySettings::tEnd) + .def_readwrite("teleport_to_start", &TrajectorySettings::teleportToStart); + + py::class_> measurement(m, "Measurement"); measurement .def(py::init<>()) @@ -256,6 +420,15 @@ namespace pyhelios{ .def_readwrite("gps_time", &Measurement::gpsTime); + py::class_> randomness_generator(m, "RandomnessGenerator"); + randomness_generator + .def(py::init<>()) + .def("compute_uniform_real_distribution", &RandomnessGenerator::computeUniformRealDistribution) + .def("uniform_real_distribution_next", &RandomnessGenerator::uniformRealDistributionNext) + .def("compute_normal_distribution", &RandomnessGenerator::computeNormalDistribution) + .def("normal_distribution_next", &RandomnessGenerator::normalDistributionNext); + + py::class_, NoiseSourceWrap> noise_source(m, "NoiseSource"); noise_source .def(py::init<>()) @@ -280,15 +453,36 @@ namespace pyhelios{ &NoiseSource::setFixedValueRemainingUses) .def("next", &NoiseSource::next); - py::class_> randomness_generator(m, "RandomnessGenerator"); - randomness_generator - .def(py::init<>()) - .def("compute_uniform_real_distribution", &RandomnessGenerator::computeUniformRealDistribution) - .def("uniform_real_distribution_next", &RandomnessGenerator::uniformRealDistributionNext) - .def("compute_normal_distribution", &RandomnessGenerator::computeNormalDistribution) - .def("normal_distribution_next", &RandomnessGenerator::normalDistributionNext); + + py::class_, NoiseSource, RandomNoiseSourceWrap>(m, "RandomNoiseSource") + .def(py::init<>()) + + .def("get_random_noise_type", &RandomNoiseSource::getRandomNoiseType) + .def("__str__", [](RandomNoiseSource &self) { + std::ostringstream oss; + oss << self; + return oss.str(); + }); + - py::class_ ray_scene_intersection(m, "RaySceneIntersection"); + py::class_, RandomNoiseSource>(m, "UniformNoiseSource") + .def(py::init(), py::arg("min") = 0.0, py::arg("max") = 1.0) + + .def_property("min", &UniformNoiseSource::getMin, &UniformNoiseSource::setMin) + .def_property("max", &UniformNoiseSource::getMax, &UniformNoiseSource::setMax) + + .def("configure_uniform_noise", &UniformNoiseSource::configureUniformNoise) + .def("noise_function", &UniformNoiseSource::noiseFunction) + .def("get_random_noise_type", &UniformNoiseSource::getRandomNoiseType) + .def("next", &UniformNoiseSource::next) + .def("__str__", [](const UniformNoiseSource &ns) { + std::ostringstream oss; + oss << ns; + return oss.str(); + }); + + + py::class_> ray_scene_intersection(m, "RaySceneIntersection"); ray_scene_intersection .def(py::init<>()) .def(py::init()) @@ -320,16 +514,14 @@ namespace pyhelios{ .def("has", py::overload_cast(&ScanningStrip::has)); - py::class_ simulation_cycle_callback(m, "SimulationCycleCallback"); + py::class_> simulation_cycle_callback(m, "SimulationCycleCallback"); simulation_cycle_callback - .def(py::init()) + .def(py::init<>()) .def("__call__", [](SimulationCycleCallback &callback, py::list measurements, py::list trajectories, const std::string &outpath) { py::gil_scoped_acquire acquire; - - // Convert Python lists to std::shared_ptr> and std::shared_ptr> auto measurements_vec = std::make_shared>(); for (auto item : measurements) { measurements_vec->push_back(item.cast()); @@ -339,51 +531,11 @@ namespace pyhelios{ for (auto item : trajectories) { trajectories_vec->push_back(item.cast()); } - callback(*measurements_vec, *trajectories_vec, outpath); }); - py::class_>(m, "IntegerList") - .def(py::init<>()) - .def("__len__", &std::list::size) - .def("__getitem__", [](const std::list &list, long index) { - size_t idx = pybind11::handlePythonIndex(index, list.size()); // REWRITE TO HELIOS NAMESPACES - auto it = list.begin(); - std::advance(it, idx); - return *it; - }) - .def("__setitem__", [](std::list &list, long index, int value) { - size_t idx = pybind11::handlePythonIndex(index, list.size()); - auto it = list.begin(); - std::advance(it, idx); - *it = value; - }) - .def("insert", [](std::list &list, long index, int value) { - size_t idx = pybind11::handlePythonIndex(index, list.size()); - auto it = list.begin(); - std::advance(it, idx); - list.insert(it, value); - }) - .def("erase", [](std::list &list, long index) { - size_t idx = pybind11::handlePythonIndex(index, list.size()); - auto it = list.begin(); - std::advance(it, idx); - list.erase(it); - }) - .def("__repr__", [](const std::list &list) { - std::ostringstream ss; - ss << "["; - bool first = true; - for (const auto &item : list) { - if (!first) ss << ", "; - ss << item; - first = false; - } - ss << "]"; - return ss.str(); - }); - py::class_ fwf_settings(m, "FWFSettings"); + py::class_> fwf_settings(m, "FWFSettings"); fwf_settings .def(py::init<>()) .def(py::init(), py::arg("fwfSettings")) @@ -401,7 +553,8 @@ namespace pyhelios{ .def_readwrite("max_fullwave_range", &FWFSettings::maxFullwaveRange_ns) .def("__str__", &FWFSettings::toString); - py::class_ rotation(m, "Rotation"); + + py::class_> rotation(m, "Rotation"); rotation .def(py::init<>()) .def(py::init(), py::arg("q0"), py::arg("q1"), py::arg("q2"), py::arg("q3"), py::arg("needsNormalization")) @@ -415,9 +568,12 @@ namespace pyhelios{ .def_property_readonly("axis", &Rotation::getAxis) .def_property_readonly("angle", &Rotation::getAngle); - py::class_ scanner_head(m, "ScannerHead"); + + py::class_> scanner_head(m, "ScannerHead"); scanner_head .def(py::init(), py::arg("headRotationAxis"), py::arg("headRotatePerSecMax_rad")) + + .def_readwrite("rotation_axis", &ScannerHead::cfg_device_rotateAxis) .def_property_readonly("mount_relative_attitude", &ScannerHead::getMountRelativeAttitudeByReference) .def_property("rotate_per_sec_max", @@ -439,8 +595,14 @@ namespace pyhelios{ .def_property("current_rotate_angle", &ScannerHead::getRotateCurrent, &ScannerHead::setCurrentRotateAngle_rad); - - py::class_ material(m, "Material"); + + + py::class_> eval_scanner_head(m, "EvalScannerHead"); + eval_scanner_head + .def(py::init(), py::arg("headRotationAxis"), py::arg("headRotatePerSecMax_rad")); + + + py::class_> material(m, "Material"); material .def(py::init<>()) .def(py::init(), py::arg("material")) @@ -449,22 +611,50 @@ namespace pyhelios{ .def_readwrite("is_ground", &Material::isGround) .def_readwrite("use_vertex_colors", &Material::useVertexColors) .def_readwrite("mat_file_path", &Material::matFilePath) - .def_readwrite("map_Kd", &Material::map_Kd) .def_readwrite("reflectance", &Material::reflectance) .def_readwrite("specularity", &Material::specularity) .def_readwrite("specular_exponent", &Material::specularExponent) .def_readwrite("classification", &Material::classification) .def_readwrite("spectra", &Material::spectra) - .def_property_readonly("ka", [](const Material &mat) { return create_numpy_array(mat.ka); }) - .def_property_readonly("kd", [](const Material &mat) { return create_numpy_array(mat.kd); }) - .def_property_readonly("ks", [](const Material &mat) { return create_numpy_array(mat.ks); }); - - - py::class_> survey(m, "Survey", py::module_local()); + .def_readwrite("map_kd", &Material::map_Kd) + .def_property("ambient_components", + [](Material &m) { + return std::vector(std::begin(m.ka), std::end(m.ka)); + }, + [](Material &m, const std::vector &value) { + if (value.size() != 4) { + throw std::runtime_error("ambient_components must have exactly 4 elements"); + } + std::copy(value.begin(), value.end(), m.ka); + }) + .def_property("diffuse_components", + [](Material &m) { + return std::vector(std::begin(m.kd), std::end(m.kd)); + }, + [](Material &m, const std::vector &value) { + if (value.size() != 4) { + throw std::runtime_error("diffuse_components must have exactly 4 elements"); + } + std::copy(value.begin(), value.end(), m.kd); + }) + .def_property("specular_components", + [](Material &m) { + return std::vector(std::begin(m.ks), std::end(m.ks)); + }, + [](Material &m, const std::vector &value) { + if (value.size() != 4) { + throw std::runtime_error("specular_components must have exactly 4 elements"); + } + std::copy(value.begin(), value.end(), m.ks); + }); + + + py::class_> survey(m, "Survey", py::module_local()); survey .def(py::init<>()) .def_readwrite("scanner", &Survey::scanner) + .def_readwrite("legs", &Survey::legs) .def("calculate_length", &Survey::calculateLength) .def_property_readonly("length", &Survey::getLength) .def_property("name", @@ -478,7 +668,7 @@ namespace pyhelios{ [](Survey &s, double simSpeedFactor) { s.simSpeedFactor = simSpeedFactor; }); - py::class_ leg(m, "Leg"); + py::class_> leg(m, "Leg"); leg .def(py::init<>()) .def(py::init>(), @@ -486,12 +676,14 @@ namespace pyhelios{ .def(py::init(), py::arg("leg")) .def_readwrite("scanner_settings", &Leg::mScannerSettings) .def_readwrite("platform_settings", &Leg::mPlatformSettings) + .def_readwrite("trajectory_settings", &Leg::mTrajectorySettings) .def_property("length", &Leg::getLength, &Leg::setLength) .def_property("serial_id", &Leg::getSerialId, &Leg::setSerialId) .def_property("strip", &Leg::getStrip, &Leg::setStrip) .def("belongs_to_strip", &Leg::isContainedInAStrip); + py::class_> scene_part(m, "ScenePart"); scene_part .def(py::init<>()) @@ -502,6 +694,7 @@ namespace pyhelios{ .def_readwrite("rotation", &ScenePart::mRotation) .def_readwrite("scale", &ScenePart::mScale) .def_readwrite("bound", &ScenePart::bound) + .def_readwrite("is_force_on_ground", &ScenePart::forceOnGround) .def_property("centroid", &ScenePart::getCentroid, &ScenePart::setCentroid) .def_property("id", &ScenePart::getId, &ScenePart::setId) @@ -565,6 +758,7 @@ namespace pyhelios{ .value("VOXEL", ScenePart::VOXEL) .export_values(); + py::class_> scanner_settings(m, "ScannerSettings"); scanner_settings .def(py::init<>()) @@ -613,22 +807,25 @@ namespace pyhelios{ .def_readwrite("speed_m_s", &PlatformSettings::movePerSec_m) .def_property_readonly("base_template", &PlatformSettings::getTemplate, py::return_value_policy::reference) - .def_property("position", &PlatformSettings::getPosition, [](PlatformSettings& self, const glm::dvec3& pos) { self.setPosition(pos); }) //&PlatformSettings::setPosition) + .def_property("position", &PlatformSettings::getPosition, [](PlatformSettings& self, const glm::dvec3& pos) { self.setPosition(pos); }) .def("cherryPick", &PlatformSettings::cherryPick, py::arg("cherries"), py::arg("fields"), py::arg("templateFields") = nullptr) .def("has_template", &PlatformSettings::hasTemplate) .def("to_string", &PlatformSettings::toString); - py::class_ scene(m, "Scene"); + + py::class_> scene(m, "Scene"); scene .def(py::init<>()) .def(py::init(), py::arg("scene")) .def_readwrite("scene_parts", &Scene::parts) + .def_readwrite("primitives", &Scene::primitives) .def_property("bbox", &Scene::getBBox, &Scene::setBBox) .def_property("bbox_crs", &Scene::getBBoxCRS, &Scene::setBBoxCRS) + .def_property("kd_grove_factory", &Scene::getKDGroveFactory, &Scene::setKDGroveFactory) .def_property_readonly("num_primitives", [](const ScenePart& self) -> size_t { return self.mPrimitives.size();}) .def_property_readonly("aabb", &Scene::getAABB, py::return_value_policy::reference) @@ -680,13 +877,24 @@ namespace pyhelios{ throw std::out_of_range("Index out of range"); } }, py::return_value_policy::reference) - + .def("build_kd_grove", &Scene::buildKDGroveWithLog, py::arg("safe") = true) .def("intersection_min_max", py::overload_cast const&, glm::dvec3 const&, glm::dvec3 const&, bool const>(&Scene::getIntersection, py::const_), py::arg("tMinMax"), py::arg("rayOrigin"), py::arg("rayDir"), py::arg("groundOnly")); - py::class_ platform(m, "Platform"); + py::class_> static_scene(m, "StaticScene"); + static_scene + .def(py::init<>()) + .def("get_static_object_part", &StaticScene::getStaticObject, py::arg("id")) + .def("set_static_object_part", &StaticScene::setStaticObject, py::arg("id"), py::arg("part")) + .def("append_static_object_part", &StaticScene::appendStaticObject, py::arg("part")) + .def("remove_static_object_part", &StaticScene::removeStaticObject, py::arg("id")) + .def("clear_static_object_parts", &StaticScene::clearStaticObjects) + .def("write_object", &StaticScene::writeObject, py::arg("path")); + + + py::class_> platform(m, "Platform"); platform .def(py::init<>()) .def(py::init(), py::arg("platform")) @@ -694,77 +902,247 @@ namespace pyhelios{ .def_readwrite("last_check_z", &Platform::lastCheckZ) .def_readwrite("dmax", &Platform::dmax) .def_readwrite("is_orientation_on_leg_init", &Platform::mSetOrientationOnLegInit) - .def_readwrite("is_on_ground", &Platform::onGround) .def_readwrite("is_stop_and_turn", &Platform::stopAndTurn) .def_readwrite("settings_speed_m_s", &Platform::cfg_settings_movePerSec_m) .def_readwrite("is_slowdown_enabled", &Platform::slowdownEnabled) .def_readwrite("is_smooth_turn", &Platform::smoothTurn) + .def_readwrite("device_relative_position", &Platform::cfg_device_relativeMountPosition) + .def_readwrite("device_relative_attitude", &Platform::cfg_device_relativeMountAttitude) + .def_readwrite("position_x_noise_source", &Platform::positionXNoiseSource) + .def_readwrite("position_y_noise_source", &Platform::positionYNoiseSource) + .def_readwrite("position_z_noise_source", &Platform::positionZNoiseSource) + .def_readwrite("attitude_x_noise_source", &Platform::attitudeXNoiseSource) + .def_readwrite("attitude_y_noise_source", &Platform::attitudeYNoiseSource) + .def_readwrite("attitude_z_noise_source", &Platform::attitudeZNoiseSource) + .def_readwrite("scene", &Platform::scene) + .def_readwrite("target_waypoint", &Platform::targetWaypoint) + .def_readwrite("origin_waypoint", &Platform::originWaypoint) + .def_readwrite("next_waypoint", &Platform::nextWaypoint) + + .def_property("position", &Platform::getPosition, &Platform::setPosition) + .def_property("attitude", [](Platform &self) { + return self.attitude; + }, [](Platform &self, const Rotation &attitude) { + self.attitude = attitude; + }) + .def_property("absolute_mount_position", [](Platform &self) { + return self.cached_absoluteMountPosition; + }, [](Platform &self, const glm::dvec3 &position) { + self.cached_absoluteMountPosition = position; + }) + .def_property("absolute_mount_attitude", [](Platform &self) { + return self.cached_absoluteMountAttitude; + }, [](Platform &self, const Rotation &attitude) { + self.cached_absoluteMountAttitude = attitude; + }) + .def_property("last_ground_check", [](const Platform &self) { + return self.lastGroundCheck; + }, [](Platform &self, const glm::dvec3 &position) { + self.lastGroundCheck = position; + }) + .def_property("cached_dir_current", [](const Platform &self) { + return self.cached_dir_current; + }, [](Platform &self, const glm::dvec3 &position) { + self.cached_dir_current = position; + }) + .def_property("cached_dir_current_xy", [](const Platform &self) { + return self.cached_dir_current_xy; + }, [](Platform &self, const glm::dvec3 &position) { + self.cached_dir_current_xy = position; + }) + .def_property("cached_vector_to_target", [](const Platform &self) { + return self.cached_vectorToTarget; + }, [](Platform &self, const glm::dvec3 &position) { + self.cached_vectorToTarget = position; + }) + .def_property("cached_vector_to_target_xy", [](const Platform &self) { + return self.cached_vectorToTarget_xy; + }, [](Platform &self, const glm::dvec3 &position) { + self.cached_vectorToTarget_xy = position; + }); + + + py::class_> moving_platform(m, "MovingPlatform"); + moving_platform + + .def(py::init<>()) + + .def_property("velocity", &MovingPlatform::getVelocity, &MovingPlatform::setVelocity) + .def("apply_settings", &MovingPlatform::applySettings) + .def("do_sim_step", &MovingPlatform::doSimStep) + .def("init_leg_manual", &MovingPlatform::initLegManual) + .def("init_leg_manual_interactive", &MovingPlatform::initLegManualIterative) + .def("waypoint_reached", &MovingPlatform::waypointReached) + .def("can_move", &MovingPlatform::canMove) + .def("clone", &MovingPlatform::clone); + + + py::class_> simple_physics_platform(m, "SimplePhysicsPlatform"); + simple_physics_platform + .def(py::init<>()) + .def_readwrite("drag_magnitude", &SimplePhysicsPlatform::mCfg_drag) + + .def("prepare_simulation", &SimplePhysicsPlatform::prepareSimulation) + .def("prepare_leg", &SimplePhysicsPlatform::prepareLeg) + .def("do_physics_step", &SimplePhysicsPlatform::doPhysicsStep) + .def("do_sim_step", &SimplePhysicsPlatform::doSimStep) + .def("do_control_step", &SimplePhysicsPlatform::doControlStep) + .def("configure_step_magnitude", &SimplePhysicsPlatform::configureStepMagnitude) + .def("check_speed_limit", &SimplePhysicsPlatform::checkSpeedLimit) + .def("clone", &SimplePhysicsPlatform::clone); + + + py::class_> ground_vehicle_platform(m, "GroundVehiclePlatform"); + ground_vehicle_platform + .def(py::init<>()) + .def("do_control_step", &GroundVehiclePlatform::doControlStep) + .def("prepare_simulation", &GroundVehiclePlatform::prepareSimulation) + .def("set_destination", &GroundVehiclePlatform::setDestination) + .def("clone", &GroundVehiclePlatform::clone); + + + py::class_> linear_path_platform(m, "LinearPathPlatform"); + linear_path_platform + .def(py::init<>()) + .def("do_sim_step", &LinearPathPlatform::doSimStep) + .def("set_destination", &LinearPathPlatform::setDestination) + .def("clone", &LinearPathPlatform::clone); + + + py::class_> helicopter_platform(m, "HelicopterPlatform"); + helicopter_platform + .def(py::init<>()) + .def_readwrite("slowdown_distance_xy", &HelicopterPlatform::cfg_slowdown_dist_xy) + .def_readwrite("slowdown_magnitude", &HelicopterPlatform::cfg_slowdown_magnitude) + .def_readwrite("speedup_magnitude", &HelicopterPlatform::cfg_speedup_magnitude) + .def_readwrite("max_engine_force_xy", &HelicopterPlatform::ef_xy_max) + .def_readwrite("roll", &HelicopterPlatform::roll) + .def_readwrite("pitch", &HelicopterPlatform::pitch) + .def_readwrite("last_sign", &HelicopterPlatform::lastSign) + .def_readwrite("base_pitch_angle", &HelicopterPlatform::cfg_pitch_base) + .def_readwrite("yaw_speed", &HelicopterPlatform::cfg_yaw_speed) + .def_readwrite("roll_speed", &HelicopterPlatform::cfg_roll_speed) + .def_readwrite("pitch_speed", &HelicopterPlatform::cfg_pitch_speed) + .def_readwrite("max_roll_offset", &HelicopterPlatform::cfg_max_roll_offset) + .def_readwrite("max_pitch_offset", &HelicopterPlatform::cfg_max_pitch_offset) + .def_readwrite("max_pitch", &HelicopterPlatform::cfg_max_pitch) + .def_readwrite("min_pitch", &HelicopterPlatform::cfg_min_pitch) + .def_readwrite("slowdown_factor", &HelicopterPlatform::cfg_slowdownFactor) + .def_readwrite("speedup_factor", &HelicopterPlatform::cfg_speedupFactor) + .def_readwrite("pitch_step_magnitude", &HelicopterPlatform::cfg_pitchStepMagnitude) + .def_readwrite("roll_step_magnitude", &HelicopterPlatform::cfg_rollStepMagnitude) + .def_readwrite("yaw_step_magnitude", &HelicopterPlatform::cfg_yawStepMagnitude) + .def_readwrite("alignment_threshold", &HelicopterPlatform::cfg_alignmentThreshold) + .def_readwrite("speed_xy", &HelicopterPlatform::speed_xy) + .def_readwrite("last_speed_xy", &HelicopterPlatform::lastSpeed_xy) + .def_readwrite("rotation", &HelicopterPlatform::r) + .def_readwrite("dir_attitude", &HelicopterPlatform::dirAttitudeXY) + .def_readwrite("cache_turn_iterations", &HelicopterPlatform::cache_turnIterations) + .def_readwrite("cache_turning", &HelicopterPlatform::cache_turning) + .def_readwrite("cache_aligning", &HelicopterPlatform::cache_aligning) + .def_readwrite("cache_distance_threshold", &HelicopterPlatform::cache_xyDistanceThreshold) + .def_readwrite("cache_speedup_finished", &HelicopterPlatform::cache_speedUpFinished) + + .def_property("heading_rad", &HelicopterPlatform::getHeadingRad, &HelicopterPlatform::setHeadingRad) + .def("do_control_step", &HelicopterPlatform::doControlStep) + .def("compute_lift_sink_rate", &HelicopterPlatform::computeLiftSinkRate) + .def("compute_xy_speed", &HelicopterPlatform::computeXYSpeed) + .def("compute_engine_force", &HelicopterPlatform::computeEngineForce) + .def("compute_rotation_angles", &HelicopterPlatform::computeRotationAngles) + .def("compute_alignment_angles", &HelicopterPlatform::computeAlignmentAngles) + .def("compute_turning_angles", &HelicopterPlatform::computeTurningAngles) + .def("compute_slowdown_step", &HelicopterPlatform::computeSlowdownStep) + .def("compute_speedup_step", &HelicopterPlatform::computeSpeedupStep) + .def("rotate", &HelicopterPlatform::rotate) + .def("handle_route", &HelicopterPlatform::handleRoute) + .def("can_stop_and_turn", &HelicopterPlatform::canStopAndTurn) + .def("prepare_simulation", &HelicopterPlatform::prepareSimulation) + .def("init_leg_manual", &HelicopterPlatform::initLegManual) + .def("init_leg", &HelicopterPlatform::initLeg) + .def("waypoint_reached", &HelicopterPlatform::waypointReached) + .def("update_static_cache", &HelicopterPlatform::updateStaticCache) + .def("compute_turn_distance_threshold", &HelicopterPlatform::computeTurnDistanceThreshold) + .def("compute_non_smooth_slowdown_dist", &HelicopterPlatform::computeNonSmoothSlowdownDist) + .def("clone", &HelicopterPlatform::clone); + + + py::class_> swap_on_repeat_handler(m, "SwapOnRepeatHandler"); + swap_on_repeat_handler + .def(py::init<>()) + + .def_property( + "baseline", + [](SwapOnRepeatHandler &self) { + return self.baseline.get(); + }, + + [](SwapOnRepeatHandler &self, ScenePart *baseline) { + self.baseline.reset(baseline); + }, + py::return_value_policy::take_ownership + ) + + .def_property_readonly("num_target_swaps", &SwapOnRepeatHandler::getNumTargetSwaps) + .def_property_readonly("num_target_replays", &SwapOnRepeatHandler::getNumTargetReplays) + .def_property("keep_crs", &SwapOnRepeatHandler::isKeepCRS, &SwapOnRepeatHandler::setKeepCRS) + .def_property("discard_on_replay", &SwapOnRepeatHandler::needsDiscardOnReplay, &SwapOnRepeatHandler::setDiscardOnReplay) + + .def("swap", &SwapOnRepeatHandler::swap) + .def("prepare", &SwapOnRepeatHandler::prepare) + .def("push_time_to_live", &SwapOnRepeatHandler::pushTimeToLive) + .def("push_swap_filters", &SwapOnRepeatHandler::pushSwapFilters); + - .def_property_readonly("device_relative_position", [](const Platform &self) { - return self.cfg_device_relativeMountPosition;}) - .def_property_readonly("device_relative_attitude", [](const Platform &self) { - return self.cfg_device_relativeMountAttitude;}) - .def_property_readonly("position_x_noise_source", [](const Platform &self) { - return self.positionXNoiseSource;}) - .def_property_readonly("position_y_noise_source", [](const Platform &self) { - return self.positionYNoiseSource;}) - .def_property_readonly("position_z_noise_source", [](const Platform &self) { - return self.positionZNoiseSource;}) - .def_property_readonly("attitude_x_noise_source", [](const Platform &self) { - return self.attitudeXNoiseSource;}) - .def_property_readonly("attitude_y_noise_source", [](const Platform &self) { - return self.attitudeYNoiseSource;}) - .def_property_readonly("attitude_z_noise_source", [](const Platform &self) { - return self.attitudeZNoiseSource;}) - .def_property_readonly("target_waypoint", [](const Platform &self) { - return self.targetWaypoint;}) - .def_property_readonly("last_ground_check", [](const Platform &self) { - return self.lastGroundCheck;}) - - .def_property_readonly("position", &Platform::getPosition) - .def_property_readonly("attitude", &Platform::getAttitude) - .def_property_readonly("absolute_mount_position", &Platform::getAbsoluteMountPosition) - .def_property_readonly("absolute_mount_attitude", &Platform::getAbsoluteMountAttitude) - - .def_property_readonly("cached_dir_current", [](const Platform &self) { - return self.cached_dir_current;}) - .def_property_readonly("cached_dir_current_xy", [](const Platform &self) { - return self.cached_dir_current_xy;}) - .def_property_readonly("cached_vector_to_target", [](const Platform &self) { - return self.cached_vectorToTarget;}) - .def_property_readonly("cached_vector_to_target_xy", [](const Platform &self) { - return self.cached_vectorToTarget_xy;}); - - py::class_ scanning_device(m, "ScanningDevice"); + py::class_> energy_model(m, "EnergyModel"); + energy_model + .def(py::init(), py::arg("device")) + + .def("compute_intensity", &EnergyModel::computeIntensity) + .def("compute_received_power", &EnergyModel::computeReceivedPower) + .def("compute_emitted_power", &EnergyModel::computeEmittedPower) + .def("compute_target_area", &EnergyModel::computeTargetArea) + .def("compute_cross_section", &EnergyModel::computeCrossSection); + + + py::class_> base_energy_model(m, "BaseEnergyModel"); + base_energy_model + .def(py::init(), py::arg("device")) + .def("compute_intensity", &BaseEnergyModel::computeIntensity) + .def("compute_received_power", &BaseEnergyModel::computeReceivedPower) + .def("compute_emitted_power", &BaseEnergyModel::computeEmittedPower) + .def("compute_target_area", &BaseEnergyModel::computeTargetArea) + .def("compute_cross_section", &BaseEnergyModel::computeCrossSection); + + + py::class_ > scanning_device(m, "ScanningDevice"); scanning_device .def(py::init()) .def(py::init< - size_t, - std::string, - double, - glm::dvec3, - Rotation, - std::list, - double, - double, - double, - double, - double, - double, - double, - std::shared_ptr> - >()) + size_t, + std::string, + double, + glm::dvec3, + Rotation, + std::list, + double, + double, + double, + double, + double, + double, + double + >()) .def_readwrite("cached_dr2", &ScanningDevice::cached_Dr2) .def_readwrite("cached_bt2", &ScanningDevice::cached_Bt2) - .def_readwrite("cached_subrayRotation", &ScanningDevice::cached_subrayRotation) + .def_readwrite("cached_subray_rotation", &ScanningDevice::cached_subrayRotation) .def_readwrite("cached_subray_divergence", &ScanningDevice::cached_subrayDivergenceAngle_rad) .def_readwrite("cached_subray_radius_step", &ScanningDevice::cached_subrayRadiusStep) .def_property("energy_model", &ScanningDevice::getEnergyModel, &ScanningDevice::setEnergyModel) - .def_property("FWF_settings", + .def_property("fwf_settings", &ScanningDevice::getFWFSettings, &ScanningDevice::setFWFSettings) .def_property("received_energy_min", @@ -787,7 +1165,6 @@ namespace pyhelios{ .def("initializeFullWaveform", &ScanningDevice::initializeFullWaveform) .def("calcIntensity", py::overload_cast(&ScanningDevice::calcIntensity, py::const_)) .def("calcIntensity", py::overload_cast(&ScanningDevice::calcIntensity, py::const_)) - .def("eval_range_error_expression", &ScanningDevice::evalRangeErrorExpression); @@ -802,7 +1179,13 @@ namespace pyhelios{ py::arg("fullWaveNoise") = false, py::arg("platformNoiseDisabled") = false) .def(py::init(), py::arg("scanner")) - + + .def_readwrite("intersection_handling_noise_source", &Scanner::intersectionHandlingNoiseSource) + .def_readwrite("rand_gen1", &Scanner::randGen1) + .def_readwrite("rand_gen2", &Scanner::randGen2) + .def_readwrite("trajectory_time_interval", &Scanner::trajectoryTimeInterval_ns) + .def_readwrite("platform", &Scanner::platform) + .def("initialize_sequential_generators", &Scanner::initializeSequentialGenerators) .def("build_scanning_pulse_process", &Scanner::buildScanningPulseProcess, py::arg("parallelization_strategy"), py::arg("dropper"), py::arg("pool")) @@ -883,9 +1266,6 @@ namespace pyhelios{ .def("set_specific_device_id", py::overload_cast(&Scanner::setDeviceId), py::arg("value"), py::arg("index")) .def("get_specific_detector", py::overload_cast(&Scanner::getDetector), py::arg("index")) .def("set_specific_detector", py::overload_cast, size_t>(&Scanner::setDetector), py::arg("value"), py::arg("index")) - - .def("get_specific_supported_pulse_freqs_hz", py::overload_cast(&Scanner::getSupportedPulseFreqs_Hz), py::arg("index")) - .def("set_specific_supported_pulse_freqs_hz", py::overload_cast&, size_t>(&Scanner::setSupportedPulseFreqs_Hz), py::arg("value"), py::arg("index")) .def("get_head_relative_emitter_position_by_ref", py::overload_cast(&Scanner::getHeadRelativeEmitterPositionByRef), py::arg("index")) .def("get__head_relative_emitter_attitude_by_ref", py::overload_cast(&Scanner::getHeadRelativeEmitterAttitudeByRef), py::arg("index")) @@ -895,16 +1275,89 @@ namespace pyhelios{ .def("get_specific_peak_intensity_index", py::overload_cast(&Scanner::getPeakIntensityIndex, py::const_), py::arg("index")) .def("set_specific_peak_intensity_index", py::overload_cast(&Scanner::setPeakIntensityIndex), py::arg("value"), py::arg("index")) - .def("get_specific_scanner_head", py::overload_cast(&Scanner::getScannerHead), py::arg("index")) .def("set_specific_scanner_head", py::overload_cast, size_t>(&Scanner::setScannerHead), py::arg("value"), py::arg("index")) .def("get_specific_beam_deflector", py::overload_cast(&Scanner::getBeamDeflector), py::arg("index")) .def("set_specific_beam_deflector", py::overload_cast, size_t>(&Scanner::setBeamDeflector), py::arg("value"), py::arg("index")) - .def_property_readonly("current_pulse_number", py::overload_cast<>(&Scanner::getCurrentPulseNumber, py::const_)) + .def_property("fms", + [](Scanner &self) { return self.fms; }, + [](Scanner &self, const std::shared_ptr &fms) { self.fms = fms; } + ) + .def_property("all_output_paths", + [](Scanner &self) { return *self.allOutputPaths; }, + [](Scanner &self, py::object paths) { + std::vector cpp_paths = paths.cast>(); + self.allOutputPaths = std::make_shared>(cpp_paths); + } + ) + .def_property("all_measurements", + [](Scanner &self) { py::list py_measurements; + for (const auto &measurement : *self.cycleMeasurements) { + py_measurements.append(measurement); + } + return py_measurements; }, + [](Scanner &self, py::list measurements) { + auto measurements_vec = std::make_shared>(); + for (auto item : measurements) { + measurements_vec->push_back(item.cast()); + } + self.allMeasurements = measurements_vec; + } + ) + .def_property("all_trajectories", + [](Scanner &self) { + py::list py_trajectories; + for (const auto &trajectory : *self.allTrajectories) { + py_trajectories.append(trajectory); + } + return py_trajectories; + }, + [](Scanner &self, py::list trajectories) { + auto trajectories_vec = std::make_shared>(); + for (auto item : trajectories) { + trajectories_vec->push_back(item.cast()); + } + self.allTrajectories = trajectories_vec; + } + ) + .def_property("cycle_measurements", + [](Scanner &self) { + + py::list py_measurements; + for (const auto &measurement : *self.cycleMeasurements) { + py_measurements.append(measurement); + } + return py_measurements; + }, + [](Scanner &self, py::list measurements) { + auto measurements_vec = std::make_shared>(); + for (auto item : measurements) { + measurements_vec->push_back(item.cast()); + } + self.cycleMeasurements = measurements_vec; + } + ) + .def_property("cycle_trajectories", + [](Scanner &self) { + py::list py_trajectories; + for (const auto &trajectory : *self.cycleTrajectories) { + py_trajectories.append(trajectory); + } + return py_trajectories; + }, + [](Scanner &self, py::list trajectories) { + auto trajectories_vec = std::make_shared>(); + for (auto item : trajectories) { + trajectories_vec->push_back(item.cast()); + } + self.cycleTrajectories = trajectories_vec; + } + ) + .def_property("num_rays", py::overload_cast<>(&Scanner::getNumRays, py::const_), py::overload_cast(&Scanner::setNumRays)) @@ -968,9 +1421,6 @@ namespace pyhelios{ .def_property_readonly("detector", py::overload_cast<>(&Scanner::getDetector)) - .def_property_readonly("supported_pulse_freqs_hz", - py::overload_cast<>(&Scanner::getSupportedPulseFreqs_Hz)) - .def_property("num_time_bins", py::overload_cast<>(&Scanner::getNumTimeBins, py::const_), py::overload_cast(&Scanner::setNumTimeBins)) @@ -979,8 +1429,7 @@ namespace pyhelios{ py::overload_cast<>(&Scanner::getPeakIntensityIndex, py::const_), py::overload_cast(&Scanner::setPeakIntensityIndex)) - - .def_property("pulse_frequency", &Scanner::getPulseFreq_Hz, &Scanner::setPulseFreq_Hz) + .def_property("pulse_freq_hz", &Scanner::getPulseFreq_Hz, &Scanner::setPulseFreq_Hz) .def_property("is_state_active", &Scanner::isActive, &Scanner::setActive) .def_property("write_wave_form", &Scanner::isWriteWaveform, &Scanner::setWriteWaveform) .def_property("calc_echowidth", &Scanner::isCalcEchowidth, &Scanner::setCalcEchowidth) @@ -993,13 +1442,45 @@ namespace pyhelios{ py::overload_cast(&Scanner::setFWFSettings)) .def_property_readonly("num_devices", &Scanner::getNumDevices) .def_property_readonly("time_wave", py::overload_cast<>(&Scanner::getTimeWave)) - - .def_readwrite("intersection_handling_noise_source", &Scanner::intersectionHandlingNoiseSource) - .def_readwrite("rand_gen1", &Scanner::randGen1) - .def_readwrite("rand_gen2", &Scanner::randGen2) - .def_readwrite("trajectory_time_interval", &Scanner::trajectoryTimeInterval_ns); + .def_property( + "cycle_measurements_mutex", + [](ScannerWrap &self) { + if (!self.get_mutex()) { + self.set_mutex(std::make_shared()); + } + return self.get_mutex(); + }, + [](ScannerWrap &self, py::object mutex_obj) { + if (mutex_obj.is_none()) { + self.set_mutex(nullptr); + } else { + auto mutex_ptr = mutex_obj.cast>(); + self.set_mutex(mutex_ptr); + } + }, + "A shared mutex for synchronized operations" + ) + .def_property( + "all_measurements_mutex", + [](ScannerWrap &self) { + if (!self.get_mutex()) { + self.set_mutex(std::make_shared()); + } + return self.get_mutex(); + }, + [](ScannerWrap &self, py::object mutex_obj) { + if (mutex_obj.is_none()) { + self.set_mutex(nullptr); + } else { + auto mutex_ptr = mutex_obj.cast>(); + self.set_mutex(mutex_ptr); + } + }, + "A shared mutex for synchronized operations" + ); + - py::class_ helios_simulation (m, "Simulation"); + py::class_ helios_simulation (m, "PyheliosSimulation"); helios_simulation .def(py::init<>()) .def(py::init< @@ -1077,13 +1558,12 @@ namespace pyhelios{ .def_property_readonly("is_running", &PyHeliosSimulation::isRunning) .def_property_readonly("survey_path", &PyHeliosSimulation::getSurveyPath) .def_property_readonly("assets_path", &PyHeliosSimulation::getAssetsPath) - .def_property_readonly("survey", &PyHeliosSimulation::getSurvey) + .def_property("survey", &PyHeliosSimulation::getSurvey, &PyHeliosSimulation::setSurvey) .def_property_readonly("scanner", &PyHeliosSimulation::getScanner) .def_property_readonly("platform", &PyHeliosSimulation::getPlatform) .def_property_readonly("scene", &PyHeliosSimulation::getScene) .def_property_readonly("num_legs", &PyHeliosSimulation::getNumLegs) - .def_property("num_threads", &PyHeliosSimulation::getNumThreads, &PyHeliosSimulation::setNumThreads) .def_property("callback_frequency", &PyHeliosSimulation::getCallbackFrequency, &PyHeliosSimulation::setCallbackFrequency) .def_property("simulation_frequency", &PyHeliosSimulation::getSimFrequency, &PyHeliosSimulation::setSimFrequency) @@ -1101,7 +1581,592 @@ namespace pyhelios{ .def_property("chunk_size", &PyHeliosSimulation::getChunkSize, &PyHeliosSimulation::setChunkSize) .def_property("warehouse_factor", &PyHeliosSimulation::getWarehouseFactor, &PyHeliosSimulation::setWarehouseFactor); + + py::class_> single_scanner(m, "SingleScanner"); + single_scanner + .def(py::init, double, std::string, double, double, double, double, double, int, bool, bool, bool, bool, bool>(), + py::arg("beam_div_rad"), + py::arg("beam_origin"), + py::arg("beam_orientation"), + py::arg("pulse_freqs"), + py::arg("pulse_length"), + py::arg("id"), + py::arg("average_power"), + py::arg("beam_quality"), + py::arg("efficiency"), + py::arg("receiver_diameter"), + py::arg("atmospheric_visibility"), + py::arg("wavelength"), + py::arg("write_waveform") = false, + py::arg("write_pulse") = false, + py::arg("calc_echowidth") = false, + py::arg("full_wave_noise") = false, + py::arg("platform_noise_disabled") = false + ) + + .def_property( + "supported_pulse_freqs_hz", + [](SingleScanner &self) { + return self.getSupportedPulseFreqs_Hz(0); + }, + [](SingleScanner &self, std::list pulse_freqs) { + self.setSupportedPulseFreqs_Hz(pulse_freqs, 0); + }, + py::return_value_policy::reference_internal + ) + .def_property("pulse_freq_hz", &SingleScanner::getPulseFreq_Hz, &SingleScanner::setPulseFreq_Hz) + + .def(py::init(), py::arg("scanner")) + .def("apply_settings", &SingleScanner::applySettings, py::arg("settings"), py::arg("idx") = 0) + .def("do_sim_step", &SingleScanner::doSimStep, py::arg("leg_index"), py::arg("current_gps_time")) + .def("calc_rays_number", &SingleScanner::calcRaysNumber, py::arg("idx") = 0) + .def("prepare_discretization", &SingleScanner::prepareDiscretization, py::arg("idx") = 0) + .def("calc_atmospheric_attenuation", py::overload_cast(&SingleScanner::calcAtmosphericAttenuation, py::const_)) + .def("check_max_NOR", py::overload_cast(&SingleScanner::checkMaxNOR)) + .def("calc_absolute_beam_attitude", py::overload_cast(&SingleScanner::calcAbsoluteBeamAttitude)) + .def("get_specific_current_pulse_number", py::overload_cast(&SingleScanner::getCurrentPulseNumber, py::const_)) + .def("get_specific_num_rays", py::overload_cast(&SingleScanner::getNumRays, py::const_)) + .def("set_specific_num_rays", py::overload_cast(&SingleScanner::setNumRays)) + .def("get_specific_pulse_length", py::overload_cast(&SingleScanner::getPulseLength_ns, py::const_)) + .def("set_specific_pulse_length", py::overload_cast(&SingleScanner::setPulseLength_ns)) + .def("get_specific_last_pulse_was_hit", py::overload_cast(&SingleScanner::lastPulseWasHit, py::const_)) + .def("set_specific_last_pulse_was_hit", py::overload_cast(&SingleScanner::setLastPulseWasHit)) + .def("get_specific_beam_divergence", py::overload_cast(&SingleScanner::getBeamDivergence, py::const_)) + .def("set_specific_beam_divergence", py::overload_cast(&SingleScanner::setBeamDivergence)) + + .def("get_max_nor", py::overload_cast(&SingleScanner::getMaxNOR, py::const_), py::arg("index")) + .def("set_max_nor", py::overload_cast(&SingleScanner::setMaxNOR), py::arg("value"), py::arg("index")) + + .def("get_specific_average_power", py::overload_cast(&SingleScanner::getAveragePower, py::const_), py::arg("index")) + .def("set_specific_average_power", py::overload_cast(&SingleScanner::setAveragePower), py::arg("value"), py::arg("index")) + + .def("get_specific_beam_quality", py::overload_cast(&SingleScanner::getBeamQuality, py::const_), py::arg("index")) + .def("set_specific_beam_quality", py::overload_cast(&SingleScanner::setBeamQuality), py::arg("value"), py::arg("index")) + + .def("get_specific_efficiency", py::overload_cast(&SingleScanner::getEfficiency, py::const_), py::arg("index")) + .def("set_specific_efficiency", py::overload_cast(&SingleScanner::setEfficiency), py::arg("value"), py::arg("index")) + + .def("get_specific_receiver_diameter", py::overload_cast(&SingleScanner::getReceiverDiameter, py::const_), py::arg("index")) + .def("set_specific_receiver_diameter", py::overload_cast(&SingleScanner::setReceiverDiameter), py::arg("value"), py::arg("index")) + + .def("get_specific_visibility", py::overload_cast(&SingleScanner::getVisibility, py::const_), py::arg("index")) + .def("set_specific_visibility", py::overload_cast(&SingleScanner::setVisibility), py::arg("value"), py::arg("index")) + + .def("get_specific_wavelength", py::overload_cast(&SingleScanner::getWavelength, py::const_), py::arg("index")) + .def("set_specific_wavelength", py::overload_cast(&SingleScanner::setWavelength), py::arg("value"), py::arg("index")) + + .def("get_specific_atmospheric_extinction", py::overload_cast(&SingleScanner::getAtmosphericExtinction, py::const_), py::arg("index")) + .def("set_specific_atmospheric_extinction", py::overload_cast(&SingleScanner::setAtmosphericExtinction), py::arg("value"), py::arg("index")) + + .def("get_specific_beam_waist_radius", py::overload_cast(&SingleScanner::getBeamWaistRadius, py::const_), py::arg("index")) + .def("set_specific_beam_waist_radius", py::overload_cast(&SingleScanner::setBeamWaistRadius), py::arg("value"), py::arg("index")) + + .def("get_head_relative_emitter_attitude", py::overload_cast(&SingleScanner::getHeadRelativeEmitterAttitude, py::const_), py::arg("index")) + .def("set_head_relative_emitter_attitude", py::overload_cast(&SingleScanner::setHeadRelativeEmitterAttitude), py::arg("value"), py::arg("index")) + + .def("get_head_relative_emitter_position", + &SingleScanner::getHeadRelativeEmitterPosition, + py::arg("index") ) + + .def("set_head_relative_emitter_position", + &SingleScanner::setHeadRelativeEmitterPosition, + py::arg("position"), py::arg("index")) + + .def("get_head_relative_emitter_position_by_ref", + &SingleScanner::getHeadRelativeEmitterPositionByRef, + py::return_value_policy::reference_internal, + py::arg("index") ) + + .def("get_head_relative_emitter_attitude_by_ref", + &SingleScanner::getHeadRelativeEmitterAttitudeByRef, + py::return_value_policy::reference_internal, + py::arg("index")) + + .def("get_bt2", &SingleScanner::getBt2, py::arg("index") ) + .def("set_bt2", &SingleScanner::setBt2, py::arg("value"), py::arg("index") ) + + .def("get_dr2", &SingleScanner::getDr2, py::arg("index") ) + .def("set_dr2", &SingleScanner::setDr2, py::arg("value"), py::arg("index") ) + + .def("get_detector", &SingleScanner::getDetector, py::arg("index") ) + .def("set_detector", &SingleScanner::setDetector, py::arg("detector"), py::arg("index") ) + + .def("get_device_id", &SingleScanner::getDeviceId, py::arg("index")) + .def("set_device_id", &SingleScanner::setDeviceId, py::arg("device_id"), py::arg("index") ) + + .def("get_fwf_settings", &SingleScanner::getFWFSettings, py::arg("index") ) + .def("set_fwf_settings", &SingleScanner::setFWFSettings, py::arg("fwf_settings"), py::arg("index") ) + + .def("get_num_time_bins", &SingleScanner::getNumTimeBins, py::arg("index") ) + .def("set_num_time_bins", &SingleScanner::setNumTimeBins, py::arg("value"), py::arg("index")) + + .def("get_time_wave", &SingleScanner::getTimeWave, py::arg("index") ) + .def("set_time_wave", + static_cast&, size_t)>(&SingleScanner::setTimeWave), + py::arg("time_wave"), py::arg("index")) + + .def("set_time_wave", + [](SingleScanner& self, std::vector time_wave, size_t index) { + self.setTimeWave(std::move(time_wave), index); + }, + py::arg("time_wave"), py::arg("index")) + .def("get_peak_intensity_index", &SingleScanner::getPeakIntensityIndex, py::arg("index") ) + .def("set_peak_intensity_index", &SingleScanner::setPeakIntensityIndex, py::arg("value"), py::arg("index") ) + + .def("get_scanner_head", &SingleScanner::getScannerHead, py::arg("index") ) + .def("set_scanner_head", &SingleScanner::setScannerHead, py::arg("scanner_head"), py::arg("index") ) + + .def("get_beam_deflector", &SingleScanner::getBeamDeflector, py::arg("index") ) + .def("set_beam_deflector", &SingleScanner::setBeamDeflector, py::arg("beam_deflector"), py::arg("index") ) + + .def("get_max_NOR", &SingleScanner::getMaxNOR, py::arg("index") ) + .def("set_max_NOR", &SingleScanner::setMaxNOR, py::arg("value"), py::arg("index") ) + .def("clone", &SingleScanner::clone); + + py::class_> multi_scanner(m, "MultiScanner"); + multi_scanner + .def(py::init, std::string, const std::list&, bool, bool, bool, bool, bool>(), + py::arg("scan_devs"), + py::arg("id"), + py::arg("pulse_freqs"), + py::arg("write_waveform") = false, + py::arg("write_pulse") = false, + py::arg("calc_echowidth") = false, + py::arg("full_wave_noise") = false, + py::arg("platform_noise_disabled") = false) + .def(py::init&, bool, bool, bool, bool>(), + py::arg("id"), + py::arg("pulse_freqs"), + py::arg("write_waveform") = false, + py::arg("calc_echowidth") = false, + py::arg("full_wave_noise") = false, + py::arg("platform_noise_disabled") = false) + + .def("on_leg_complete", &MultiScanner::onLegComplete) + .def("prepare_simulation", &MultiScanner::prepareSimulation, py::arg("legacy_energy_model") = 0) + .def("apply_settings", &MultiScanner::applySettings, py::arg("settings"), py::arg("idx")) + .def("do_sim_step", &MultiScanner::doSimStep, py::arg("leg_index"), py::arg("current_gps_time")) + .def("calc_rays_number", &MultiScanner::calcRaysNumber, py::arg("idx")) + .def("prepare_discretization", &MultiScanner::prepareDiscretization, py::arg("idx")) + .def("calc_absolute_beam_attitude", &MultiScanner::calcAbsoluteBeamAttitude, py::arg("idx")) + .def("calc_atmospheric_attenuation", &MultiScanner::calcAtmosphericAttenuation, py::arg("idx")) + .def("check_max_nor", &MultiScanner::checkMaxNOR, py::arg("nor"), py::arg("idx")) + .def("compute_subrays", &MultiScanner::computeSubrays) + .def("initialize_full_waveform", &MultiScanner::initializeFullWaveform, + py::arg("min_hit_dist_m"), py::arg("max_hit_dist_m"), + py::arg("min_hit_time_ns"), py::arg("max_hit_time_ns"), + py::arg("ns_per_bin"), py::arg("distance_threshold"), + py::arg("peak_intensity_index"), py::arg("num_fullwave_bins"), py::arg("idx")) + + .def("calc_intensity", + py::overload_cast(&MultiScanner::calcIntensity, py::const_)) + + .def("calc_intensity", + py::overload_cast(&MultiScanner::calcIntensity, py::const_)) + + .def("set_device_index", &MultiScanner::setDeviceIndex, py::arg("new_idx"), py::arg("old_idx")) + + + .def("get_device_id", &MultiScanner::getDeviceId, py::arg("idx")) + .def("set_device_id", &MultiScanner::setDeviceId, py::arg("device_id"), py::arg("idx")) + + .def("get_pulse_length", &MultiScanner::getPulseLength_ns, py::arg("idx")) + .def("set_pulse_length", &MultiScanner::setPulseLength_ns, py::arg("pulse_length"), py::arg("idx")) + + .def("get_beam_divergence", &MultiScanner::getBeamDivergence, py::arg("idx")) + .def("set_beam_divergence", &MultiScanner::setBeamDivergence, py::arg("beam_divergence"), py::arg("idx")) + + .def("get_average_power", &MultiScanner::getAveragePower, py::arg("idx")) + .def("set_average_power", &MultiScanner::setAveragePower, py::arg("average_power"), py::arg("idx")) + + .def("get_num_rays", &MultiScanner::getNumRays, py::arg("idx")) + .def("set_num_rays", &MultiScanner::setNumRays, py::arg("num_rays"), py::arg("idx")) + + .def("get_last_pulse_was_hit", &MultiScanner::lastPulseWasHit, py::arg("idx")) + .def("set_last_pulse_was_hit", &MultiScanner::setLastPulseWasHit, py::arg("last_pulse_was_hit"), py::arg("idx")) + + .def("get_scanner_head", &MultiScanner::getScannerHead, py::arg("idx")) + .def("set_scanner_head", &MultiScanner::setScannerHead, py::arg("scanner_head"), py::arg("idx")) + + .def("get_beam_deflector", &MultiScanner::getBeamDeflector, py::arg("idx")) + .def("set_beam_deflector", &MultiScanner::setBeamDeflector, py::arg("beam_deflector"), py::arg("idx")) + + .def("get_detector", &MultiScanner::getDetector, py::arg("idx")) + .def("set_detector", &MultiScanner::setDetector, py::arg("detector"), py::arg("idx")) + + .def("get_fwf_settings", &MultiScanner::getFWFSettings, py::arg("idx")) + .def("set_fwf_settings", &MultiScanner::setFWFSettings, py::arg("fwf_settings"), py::arg("idx")) + + .def("get_beam_quality", &MultiScanner::getBeamQuality, py::arg("idx")) + .def("set_beam_quality", &MultiScanner::setBeamQuality, py::arg("beam_quality"), py::arg("idx")) + + .def("get_efficiency", &MultiScanner::getEfficiency, py::arg("idx")) + .def("set_efficiency", &MultiScanner::setEfficiency, py::arg("efficiency"), py::arg("idx")) + + .def("get_receiver_diameter", &MultiScanner::getReceiverDiameter, py::arg("idx")) + .def("set_receiver_diameter", &MultiScanner::setReceiverDiameter, py::arg("receiver_diameter"), py::arg("idx")) + + .def("get_visibility", &MultiScanner::getVisibility, py::arg("idx")) + .def("set_visibility", &MultiScanner::setVisibility, py::arg("visibility"), py::arg("idx")) + + .def("get_wavelength", &MultiScanner::getWavelength, py::arg("idx")) + .def("set_wavelength", &MultiScanner::setWavelength, py::arg("wavelength"), py::arg("idx")) + + .def("get_atmospheric_extinction", &MultiScanner::getAtmosphericExtinction, py::arg("idx")) + .def("set_atmospheric_extinction", &MultiScanner::setAtmosphericExtinction, py::arg("atmospheric_extinction"), py::arg("idx")) + + .def("get_beam_waist_radius", &MultiScanner::getBeamWaistRadius, py::arg("idx")) + .def("set_beam_waist_radius", &MultiScanner::setBeamWaistRadius, py::arg("beam_waist_radius"), py::arg("idx")) + + .def("get_head_relative_emitter_position", &MultiScanner::getHeadRelativeEmitterPosition, py::arg("idx")) + .def("set_head_relative_emitter_position", &MultiScanner::setHeadRelativeEmitterPosition, py::arg("pos"), py::arg("idx")) + + .def("get_head_relative_emitter_attitude", &MultiScanner::getHeadRelativeEmitterAttitude, py::arg("idx")) + .def("set_head_relative_emitter_attitude", &MultiScanner::setHeadRelativeEmitterAttitude, py::arg("attitude"), py::arg("idx")) + + .def("get_head_relative_emitter_position_by_ref", &MultiScanner::getHeadRelativeEmitterPositionByRef, py::arg("idx") = 0) + .def("get_head_relative_emitter_attitude_by_ref", &MultiScanner::getHeadRelativeEmitterAttitudeByRef, py::arg("idx") = 0) + + .def("get_time_wave", &MultiScanner::getTimeWave, py::arg("index") ) + .def("set_time_wave", + static_cast&, size_t)>(&MultiScanner::setTimeWave), + py::arg("time_wave"), py::arg("index")) + + .def("set_time_wave", + [](MultiScanner& self, std::vector time_wave, size_t index) { + self.setTimeWave(std::move(time_wave), index); + }, + py::arg("time_wave"), py::arg("index")) + .def("get_bt2", &MultiScanner::getBt2, py::arg("idx")) + .def("set_bt2", &MultiScanner::setBt2, py::arg("bt2"), py::arg("idx")) + + .def("get_dr2", &MultiScanner::getDr2, py::arg("idx")) + .def("set_dr2", &MultiScanner::setDr2, py::arg("dr2"), py::arg("idx")) + + .def("get_current_pulse_number", &MultiScanner::getCurrentPulseNumber, py::arg("idx")) + + .def("get_pulse_freqs", &MultiScanner::getSupportedPulseFreqs_Hz, py::arg("idx")) + .def("set_pulse_freqs", &MultiScanner::setSupportedPulseFreqs_Hz, py::arg("pulse_freqs_hz"), py::arg("idx")) + + .def("get_max_nor", &MultiScanner::getMaxNOR, py::arg("idx")) + .def("set_max_nor", &MultiScanner::setMaxNOR, py::arg("max_nor"), py::arg("idx")) + + .def("get_num_time_bins", &MultiScanner::getNumTimeBins, py::arg("idx")) + .def("set_num_time_bins", &MultiScanner::setNumTimeBins, py::arg("num_time_bins"), py::arg("idx")) + + .def("get_peak_intensity_index", &MultiScanner::getPeakIntensityIndex, py::arg("idx")) + .def("set_peak_intensity_index", &MultiScanner::setPeakIntensityIndex, py::arg("peak_intensity_index"), py::arg("idx")) + + .def("get_received_energy_min", &MultiScanner::getReceivedEnergyMin, py::arg("idx")) + .def("set_received_energy_min", &MultiScanner::setReceivedEnergyMin, py::arg("received_energy_min"), py::arg("idx")) + .def("clone", &MultiScanner::clone); + + + py::class_> simulation(m, "Simulation"); + simulation + + .def(py::init, int, std::string, bool>(), + py::arg("parallelizationStrategy"), + py::arg("pulseThreadPoolInterface"), + py::arg("chunkSize"), + py::arg("fixedGpsTimeStart") = "", + py::arg("legacyEnergyModel") = false + ) + + .def_readwrite("current_leg_index", &Simulation::mCurrentLegIndex) + .def_readwrite("callback", &Simulation::callback) + .def_readwrite("is_finished", &Simulation::finished) + + .def_property("sim_speed_factor", &Simulation::getSimSpeedFactor, &Simulation::setSimSpeedFactor) + .def_property("scanner", &Simulation::getScanner, &Simulation::setScanner) + .def_property("simulation_frequency", &Simulation::getSimFrequency, &Simulation::setSimFrequency) + .def_property("callback_frequency", &Simulation::getCallbackFrequency, &Simulation::setCallbackFrequency) + + .def("start", &Simulation::start) + .def("pause", &Simulation::pause) + .def("stop", &Simulation::stop); + + + py::class_> fms_facade(m, "FMSFacade"); + fms_facade + .def(py::init<>()) + + .def_readwrite("factory", &FMSFacade::factory) + .def_readwrite("read", &FMSFacade::read) + .def_readwrite("write", &FMSFacade::write) + .def_readwrite("serialization", &FMSFacade::serialization) + + .def("disconnect", &FMSFacade::disconnect); + + + py::class_> fms_write_facade(m, "FMSWriteFacade"); + fms_write_facade + .def(py::init<>()) + .def("disconnect", &FMSWriteFacade::disconnect) + .def("get_measurement_writer_output_path", &FMSWriteFacade::getMeasurementWriterOutputPath); + + + py::class_> fms_facade_factory(m, "FMSFacadeFactory"); + fms_facade_factory + .def(py::init<>()) + + .def("build_facade", py::overload_cast(&FMSFacadeFactory::buildFacade), + py::arg("outdir"), + py::arg("lasScale"), + py::arg("lasOutput"), + py::arg("las10"), + py::arg("zipOutput"), + py::arg("splitByChannel"), + py::arg("survey"), + py::arg("updateSurvey") = true) + + .def("build_facade", py::overload_cast(&FMSFacadeFactory::buildFacade), + py::arg("outdir"), + py::arg("lasScale"), + py::arg("lasOutput"), + py::arg("las10"), + py::arg("zipOutput"), + py::arg("survey"), + py::arg("updateSurvey") = true); + + + py::class_> survey_playback(m, "SurveyPlayback"); + survey_playback + .def(py::init, std::shared_ptr, int, std::shared_ptr, int, std::string, bool, bool, bool>(), + py::arg("survey"), + py::arg("fms"), + py::arg("parallelizationStrategy"), + py::arg("pulseThreadPoolInterface"), + py::arg("chunkSize"), + py::arg("fixedGpsTimeStart"), + py::arg("legacyEnergyModel"), + py::arg("exportToFile") = true, + py::arg("disableShutdown") = false) + + .def_readwrite("fms", &SurveyPlayback::fms) + .def_readwrite("survey", &SurveyPlayback::mSurvey) + + .def("do_sim_step", &SurveyPlayback::doSimStep); + + + py::class_>(m, "PulseThreadPoolFactory") + .def(py::init(), + py::arg("parallelizationStrategy"), + py::arg("poolSize"), + py::arg("deviceAccuracy"), + py::arg("chunkSize"), + py::arg("warehouseFactor") = 1) + .def("make_pulse_thread_pool", &PulseThreadPoolFactory::makePulseThreadPool) + .def("make_basic_pulse_thread_pool", &PulseThreadPoolFactory::makeBasicPulseThreadPool) + .def("make_pulse_warehouse_thread_pool", &PulseThreadPoolFactory::makePulseWarehouseThreadPool); + + + py::class_>(m, "PulseThreadPoolInterface") + .def(py::init<>()) + .def("run_pulse_task", &PulseThreadPoolInterface::run_pulse_task) + .def("try_run_pulse_task", &PulseThreadPoolInterface::try_run_pulse_task) + .def("join", &PulseThreadPoolInterface::join); + + + py::class_>(m, "PulseWarehouseThreadPool") + .def(py::init(), + py::arg("_pool_size"), + py::arg("deviceAccuracy"), + py::arg("maxTasks") = 256) + .def("run_pulse_task", &PulseWarehouseThreadPool::run_pulse_task, + py::arg("dropper")) + .def("try_run_pulse_task", &PulseWarehouseThreadPool::try_run_pulse_task, + py::arg("dropper")) + .def("join", &PulseWarehouseThreadPool::join); + + + py::class_>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&>>(m, "PulseTaskDropper") + .def(py::init()) + + .def("add", (bool (TaskDropper>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&>::*)(std::shared_ptr)) + &TaskDropper>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&>::add) + + .def("drop", (void (TaskDropper>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&>::*)()) + &TaskDropper>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&>::drop) + + .def("drop", (void (TaskDropper>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&>::*)(std::vector>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&)) + &TaskDropper>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource&>::drop); + + + py::class_>(m, "ScanningPulseProcess") + .def(py::init>()) + .def("handle_pulse_computation", &ScanningPulseProcess::handlePulseComputation) + .def("on_leg_complete", &ScanningPulseProcess::onLegComplete) + .def("on_simulation_finished", &ScanningPulseProcess::onSimulationFinished) + .def("get_scanner", &ScanningPulseProcess::getScanner) + .def("is_write_waveform", &ScanningPulseProcess::isWriteWaveform) + .def("is_calc_echowidth", &ScanningPulseProcess::isCalcEchowidth) + .def("get_all_measurements", &ScanningPulseProcess::getAllMeasurements, py::return_value_policy::reference_internal) + .def("get_all_measurements_mutex", &ScanningPulseProcess::getAllMeasurementsMutex, py::return_value_policy::reference_internal) + .def("get_cycle_measurements", &ScanningPulseProcess::getCycleMeasurements, py::return_value_policy::reference_internal) + .def("get_cycle_measurements_mutex", &ScanningPulseProcess::getCycleMeasurementsMutex, py::return_value_policy::reference_internal); + + + py::class_>(m, "BuddingScanningPulseProcess") + .def(py::init< + std::shared_ptr, + PulseTaskDropper&, + PulseThreadPool&, + RandomnessGenerator&, + RandomnessGenerator&, + UniformNoiseSource& +#if DATA_ANALYTICS >= 2 + , std::shared_ptr +#endif + >(), + py::arg("scanner"), + py::arg("dropper"), + py::arg("pool"), + py::arg("randGen1"), + py::arg("randGen2"), + py::arg("intersectionHandlingNoiseSource") +#if DATA_ANALYTICS >= 2 + , py::arg("pulseRecorder") +#endif + ) + .def("handle_pulse_computation", &BuddingScanningPulseProcess::handlePulseComputation) + .def("on_leg_complete", &BuddingScanningPulseProcess::onLegComplete) + .def("on_simulation_finished", &BuddingScanningPulseProcess::onSimulationFinished); + + + py::class_>(m, "WarehouseScanningPulseProcess") + .def(py::init< + std::shared_ptr, + PulseTaskDropper&, + PulseWarehouseThreadPool&, + RandomnessGenerator&, + RandomnessGenerator&, + UniformNoiseSource& +#if DATA_ANALYTICS >= 2 + , std::shared_ptr +#endif + >(), + py::arg("scanner"), + py::arg("dropper"), + py::arg("pool"), + py::arg("randGen1"), + py::arg("randGen2"), + py::arg("intersectionHandlingNoiseSource") +#if DATA_ANALYTICS >= 2 + , py::arg("pulseRecorder") +#endif + ) + .def("handle_pulse_computation", &WarehouseScanningPulseProcess::handlePulseComputation) + .def("on_leg_complete", &WarehouseScanningPulseProcess::onLegComplete) + .def("on_simulation_finished", &WarehouseScanningPulseProcess::onSimulationFinished); + + + py::class_> kdgrove(m, "KDGrove"); + kdgrove + .def(py::init(), py::arg("initTreesCapacity") = 1); + + + py::class_>(m, "KDGroveFactory") + .def(py::init>(), py::arg("kdtf")); + + + py::class_>(m, "KDTreeFactory") + .def(py::init<>()); + + + py::class_>(m, "SimpleKDTreeFactory") + .def(py::init<>()); + + + py::class_>(m, "SimpleKDTreeGeometricStrategy") + .def(py::init(), py::arg("kdtf")); + + + py::class_>(m, "MultiThreadKDTreeFactory") + .def(py::init, std::shared_ptr, size_t, size_t>(), + py::arg("kdtf"), + py::arg("gs"), + py::arg("numJobs") = 2, + py::arg("geomJobs") = 2); + + + py::class_>(m, "SAHKDTreeGeometricStrategy") + .def(py::init(), py::arg("kdtf")); + + + py::class_>(m, "SAHKDTreeFactory") + .def(py::init(), + py::arg("lossNodes") = 21, + py::arg("ci") = 1, + py::arg("cl") = 1, + py::arg("co") = 1); + + + py::class_>(m, "MultiThreadSAHKDTreeFactory") + .def(py::init, std::shared_ptr, size_t, size_t>(), + py::arg("kdtf"), + py::arg("gs"), + py::arg("numJobs") = 2, + py::arg("geomJobs") = 2); + + + py::class_>(m, "AxisSAHKDTreeFactory") + .def(py::init(), + py::arg("lossNodes") = 21, + py::arg("ci") = 1, + py::arg("cl") = 1, + py::arg("co") = 1); + + + py::class_>(m, "AxisSAHKDTreeGeometricStrategy") + .def(py::init(), py::arg("kdtf")); + + + py::class_>(m, "FastSAHKDTreeFactory") + .def(py::init(), + py::arg("lossNodes") = 32, + py::arg("ci") = 1, + py::arg("cl") = 1, + py::arg("co") = 1); + + + py::class_>(m, "FastSAHKDTreeGeometricStrategy") + .def(py::init(), + py::arg("kdtf")); + + m.def("calc_time_propagation", &calcTimePropagation, py::arg("timeWave"), py::arg("numBins"), py::arg("scanner")); } diff --git a/python/pyhelios/leg.py b/python/pyhelios/leg.py index e4e54048f..bdb39c9e4 100644 --- a/python/pyhelios/leg.py +++ b/python/pyhelios/leg.py @@ -1,38 +1,74 @@ from pyhelios.utils import Validatable, ValidatedCppManagedProperty from pyhelios.platforms import PlatformSettings from pyhelios.scanner import ScannerSettings -from typing import Optional, List, Tuple, Annotated, Type, Any +from pyhelios.primitives import TrajectorySettings +from typing import Optional, Type, Dict, Any +import xml.etree.ElementTree as ET import _helios - - class ScanningStrip(Validatable): - def __init__(self, strip_id: Optional[str] = "", legs: Optional[List['Leg']] = None) -> None: + def __init__(self, strip_id: Optional[str] = "", legs: Optional[dict[int, 'Leg']] = None) -> None: self._cpp_object = _helios.ScanningStrip(strip_id) self.strip_id = strip_id - + self.legs = legs or {} strip_id: Optional[str] = ValidatedCppManagedProperty("strip_id") + class Leg(Validatable): def __init__(self, platform_settings: Optional[PlatformSettings] = None, scanner_settings: Optional[ScannerSettings] = None, - scanning_strip: Optional[ScanningStrip] = None, length: Optional[float] = 0.0, serial_id: Optional[int] = 0, belongs_to_strip: Optional[bool] = False) -> None: - + strip: Optional[ScanningStrip] = None, trajectory_settings: Optional[TrajectorySettings] = None, + length: Optional[float] = 0.0, serial_id: Optional[int] = 0) -> None: self._cpp_object = _helios.Leg() - self.platform_settings = platform_settings - self.scanner_settings = scanner_settings - self.strip = scanning_strip - - + self.platform_settings = platform_settings or PlatformSettings() + self.scanner_settings = scanner_settings or ScannerSettings() + self.strip = strip or ScanningStrip() + self.trajectory_settings = trajectory_settings self.length = length self.serial_id = serial_id - platform_settings: Optional[PlatformSettings] = ValidatedCppManagedProperty("platform_settings") scanner_settings: Optional[ScannerSettings] = ValidatedCppManagedProperty("scanner_settings") - scanning_strip: Optional[ScanningStrip] = ValidatedCppManagedProperty("scanning_strip") + strip: Optional[ScanningStrip] = ValidatedCppManagedProperty("strip") + trajectory_settings: Optional[TrajectorySettings] = ValidatedCppManagedProperty("trajectory_settings") length: Optional[float] = ValidatedCppManagedProperty("length") serial_id: Optional[int] = ValidatedCppManagedProperty("serial_id") + + @classmethod + def _set_settings(cls, settings_node: ET.Element, settings_templates: Optional[Dict[str, Any]], settings_class: Type[Any]) -> Any: + template_id = settings_node.get('template') + if template_id and template_id in settings_templates: # TODO add logic for case when we face unknown template + + return settings_class.from_xml_node(settings_node, settings_templates[template_id]) + else: + return settings_class.from_xml_node(settings_node) + + @classmethod + def from_xml(cls, leg_origin_node: ET.Element, id: int, strips: Optional[Dict[str, ScanningStrip]] = None, + platform_settings_templates: Optional[Dict[str, PlatformSettings]] = None, scanner_settings_templates: Optional[Dict[str, ScannerSettings]] = None) -> 'Leg': + leg = cls() + #TODO: add "Obtain trajectory interpolator" + leg.serial_id = id + strip_id = leg_origin_node.get('stripId') + + if strip_id: + strip = strips.get(strip_id, ScanningStrip(strip_id)) + strips[strip_id] = strip + leg.strip = strip + strip.legs[id] = leg + + platform_settings_node = leg_origin_node.find("platformSettings") + if platform_settings_node is not None: + leg.platform_settings = cls._set_settings(platform_settings_node, platform_settings_templates, PlatformSettings) + + scanner_settings_node = leg_origin_node.find("scannerSettings") + if scanner_settings_node is not None: + leg.scanner_settings = cls._set_settings(scanner_settings_node, scanner_settings_templates, ScannerSettings) + + trajectory_settings_node = leg_origin_node.find("TrajectorySettings") + if trajectory_settings_node is not None: + leg.trajectory_settings = TrajectorySettings.from_xml_node(trajectory_settings_node) + return cls._validate(leg) \ No newline at end of file diff --git a/python/pyhelios/platforms.py b/python/pyhelios/platforms.py index 56f7a29c9..38cdf5f7f 100644 --- a/python/pyhelios/platforms.py +++ b/python/pyhelios/platforms.py @@ -1,15 +1,15 @@ -from pyhelios.utils import Validatable, ValidatedCppManagedProperty -from pyhelios.primitives import Rotation, Primitive, AABB -from pyhelios.scene import ScenePart, Scene - -from typing import Optional, List, Tuple, Annotated, Type, Any +from pyhelios.utils import Validatable, ValidatedCppManagedProperty, AssetManager +from pyhelios.primitives import Rotation +from pyhelios.scene import Scene +import math +import xml.etree.ElementTree as ET +from typing import Optional, List, Tuple import _helios - class PlatformSettings(Validatable): - def __init__(self, id: Optional[str] = "", x: Optional[float] = 0.0, y: Optional[float] = 0.0, z: Optional[float] = 0.0, is_on_ground: Optional[bool] = False, - position: Optional[List[float]] = [0, 0, 0], is_yaw_angle_specified: Optional[bool] = False, yaw_angle: Optional[float] = 0.0, - is_stop_and_turn: Optional[bool] = True, is_smooth_turn: Optional[bool] = False, speed_m_s: Optional[float] = 70.0) -> None: + def __init__(self, id: Optional[str] = "DEFAULT_TEMPLATE1_HELIOSCPP", x: Optional[float] = 0.0, y: Optional[float] = 0.0, z: Optional[float] = 0.0, is_on_ground: Optional[bool] = False, + position: Optional[List[float]] = None, is_yaw_angle_specified: Optional[bool] = False, yaw_angle: Optional[float] = 0.0, + is_stop_and_turn: Optional[bool] = True, is_smooth_turn: Optional[bool] = False, is_slowdown_enabled: Optional[bool] = True, speed_m_s: Optional[float] = 70.0) -> None: self._cpp_object = _helios.PlatformSettings() self.id = id @@ -17,11 +17,12 @@ def __init__(self, id: Optional[str] = "", x: Optional[float] = 0.0, y: Optional self.y = y self.z = z self.is_on_ground = is_on_ground - self.position = position + self.position = position or [0, 0, 0] self.is_yaw_angle_specified = is_yaw_angle_specified self.yaw_angle = yaw_angle self.is_stop_and_turn = is_stop_and_turn self.is_smooth_turn = is_smooth_turn + self.is_slowdown_enabled = is_slowdown_enabled self.speed_m_s = speed_m_s id: Optional[str] = ValidatedCppManagedProperty("id") @@ -34,16 +35,59 @@ def __init__(self, id: Optional[str] = "", x: Optional[float] = 0.0, y: Optional yaw_angle: Optional[float] = ValidatedCppManagedProperty("yaw_angle") is_stop_and_turn: Optional[bool] = ValidatedCppManagedProperty("is_stop_and_turn") is_smooth_turn: Optional[bool] = ValidatedCppManagedProperty("is_smooth_turn") + is_slowdown_enabled: Optional[bool] = ValidatedCppManagedProperty("is_slowdown_enabled") speed_m_s: Optional[float] = ValidatedCppManagedProperty("speed_m_s") + + @classmethod + def from_xml_node(cls, node: ET.Element, template: Optional['PlatformSettings'] = None) -> 'PlatformSettings': + settings = cls.copy_template(template) if template else cls() + + settings.id = node.get('id', settings.id) + settings.x = float(node.get('x', settings.x)) + settings.y = float(node.get('y', settings.y)) + settings.z = float(node.get('z', settings.z)) + settings.is_on_ground = bool(node.get('onGround', settings.is_on_ground)) + settings.is_stop_and_turn = bool(node.get('stopAndTurn', settings.is_stop_and_turn)) + settings.is_smooth_turn = bool(node.get('smoothTurn', settings.is_smooth_turn)) + settings.is_slowdown_enabled = bool(node.get('slowdownEnabled', settings.is_slowdown_enabled)) + settings.speed_m_s = float(node.get('movePerSec_m', settings.speed_m_s)) + yaw_at_departure_deg = node.get("yawAtDeparture_deg") + if yaw_at_departure_deg is not None: + settings.is_yaw_angle_specified = True + settings.yaw_angle = math.radians(float(yaw_at_departure_deg)) + if settings.is_stop_and_turn and settings.is_smooth_turn: + raise ValueError("Platform cannot be both stop-and-turn and smooth-turn") + + return cls._validate(settings) + @classmethod + def copy_template(cls, template: 'PlatformSettings') -> 'PlatformSettings': + """Create a copy of the template to be used""" + + return PlatformSettings( + id=template.id, + x=template.x, + y=template.y, + z=template.z, + is_on_ground=template.is_on_ground, + position=template.position, + is_yaw_angle_specified=template.is_yaw_angle_specified, + yaw_angle=template.yaw_angle, + is_stop_and_turn=template.is_stop_and_turn, + is_smooth_turn=template.is_smooth_turn, + is_slowdown_enabled=template.is_slowdown_enabled, + speed_m_s=template.speed_m_s + ) + class Platform(Validatable): - def __init__(self, platform_settings: Optional[PlatformSettings] = None, last_check_z: Optional[float] = 0.0, dmax: Optional[float] = 0.0, is_orientation_on_leg_init: Optional[bool] = False, + def __init__(self, id: Optional[str] = '', platform_settings: Optional[PlatformSettings] = None, last_check_z: Optional[float] = 0.0, dmax: Optional[float] = 0.0, is_orientation_on_leg_init: Optional[bool] = False, is_on_ground: Optional[bool] = False, is_stop_and_turn: Optional[bool] = True, settings_speed_m_s: Optional[float] = 70.0, - is_slowdown_enabled: Optional[bool] = False, is_smooth_turn: Optional[bool] = False) -> None: + is_slowdown_enabled: Optional[bool] = False, is_smooth_turn: Optional[bool] = False, position: Optional[List[float]] = None, scene: Optional[Scene] = None) -> None: self._cpp_object = _helios.Platform() + self.id = id self.last_check_z = last_check_z self.dmax = dmax self.is_orientation_on_leg_init = is_orientation_on_leg_init @@ -52,7 +96,14 @@ def __init__(self, platform_settings: Optional[PlatformSettings] = None, last_ch self.settings_speed_m_s = settings_speed_m_s self.is_slowdown_enabled = is_slowdown_enabled self.is_smooth_turn = is_smooth_turn - + self.position = position or [0, 0, 0] + self.device_relative_position = [0.0, 0.0, 0.0] + self.device_relative_attitude = Rotation(0.0, 1.0, 0.0, 0.0) + self.cached_attitude = Rotation(0.0, 0.0, 1.0, 0.0) + self.scene = scene + self.target_waypoint = [0.0, 0.0, 0.0] + self.next_waypoint = [0.0, 0.0, 0.0] + self.origin_waypoint = [0.0, 0.0, 0.0] last_check_z: Optional[float] = ValidatedCppManagedProperty("last_check_z") @@ -63,5 +114,222 @@ def __init__(self, platform_settings: Optional[PlatformSettings] = None, last_ch settings_speed_m_s: Optional[float] = ValidatedCppManagedProperty("settings_speed_m_s") is_slowdown_enabled: Optional[bool] = ValidatedCppManagedProperty("is_slowdown_enabled") is_smooth_turn: Optional[bool] = ValidatedCppManagedProperty("is_smooth_turn") + position: Optional[List[float]] = ValidatedCppManagedProperty("position") + scene: Optional[Scene] = ValidatedCppManagedProperty("scene") + + @classmethod + def from_xml(cls, filename: str, id: Optional[str] = None) -> 'Platform': + file_path = AssetManager().find_file_by_name(filename, auto_add=True) + tree = ET.parse(file_path) + root = tree.getroot() + + platform_element = root.find(f".//platform[@id='{id}']") + if platform_element is None: + raise ValueError(f"No platform found with id: {id}") + + platform = cls(id=id) + platform = cls._initialize_platform_from_xml(platform, platform_element) + + return cls._validate(platform) + + @classmethod + def _initialize_platform_from_xml(cls, platform: 'Platform', platform_element: ET.Element) -> 'Platform': + platform_type = platform_element.get('type').lower() + + # Select platform subclass based on type + if platform_type == "groundvehicle": + platform = GroundVehiclePlatform(id=platform.id) + elif platform_type == "linearpath": + platform = LinearPathPlatform(id=platform.id) + elif platform_type == "multicopter": + platform = HelicopterPlatform(id=platform.id) + + # Apply type-specific settings + cls._apply_platform_specific_settings(platform, platform_element) + + # Parse scanner mount and other general settings + platform.device_relative_position, platform.device_relative_attitude = cls._parse_scanner_mount(platform_element) + + return platform + + @classmethod + def _apply_platform_specific_settings(cls, platform: 'Platform', platform_element: ET.Element): + if isinstance(platform, SimplePhysicsPlatform): + platform.drag_magnitude = float(platform_element.get('drag', 1.0)) + if isinstance(platform, HelicopterPlatform): + platform._apply_helicopter_specific_settings(platform_element) + + def _apply_helicopter_specific_settings(platform: 'HelicopterPlatform', platform_element: ET.Element): + platform.speedup_magnitude = float(platform_element.get('speedup_magnitude', 2.0)) + platform.slowdown_magnitude = float(platform_element.get('slowdown_magnitude', 2.0)) + platform.max_engine_force_xy = float(platform_element.get('engine_max_force', 0.1)) + platform.base_pitch_angle = math.radians(float(platform_element.get('base_pitch_deg', -5.0))) + platform.pitch_speed = math.radians(float(platform_element.get('pitch_speed_deg', 85.94))) + platform.roll_speed = math.radians(float(platform_element.get('roll_speed_deg', 28.65))) + platform.yaw_speed = math.radians(float(platform_element.get('yaw_speed_deg', 85.94))) + platform.max_pitch_offset = math.radians(float(platform_element.get('max_pitch_offset_deg', 35.0))) + platform.max_roll_offset = math.radians(float(platform_element.get('max_roll_offset_deg', 25.0))) + platform.max_pitch = platform.base_pitch_angle + platform.max_pitch_offset + platform.min_pitch = platform.base_pitch_angle - platform.max_pitch_offset + platform.slowdown_distance_xy = float(platform_element.get('slowdown_distance_xy', 5.0)) + + def _parse_scanner_mount(platform_element) -> Tuple[List[float], Rotation]: + scanner_mount = platform_element.find('scannerMount') + device_relative_position = [0.0, 0.0, 0.0] + + if scanner_mount is not None: + x = float(scanner_mount.get('x', '0.0')) + y = float(scanner_mount.get('y', '0.0')) + z = float(scanner_mount.get('z', '0.0')) + device_relative_position = [x, y, z] + + # Parse the rotation + device_relative_attitude = Rotation.from_xml_node(scanner_mount) + + return device_relative_position, device_relative_attitude + + def retrieve_current_settings(self) -> PlatformSettings: + current_settings = PlatformSettings() + current_settings.speed_m_s = self.settings_speed_m_s + current_settings.is_on_ground = self.is_on_ground + current_settings.position = self.position + return current_settings + + def update_static_cache(self): + self.cached_origin_to_target_dir_xy = self._normalize([ + self.target_waypoint[0] - self.origin_waypoint[0], + self.target_waypoint[1] - self.origin_waypoint[1], + 0 + ]) + + self.cached_target_to_next_dir_xy = self._normalize([ + self.next_waypoint[0] - self.target_waypoint[0], + self.next_waypoint[1] - self.target_waypoint[1], + 0 + ]) + + def _normalize(self, vector): + length = math.sqrt(sum(comp ** 2 for comp in vector)) + if length == 0: + return [0, 0, 0] + return [comp / length for comp in vector] + + def prepare_simulation(self, pulse_frequency: int): + self.cached_absolute_attitude = self.attitude.apply_rotation(self.device_relative_attitude) + self.update_static_cache() + + +class MovingPlatform(Platform): + def __init__(self, id: str | None = '', platform_settings: PlatformSettings | None = None, velocity: Optional[List[float]] = [0, 0, 0], last_check_z: float | None = 0, dmax: float | None = 0, is_orientation_on_leg_init: bool | None = False, is_on_ground: bool | None = False, is_stop_and_turn: bool | None = True, settings_speed_m_s: float | None = 70, is_slowdown_enabled: bool | None = False, is_smooth_turn: bool | None = False) -> None: + super().__init__() + self._cpp_object = _helios.MovingPlatform() + self.velocity = velocity + self.id = id + + velocity: List[float] = ValidatedCppManagedProperty("velocity") + +class SimplePhysicsPlatform(MovingPlatform): + def __init__(self, id: str | None = '', platform_settings: PlatformSettings | None = None, drag_magnitude: Optional[float] = 1.0, last_check_z: float | None = 0, dmax: float | None = 0, is_orientation_on_leg_init: bool | None = False, is_on_ground: bool | None = False, is_stop_and_turn: bool | None = True, settings_speed_m_s: float | None = 70, is_slowdown_enabled: bool | None = False, is_smooth_turn: bool | None = False) -> None: + super().__init__() + self._cpp_object = _helios.SimplePhysicsPlatform() + self.drag_magnitude = drag_magnitude + self.id = id + self.speed_step_magnitude = 0.0 + + drag_magnitude: float = ValidatedCppManagedProperty("drag_magnitude") + + def prepare_simulation(self, pulse_frequency: int): + self.speed_step_magnitude = self.drag_magnitude / float(pulse_frequency) + +class GroundVehiclePlatform(SimplePhysicsPlatform): + def __init__(self, id: str | None = '', platform_settings: PlatformSettings | None = None, last_check_z: float | None = 0, dmax: float | None = 0, is_orientation_on_leg_init: bool | None = False, is_on_ground: bool | None = False, is_stop_and_turn: bool | None = True, settings_speed_m_s: float | None = 70, is_slowdown_enabled: bool | None = False, is_smooth_turn: bool | None = False) -> None: + super().__init__() + self._cpp_object = _helios.GroundVehiclePlatform() + self.id = id + + def prepare_simulation(self, pulse_frequency: int): + SimplePhysicsPlatform.prepare_simulation(self, pulse_frequency) + + +class LinearPathPlatform(MovingPlatform): + def __init__(self, id: str | None = '', platform_settings: PlatformSettings | None = None, last_check_z: float | None = 0, dmax: float | None = 0, is_orientation_on_leg_init: bool | None = False, is_on_ground: bool | None = False, is_stop_and_turn: bool | None = True, settings_speed_m_s: float | None = 70, is_slowdown_enabled: bool | None = False, is_smooth_turn: bool | None = False) -> None: + super().__init__() + self._cpp_object = _helios.LinearPathPlatform() + self.id = id + +class HelicopterPlatform(SimplePhysicsPlatform): + def __init__(self, id: str | None = '', platform_settings: PlatformSettings | None = None, drag_magnitude: float | None = 1.0, last_check_z: float | None = 0, dmax: float | None = 0, is_orientation_on_leg_init: bool | None = False, is_on_ground: bool | None = False, is_stop_and_turn: bool | None = True, settings_speed_m_s: float | None = 70, is_slowdown_enabled: bool | None = False, is_smooth_turn: bool | None = False) -> None: + + super().__init__() + self._cpp_object = _helios.HelicopterPlatform() + self.id = id + self.slowdown_distance_xy = 5.0 + self.slowdown_magnitude = 2.0 + self.speedup_magnitude = 2.0 + self.max_engine_force_xy = 0.1 + self.heading_rad = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + self.last_sign = 1.0 + self.base_pitch_angle = -0.087 + self.pitch_speed = 1.5 + self.roll_speed = 0.5 + self.yaw_speed = 1.5 + self.max_pitch_offset = 0.61 + self.max_roll_offset = 0.45 + self.max_pitch = self.base_pitch_angle + self.max_pitch_offset + self.min_pitch = self.base_pitch_angle - self.max_pitch_offset + self.pitch_step_magnitude = 0.0 + self.roll_step_magnitude = 0.0 + self.yaw_step_magnitude = 0.0 + self.slowdown_factor = 0.0 + self.speedup_factor = 0.0 + + slowdown_distance_xy: float = ValidatedCppManagedProperty("slowdown_distance_xy") + slowdown_magnitude: float = ValidatedCppManagedProperty("slowdown_magnitude") + speedup_magnitude: float = ValidatedCppManagedProperty("speedup_magnitude") + max_engine_force_xy: float = ValidatedCppManagedProperty("max_engine_force_xy") + heading_rad: float = ValidatedCppManagedProperty("heading_rad") + roll: float = ValidatedCppManagedProperty("roll") + pitch: float = ValidatedCppManagedProperty("pitch") + last_sign: float = ValidatedCppManagedProperty("last_sign") + base_pitch_angle: float = ValidatedCppManagedProperty("base_pitch_angle") + pitch_speed: float = ValidatedCppManagedProperty("pitch_speed") + roll_speed: float = ValidatedCppManagedProperty("roll_speed") + yaw_speed: float = ValidatedCppManagedProperty("yaw_speed") + max_pitch_offset: float = ValidatedCppManagedProperty("max_pitch_offset") + max_roll_offset: float = ValidatedCppManagedProperty("max_roll_offset") + max_pitch: float = ValidatedCppManagedProperty("max_pitch") + min_pitch: float = ValidatedCppManagedProperty("min_pitch") + + def prepare_simulation(self, pulse_frequency: int): + self.pitch_step_magnitude = self.pitch_speed / pulse_frequency + self.roll_step_magnitude = self.roll_speed / pulse_frequency + self.yaw_step_magnitude = self.yaw_speed / pulse_frequency + self.slowdown_factor = 1.0 - self.slowdown_magnitude /pulse_frequency + self.speedup_factor = 1.0 + self.speedup_magnitude / pulse_frequency + SimplePhysicsPlatform.prepare_simulation(pulse_frequency) + Platform.prepare_simulation(self, pulse_frequency) + + +SR22 = Platform.from_xml("data/platforms.xml", id="sr22") + +QUADCOPTER = Platform.from_xml("data/platforms.xml", id="quadcopter") + +COPTER_LIN_PATH = Platform.from_xml("data/platforms.xml", id="copter_linearpath") + +TRACTOR = Platform.from_xml("data/platforms.xml", id="tractor") + +TRACTOR_LEFT_SIDE = Platform.from_xml("data/platforms.xml", id="tractor_leftside") + +VEHILE_LIN_PATH = Platform.from_xml("data/platforms.xml", id="vehicle_linearpath") + +VMX_450_CAR_LEFT = Platform.from_xml("data/platforms.xml", id="vmx-450-car-left") + +VMX_450_CAR_RIGHT = Platform.from_xml("data/platforms.xml", id="vmx-450-car-right") + +VMQ_1HA_CAR_0 = Platform.from_xml("data/platforms.xml", id="vmq-1ha-car-0") +SIMPLE_LIN_PATH = Platform.from_xml("data/platforms.xml", id="simple_linearpath") +TRIPOD = Platform.from_xml("data/platforms.xml", id="tripod") diff --git a/python/pyhelios/primitives.py b/python/pyhelios/primitives.py index ff5a800a4..8e9669f1e 100644 --- a/python/pyhelios/primitives.py +++ b/python/pyhelios/primitives.py @@ -1,92 +1,381 @@ -from pyhelios.utils import Validatable, ValidatedCppManagedProperty +from pyhelios.utils import Validatable, ValidatedCppManagedProperty, RandomnessGenerator -from pydantic import ConfigDict -from typing import Optional, List +from pydantic import BaseModel, Field +from typing import Optional, List, Dict +from enum import Enum +import threading +import re import numpy as np - +import xml.etree.ElementTree as ET +from collections import deque +import math +from abc import ABC, abstractmethod import _helios + + +class PrimitiveType(Enum): + NONE = _helios.PrimitiveType.NONE + TRIANGLE = _helios.PrimitiveType.TRIANGLE + VOXEL = _helios.PrimitiveType.VOXEL + class Rotation(Validatable): - model_config = ConfigDict(arbitrary_types_allowed=True) - def __init__(self, q0: Optional[float] = .0, q1: Optional[float] = .0, q2: Optional[float] = .0, q3: Optional[float] = .0): + """ + q0: float - The scalar part of the quaternion + q1: float - The x component of the vector part of the quaternion + q2: float - The y component of the vector part of the quaternion + q3: float - The z component of the vector part of the quaternion + """ + + def __init__(self, q0: Optional[float] = .0, q1: Optional[float] = .0, q2: Optional[float] = .0, q3: Optional[float] = .0, needs_normalization: Optional[bool] = False): - self._cpp_object = _helios.Rotation(q0, q1, q2, q3, False) + self._cpp_object = _helios.Rotation(q0, q1, q2, q3, needs_normalization) self.q0 = q0 self.q1 = q1 self.q2 = q2 self.q3 = q3 + #TODO: add normalization procedure q0: Optional[float] = ValidatedCppManagedProperty("q0") q1: Optional[float] = ValidatedCppManagedProperty("q1") q2: Optional[float] = ValidatedCppManagedProperty("q2") q3: Optional[float] = ValidatedCppManagedProperty("q3") + @classmethod + def from_xml_node(cls, beam_origin_node: ET.Element) -> 'Rotation': + if beam_origin_node is None: + return cls._validate(cls(q0=1., q1=1., q2=0., q3=0.)) + + rotation_node = beam_origin_node.findall('rot') + result_rotation = cls(q0=1.0, q1=0.0, q2=0.0, q3=0.0) # Identity rotation + + for node in rotation_node: + axis = node.get('axis') + angle_deg = float(node.get('angle_deg', 0.0)) + angle_rad = math.radians(angle_deg) # Convert to radians + if angle_rad != 0: + rotation = cls._create_rotation_from_axis_angle(axis, angle_rad) + result_rotation = result_rotation.apply_rotation(rotation) # Combine rotations + return result_rotation._validate(result_rotation) + + @staticmethod + def _create_rotation_from_axis_angle(axis: str, angle: float) -> 'Rotation': + if axis.lower() == 'x': + return Rotation( + math.cos(angle / 2), + math.sin(angle / 2), + 0.0, + 0.0 + ) + elif axis.lower() == 'y': + return Rotation( + math.cos(angle / 2), + 0.0, + math.sin(angle / 2), + 0.0 + ) + elif axis.lower() == 'z': + return Rotation( + math.cos(angle / 2), + 0.0, + 0.0, + -math.sin(angle / 2) + ) + + def scale(self, factor: float) -> None: + """Scale the positions of all vertices by a given factor.""" + for vertex in self.vertices: + vertex.position[0] *= factor + vertex.position[1] *= factor + vertex.position[2] *= factor + + def translate(self, shift: List[float]) -> None: + """Translate the positions of all vertices by a given shift.""" + for vertex in self.vertices: + vertex.position[0] += shift[0] + vertex.position[1] += shift[1] + vertex.position[2] += shift[2] + + def apply_rotation(self, r: 'Rotation') -> 'Rotation': + return Rotation(r.q0 * self.q0 - (r.q1 * self.q1 + r.q2 * self.q2 + r.q3 * self.q3), + r.q1 * self.q0 + r.q0 * self.q1 + (r.q2 * self.q3 - r.q3 * self.q2), + r.q2 * self.q0 + r.q0 * self.q2 + (r.q3 * self.q1 - r.q1 * self.q3), + r.q3 * self.q0 + r.q0 * self.q3 + (r.q1 * self.q2 - r.q2 * self.q1)) + + def apply_vector_rotation(self, vec: List[float]) -> List[float]: + x = vec[0] + y = vec[1] + z = vec[2] + s = self.q1 * x + self.q2 * y + self.q3 * z + return [2*(self.q0*(self.q0 * x - (self.q2 * z - self.q3 * y)) + s * self.q1) - x, + 2*(self.q0*(self.q0 * y - (self.q3 * x - self.q1 * z)) + s * self.q2) - y, + 2*(self.q0*(self.q0 * z - (self.q1 * y - self.q2 * x)) + s * self.q3) - z] + + def clone(self): + return Rotation(self.q0, self.q1, self.q2, self.q3) + class Vertex(Validatable): - def __init__(self, pos: Optional[List[float]]=[]) -> None: + def __init__(self, position: Optional[List[float]]=None, tex_coords: Optional[List[float]] = None, normal: Optional[List[float]]=None) -> None: self._cpp_object = _helios.Vertex() + self.position = position or [0., 0., 0.] + self.normal = normal or [0., 0., 0.] + self.tex_coords = tex_coords or [0., 0.] - + position: Optional[List[float]] = ValidatedCppManagedProperty("position") + normal: Optional[List[float]] = ValidatedCppManagedProperty("normal") + tex_coords: Optional[List[float]] = ValidatedCppManagedProperty("tex_coords") + + def clone(self): + new_vertex = Vertex(self.position[:], self.tex_coords[:]) + new_vertex._cpp_object = self._cpp_object.clone() + return new_vertex + + class DetailedVoxel(Validatable): def __init__(self, center: Optional[List[float]], voxel_size: Optional[float], int_values: Optional[List[int]] = None, float_values: Optional[List[float]] = None) -> None: #def __init__(self, nb_echos: Optional[int] = 0, nb_sampling: Optional[int] = 0, max_pad: Optional[float] = .0): #: self._cpp_object = _helios.DetailedVoxel((center, voxel_size, int_values, float_values) if int_values is not None and float_values is not None else ()) - - self.nb_echos = self._cpp_object.nb_echos - - self.nb_sampling = self._cpp_object.nb_sampling - self.max_pad = self._cpp_object.max_pad + self.center = center + self.voxel_size = voxel_size + self.int_values = int_values + self.float_values = float_values + self.nb_echos = self.int_values[0] if self.int_values is not None else 0 + + self.nb_sampling = self.int_values[1] if self.int_values is not None else 0 + self.max_pad = 0.0 nb_echos: Optional[int] = ValidatedCppManagedProperty("nb_echos") nb_sampling: Optional[int] = ValidatedCppManagedProperty("nb_sampling") max_pad: Optional[float] = ValidatedCppManagedProperty("max_pad") - -class Triangle(Validatable): - def __init__(self, v0: Optional[Vertex], v1: Optional[Vertex], v2: Optional[Vertex]) -> None: - self._cpp_object = _helios.Triangle(v0._cpp_object, v1._cpp_object, v2._cpp_object) + def clone(self): + new_voxel = DetailedVoxel(self.center, self.voxel_size, self.int_values, self.float_values) + new_voxel._cpp_object = self._cpp_object.clone() + return new_voxel + + +class EnergyModel(Validatable): + def __init__(self, scanning_device: 'ScanningDevice') -> None: + self._cpp_object = _helios.EnergyModel(scanning_device._cpp_object) + from pyhelios.scanner import ScanningDevice + self.scanning_device = scanning_device + + +class BaseEnergyModel(EnergyModel): + def __init__(self, scanning_device: 'ScanningDevice') -> None: + super().__init__(scanning_device) + self._cpp_object = _helios.BaseEnergyModel(scanning_device._cpp_object) class Material(Validatable): - def __init__(self, name: Optional[str], mat_file_path: Optional[str], is_ground: Optional[bool] = False, - use_vertex_colors: Optional[bool] = False, reflectance: Optional[float] = None, specularity: Optional[float] = .0, ) -> None: + def __init__(self, mat_file_path: Optional[str] = "", name: Optional[str] = "default", is_ground: Optional[bool] = False, + use_vertex_colors: Optional[bool] = False, reflectance: Optional[float] = .0, specularity: Optional[float] = .0, + ambient_components: Optional[List[float]] = None, diffuse_components: Optional[List[float]] = None, + specular_components: Optional[List[float]] = None, map_kd: Optional[str] = "", spectra: Optional[str] = "") -> None: self._cpp_object = _helios.Material() self.name = name self.mat_file_path = mat_file_path self.is_ground = is_ground - self.use_vertex_colord = use_vertex_colors + self.use_vertex_colors = use_vertex_colors self.reflectance = reflectance self.specularity = specularity + self.ambient_components = ambient_components or [0.0, 0.0, 0.0, 0.0] + self.diffuse_components = diffuse_components or [0.0, 0.0, 0.0, 0.0] + self.specular_components = specular_components or [0.0, 0.0, 0.0, 0.0] + self.classification = 0 + self.specular_exponent = 10 + self.map_kd = map_kd + self.spectra = spectra name: Optional[str] = ValidatedCppManagedProperty("name") mat_file_path: Optional[str] = ValidatedCppManagedProperty("mat_file_path") is_ground: Optional[bool] = ValidatedCppManagedProperty("is_ground") - use_vertex_colors: Optional[bool] = ValidatedCppManagedProperty("use_vertex_color") + use_vertex_colors: Optional[bool] = ValidatedCppManagedProperty("use_vertex_colors") reflectance: Optional[float] = ValidatedCppManagedProperty("reflectance") specularity: Optional[float] = ValidatedCppManagedProperty("specularity") - + ambient_components: Optional[List[float]] = ValidatedCppManagedProperty("ambient_components") + diffuse_components: Optional[List[float]] = ValidatedCppManagedProperty("diffuse_components") + specular_components: Optional[List[float]] = ValidatedCppManagedProperty("specular_components") + classification: Optional[int] = ValidatedCppManagedProperty("classification") + specular_exponent: Optional[float] = ValidatedCppManagedProperty("specular_exponent") + map_kd: Optional[str] = ValidatedCppManagedProperty("map_kd") + spectra: Optional[str] = ValidatedCppManagedProperty("spectra") -class AABB(Validatable): - def __init__(self) -> None: - self._cpp_object = _helios.AABB.create() + def calculate_specularity(self,): + ds_sum = sum(self.diffuse_components) + sum(self.specular_components) + if ds_sum > 0: + self.specularity = sum(self.specular_components) / ds_sum + + + @classmethod + def load_materials(cls, filePathString: str) -> Dict[str, 'Material']: + newMats = {} + is_first_material = True + + with open(filePathString, 'r') as f: + for line in f: + line = line.strip() + if not line or line in ["\r", "\r\n", "\n"]: + continue + + lineParts = re.split(r'\s+', line) + + # Wavefront .mtl standard attributes + if lineParts[0] == "newmtl" and len(lineParts) >= 2: + if not is_first_material: + newMats[newMat.name] = newMat + newMat = Material() + newMat.mat_file_path = filePathString + newMat.name = lineParts[1] + is_first_material = False + elif lineParts[0] == "Ka" and len(lineParts) >= 4: + newMat.ambient_components = [float(x) for x in lineParts[1:4]] + [0.0] + elif lineParts[0] == "Kd" and len(lineParts) >= 4: + newMat.diffuse_components = [float(x) for x in lineParts[1:4]] + [0.0] + elif lineParts[0] == "Ks" and len(lineParts) >= 4: + newMat.specular_components = [float(x) for x in lineParts[1:4]] + [0.0] + elif lineParts[0] == "Ns" and len(lineParts) >= 2: + newMat.specular_exponent = float(lineParts[1]) + elif lineParts[0] == "map_Kd" and len(lineParts) >= 2: + newMat.map_kd = lineParts[1] + + # HELIOS-specific additions + elif lineParts[0] == "helios_reflectance" and len(lineParts) >= 2: + newMat.reflectance = float(lineParts[1]) + elif lineParts[0] == "helios_isGround" and len(lineParts) >= 2: + newMat.is_ground = lineParts[1].lower() in ["true", "1"] + elif lineParts[0] == "helios_useVertexColors" and len(lineParts) >= 2: + newMat.use_vertex_colors = bool(int(lineParts[1])) + elif lineParts[0] == "helios_classification" and len(lineParts) >= 2: + newMat.classification = int(lineParts[1]) + elif lineParts[0] == "helios_spectra" and len(lineParts) >= 2: + newMat.spectra = lineParts[1] + + newMat.calculate_specularity() + newMat = cls._validate(newMat) + newMats[newMat.name] = newMat + + return newMats + @classmethod + def parse_materials(cls, params: Dict[str, any]) -> List[Optional['Material']]: + materials = [] + matfile = params.get("matfile") + if not matfile: + return -class Primitive(Validatable): - def __init__(self, material: Optional[Material] = None, aabb: Optional[AABB] = None, - detailed_voxel: Optional[DetailedVoxel] = None, vertices: Optional[List[Vertex]] = None, triangles: Optional[List[Triangle]] = None, - scene_parts: Optional[List['ScenePart']] = None) -> None: + mats = cls.load_materials(matfile) + matname = params.get("matname") + if matname and matname in mats: + materials.append(mats[matname]) + elif mats: + materials.append(next(iter(mats.values()))) # Pick the first material + + random_materials_count = params.get("randomMaterials") + if random_materials_count: + random_range = params.get("randomRange", 1.0) + uns = UniformNoiseSource(0, random_range) + + base_material = materials[0] if materials else Material() # Default base material + for _ in range(random_materials_count): + randomized_material = Material(name=base_material.name, **base_material.__dict__) + # Apply random adjustments to reflectance and other properties + randomized_material.reflectance += uns.next() + randomized_material.reflectance = max(0.0, min(randomized_material.reflectance, 1.0)) # Clamp between 0 and 1 + randomized_material.calculate_specularity() + materials.append(randomized_material) + + return materials + + +class Primitive(Validatable, ABC): + def __init__(self, material: Optional[Material] = None, aabb: Optional['AABB'] = None, + detailed_voxel: Optional[DetailedVoxel] = None, triangles: Optional[List['Triangle']] = None, + scene_part: Optional['ScenePart'] = None) -> None: self._cpp_object = _helios.Primitive() - if scene_parts is not None: + self.material = material + if scene_part is not None: from pyhelios.scene import ScenePart - self.scene_parts = scene_parts + self.scene_part = scene_part + else: + self.scene_part = None + + @property + @abstractmethod + def vertices(self) -> List[Vertex]: + pass + + def rotate(self, r: Rotation) -> None: + for vertex in self.vertices: + vertex.position = r.apply_vector_rotation(vertex.position) + vertex.normal = r.apply_vector_rotation(vertex.normal) + + def scale(self, factor: float) -> None: + for vertex in self.vertices: + vertex.position[0] *= factor + vertex.position[1] *= factor + vertex.position[2] *= factor + + def translate(self, shift: List[float]) -> None: + for vertex in self.vertices: + vertex.position[0] += shift[0] + vertex.position[1] += shift[1] + vertex.position[2] += shift[2] + + +class AABB(Primitive): + def __init__(self) -> None: + super().__init__() + self._cpp_object = _helios.AABB() + self._vertices: Optional[List[Vertex]] = None + self.bounds: Optional[List[List[float]]] = None + + @property + def vertices(self) -> List[Vertex]: + """Return the vertices for this AABB.""" + return self._vertices + + +class Triangle(Primitive): + def __init__(self, v0: Optional[Vertex], v1: Optional[Vertex], v2: Optional[Vertex]) -> None: + super().__init__() + self._cpp_object = _helios.Triangle(v0._cpp_object, v1._cpp_object, v2._cpp_object) + self._vertices: List[Vertex] = [v0, v1, v2] + self.face_normal_set = False + self.face_normal: List[float] = [0,0,0] + + @property + def vertices(self) -> List[Vertex]: + """Return the vertices for this Triangle.""" + return self._vertices + + def get_face_normal(self): + if not self.face_normal_set: + self.update_face_normal() + self.face_normal_set = True + return self.face_normal + + def update_face_normal(self): + # Compute the normal using the cross product of two edges of the triangle + v0, v1, v2 = [np.array(v.position) for v in self.vertices] + edge1 = v1 - v0 + edge2 = v2 - v0 + cross_prod = np.cross(edge1, edge2) + if np.linalg.norm(cross_prod) > 1e-8: + self.face_normal = cross_prod / np.linalg.norm(cross_prod) else: - self.scene_parts = [] + self.face_normal = [0, 0, 0] # Normalize the normal + def clone(self): + new_triangle = Triangle(self.v0, self.v1, self.v2) + new_triangle._cpp_object = self._cpp_object.clone() + return new_triangle class Trajectory(Validatable): - def __init__(self, gps_time: Optional[float] = .0, position: Optional[List[float]] = [0, 0, 0], roll: Optional[float] = .0, pitch: Optional[float] = .0, yaw: Optional[float] = .0) -> None: + def __init__(self, gps_time: Optional[float] = .0, position: Optional[List[float]] = None, roll: Optional[float] = .0, pitch: Optional[float] = .0, yaw: Optional[float] = .0) -> None: self._cpp_object = _helios.Trajectory() - self.position = position + self.position = position or [0., 0., 0.] self.roll = roll self.pitch = pitch self.yaw = yaw @@ -97,18 +386,41 @@ def __init__(self, gps_time: Optional[float] = .0, position: Optional[List[float pitch: Optional[float] = ValidatedCppManagedProperty("pitch") yaw: Optional[float] = ValidatedCppManagedProperty("yaw") + def clone(self): + return Trajectory(self.gps_time, self.position, self.roll, self.pitch, self.yaw) + +class TrajectorySettings(Validatable): + def __init__(self, start_time: Optional[float] = np.finfo(float).min, end_time: Optional[float] = np.finfo(float).max, teleport_to_start: Optional[bool] = False) -> None: + self._cpp_object = _helios.TrajectorySettings() + self.start_time = start_time + self.end_time = end_time + self.teleport_to_start = teleport_to_start + + start_time: Optional[float] = ValidatedCppManagedProperty("start_time") + end_time: Optional[float] = ValidatedCppManagedProperty("end_time") + teleport_to_start: Optional[bool] = ValidatedCppManagedProperty("teleport_to_start") + + @classmethod + def from_xml_node(cls, trajectory_settings_node: ET.Element) -> 'TrajectorySettings': + if trajectory_settings_node is None: + return cls._validate(cls()) + start_time = float(trajectory_settings_node.get('startTime', np.finfo(float).min)) + end_time = float(trajectory_settings_node.get('endTime', np.finfo(float).max)) + teleport_to_start = bool(trajectory_settings_node.get('teleportToStart', False)) + return cls._validate(cls(start_time=start_time, end_time=end_time, teleport_to_start=teleport_to_start)) + class Measurement(Validatable): - def __init__(self, hit_object_id: Optional[str] = "", position: Optional[List[float]] = [0, 0, 0], beam_direction: Optional[List[float]] = [0, 0, 0], - beam_origin: Optional[List[float]] = [0, 0, 0], distance: Optional[float] = .0, intensity: Optional[float] = .0, + def __init__(self, hit_object_id: Optional[str] = "", position: Optional[List[float]] = None, beam_direction: Optional[List[float]] = None, + beam_origin: Optional[List[float]] = None, distance: Optional[float] = .0, intensity: Optional[float] = .0, echo_width: Optional[float] = .0, return_number: Optional[int] = 0, pulse_return_number: Optional[int] = 0, fullwave_index: Optional[int] = 0, classification: Optional[int] = 0, gps_time: Optional[float] = .0): self._cpp_object = _helios.Measurement() self.hit_object_id = hit_object_id - self.position = position - self.beam_direction = beam_direction - self.beam_origin = beam_origin + self.position = position or [0., 0., 0.] + self.beam_direction = beam_direction or [0., 0., 0.] + self.beam_origin = beam_origin or [0., 0., 0.] self.distance = distance self.intensity = intensity self.echo_width = echo_width @@ -131,15 +443,42 @@ def __init__(self, hit_object_id: Optional[str] = "", position: Optional[List[fl classification: Optional[int] = ValidatedCppManagedProperty("classification") gps_time: Optional[float] = ValidatedCppManagedProperty("gps_time") + def clone(self): + return Measurement(self.hit_object_id, self.position, self.beam_direction, self.beam_origin, self.distance, self.intensity, self.echo_width, self.return_number, self.pulse_return_number, self.fullwave_index, self.classification, self.gps_time) + class FWFSettings(Validatable): - def __init__(self, bin_size_ns: Optional[float] = .0, beam_sample_quality: Optional[int] = 0) -> None: + def __init__(self, bin_size: Optional[float] = .25, beam_sample_quality: Optional[int] = 3, + pulse_length: Optional[float] = 4.0, max_fullwave_range: Optional[float] = 0.0, + aperture_diameter: Optional[float] = 0.15, win_size: Optional[float] = None) -> None: self._cpp_object = _helios.FWFSettings() - self.bin_size_ns = bin_size_ns + self.bin_size = bin_size self.beam_sample_quality = beam_sample_quality + self.pulse_length = pulse_length + self.win_size = win_size if win_size is not None else pulse_length / 4 + self.max_fullwave_range = max_fullwave_range + self.aperture_diameter = aperture_diameter - bin_size_ns: Optional[float] = ValidatedCppManagedProperty("bin_size_ns") + bin_size: Optional[float] = ValidatedCppManagedProperty("bin_size") beam_sample_quality: Optional[int] = ValidatedCppManagedProperty("beam_sample_quality") + pulse_length: Optional[float] = ValidatedCppManagedProperty("pulse_length") + win_size: Optional[float] = ValidatedCppManagedProperty("win_size") + max_fullwave_range: Optional[float] = ValidatedCppManagedProperty("max_fullwave_range") + aperture_diameter: Optional[float] = ValidatedCppManagedProperty("aperture_diameter") + + @classmethod + def from_xml_node(cls, node: ET.Element) -> 'FWFSettings': + if node is None: + return cls._validate(cls()) + bin_size = float(node.get('binSize_ns', 0.25)) + beam_sample_quality = int(node.get('beamSampleQuality', 3)) + max_fullwave_range = float(node.get('maxFullwaveRange_ns', 0.0)) + aperture_diameter = float(node.get('apertureDiameter_m', 0.15)) + win_size = float(node.get('winSize_ns', str(cls().pulse_length / 4))) + return cls._validate(cls(bin_size=bin_size, beam_sample_quality=beam_sample_quality, max_fullwave_range=max_fullwave_range, aperture_diameter=aperture_diameter, win_size=win_size)) + + def clone(self): + return FWFSettings(self.bin_size, self.beam_sample_quality, self.pulse_length, self.max_fullwave_range, self.aperture_diameter, self.win_size) class AbstractBeamDeflector(Validatable): @@ -165,3 +504,335 @@ def __init__(self, scan_angle_max: Optional[float] = .0, scan_freq_max: Optional vertical_angle_max: Optional[float] = ValidatedCppManagedProperty("vertical_angle_max") current_beam_angle: Optional[float] = ValidatedCppManagedProperty("current_beam_angle") angle_diff_rad: Optional[float] = ValidatedCppManagedProperty("angle_diff_rad") + + @classmethod + def from_xml_node(cls, deflector_origin_node: ET.Element) -> 'AbstractBeamDeflector': + + optics_type = deflector_origin_node.get('optics') + scan_freq_max = float(deflector_origin_node.get('scanFreqMax_Hz', 0.0)) + scan_freq_min = float(deflector_origin_node.get('scanFreqMin_Hz', 0.0)) + scan_angle_max = float(deflector_origin_node.get('scanAngleMax_deg', 0.0)) + + if optics_type == "oscillating": + scan_product = int(deflector_origin_node.get("scanProduct", 1000000)) + return OscillatingMirrorBeamDeflector(scan_angle_max, scan_freq_max, scan_freq_min, scan_product) + + elif optics_type == "conic": + return ConicBeamDeflector(scan_angle_max, scan_freq_max, scan_freq_min) + + elif optics_type == "line": + num_fibers = int(deflector_origin_node.get("numFibers", 1)) + return FiberArrayBeamDeflector(scan_angle_max, scan_freq_max, scan_freq_min, num_fibers) + + elif optics_type == "rotating": + scan_angle_effective_max_deg = float(deflector_origin_node.get("scanAngleEffectiveMax_deg", 0.0)) + scan_angle_effective_max_rad = math.radians(scan_angle_effective_max_deg) + return PolygonMirrorBeamDeflector(scan_angle_max, scan_freq_max, scan_freq_min, scan_angle_effective_max_rad) + + elif optics_type == "risley": + rotor_freq_1_hz = int(deflector_origin_node.get("rotorFreq1_Hz", 7294)) + rotor_freq_2_hz = int(deflector_origin_node.get("rotorFreq2_Hz", -4664)) + return RisleyBeamDeflector(scan_angle_max, rotor_freq_1_hz, rotor_freq_2_hz) + + def clone(self): + new_deflector = AbstractBeamDeflector(self.scan_angle_max, self.scan_freq_max, self.scan_freq_min, self.scan_freq, self.scan_angle, self.vertical_angle_min, self.vertical_angle_max, self.current_beam_angle, self.angle_diff_rad) + new_deflector._cpp_object = self._cpp_object.clone() + return new_deflector + + +class OscillatingMirrorBeamDeflector(AbstractBeamDeflector): + def __init__(self, scan_angle_max: float, scan_freq_max: float, scan_freq_min: float, scan_product: int) -> None: + super().__init__() + self._cpp_object = _helios.OscillatingMirrorBeamDeflector(scan_angle_max, scan_freq_max, scan_freq_min, scan_product) + self.scan_product = scan_product + + def clone(self): + new_deflector = OscillatingMirrorBeamDeflector(self.scan_angle_max, self.scan_freq_max, self.scan_freq_min, self.scan_product) + new_deflector._cpp_object = self._cpp_object.clone() + return new_deflector + + scan_product: Optional[int] = ValidatedCppManagedProperty("scan_product") + + +class ConicBeamDeflector(AbstractBeamDeflector): + def __init__(self, scan_angle_max:float, scan_freq_max: float, scan_freq_min:float) -> None: + super().__init__() + self._cpp_object = _helios.ConicBeamDeflector(scan_angle_max, scan_freq_max, scan_freq_min) + self.scan_angle_max = scan_angle_max + self.scan_freq_max = scan_freq_max + self.scan_freq_min = scan_freq_min + + def clone(self): + new_deflector = ConicBeamDeflector(self.scan_angle_max, self.scan_freq_max, self.scan_freq_min) + new_deflector._cpp_object = self._cpp_object.clone() + return new_deflector + + +class FiberArrayBeamDeflector(AbstractBeamDeflector): + def __init__(self, scan_angle_max: float, scan_freq_max: float, scan_freq_min: float, num_fibers: int) -> None: + super().__init__() + self._cpp_object = _helios.FiberArrayBeamDeflector(scan_angle_max, scan_freq_max, scan_freq_min, num_fibers) + self.scan_angle_max = scan_angle_max + self.scan_freq_max = scan_freq_max + self.scan_freq_min = scan_freq_min + self.num_fibers = num_fibers + + num_fibers: Optional[int] = ValidatedCppManagedProperty("num_fibers") + + def clone(self): + new_deflector = FiberArrayBeamDeflector(self.scan_angle_max, self.scan_freq_max, self.scan_freq_min, self.num_fibers) + new_deflector._cpp_object = self._cpp_object.clone() + return new_deflector + + +class PolygonMirrorBeamDeflector(AbstractBeamDeflector): + def __init__(self, scan_freq_max: float, scan_freq_min: float, scan_angle_max: float, scan_angle_effective_max: float) -> None: + super().__init__() + self._cpp_object = _helios.PolygonMirrorBeamDeflector(scan_freq_max, scan_freq_min, scan_angle_max, scan_angle_effective_max) + self.scan_angle_max = scan_angle_max + self.scan_freq_max = scan_freq_max + self.scan_freq_min = scan_freq_min + self.scan_angle_effective_max = scan_angle_effective_max + + def clone(self): + new_deflector = PolygonMirrorBeamDeflector(self.scan_angle_max, self.scan_freq_max, self.scan_freq_min, self.scan_angle_effective_max) + new_deflector._cpp_object = self._cpp_object.clone() + return new_deflector + + +class RisleyBeamDeflector(AbstractBeamDeflector): + def __init__(self, scan_angle_max: float, rotor_speed_rad_1: float, rotor_speed_rad_2: float) -> None: + super().__init__() + self._cpp_object = _helios.RisleyBeamDeflector(scan_angle_max, rotor_speed_rad_1, rotor_speed_rad_2) + self.rotor_speed_rad_1 = rotor_speed_rad_1 + self.rotor_speed_rad_2 = rotor_speed_rad_2 + + rotor_freq_1: Optional[float] = ValidatedCppManagedProperty("rotor_speed_rad_1") + rotor_freq_2: Optional[float] = ValidatedCppManagedProperty("rotor_speed_rad_2") + + def clone(self): + new_deflector = RisleyBeamDeflector(self.scan_angle_max, self.rotor_speed_rad_1, self.rotor_speed_rad_2) + new_deflector._cpp_object = self._cpp_object.clone() + return new_deflector + + +class NoiseSource(Validatable): + def __init__(self, + clip_min: Optional[float] = 0.0, + clip_max: Optional[float] = 1.0, + clip_enabled: Optional[bool] = False, + fixed_lifespan: Optional[float] = None, + fixed_value_remaining_uses: Optional[int] = None): + + self._cpp_object = _helios.NoiseSource() + self.clip_min = clip_min + self.clip_max = clip_max + self.clip_enabled = clip_enabled + self.fixed_lifespan = fixed_lifespan + self.fixed_value_remaining_uses = fixed_value_remaining_uses + + clip_min: Optional[float] = ValidatedCppManagedProperty("clip_min") + clip_max: Optional[float] = ValidatedCppManagedProperty("clip_max") + clip_enabled: Optional[bool] = ValidatedCppManagedProperty("clip_enabled") + fixed_lifespan: Optional[float] = ValidatedCppManagedProperty("fixed_lifespan") + fixed_value_remaining_uses: Optional[int] = ValidatedCppManagedProperty("fixed_value_remaining_uses") + + @property + def fixed_value_enabled(self) -> bool: + """ + Read-only property that checks if fixed value is enabled. + This binds to the C++ isFixedValueEnabled method. + """ + return self._cpp_object.fixed_value_enabled + + def next(self) -> float: + """ + Implement the logic for generating the next noise value. + Mirrors the logic from the C++ `NoiseSource::next()`. + """ + if self.fixed_lifespan == 0: + return self.fixed_value # Return fixed value immediately if lifespan is 0 + + if self.fixed_lifespan > 0 and self.fixed_value_remaining_uses > 0: + self.fixed_value_remaining_uses -= 1 + return self.fixed_value # Decrement remaining uses and return fixed value + + # Otherwise, generate a new noise value + self.fixed_value = self.clip(self.noise_function()) + + # If fixed lifespan is greater than 1, update remaining uses + if self.fixed_lifespan and self.fixed_lifespan > 1: + self.fixed_value_remaining_uses = self.fixed_lifespan - 1 + + return self.fixed_value + + def clip(self, value: float) -> float: + """ + Clips the noise value to be within the min and max bounds (clip_min and clip_max) if clipping is enabled. + This mirrors the C++ `NoiseSource::clip()` function. + """ + if self.clip_enabled: + value = max(self.clip_min, min(value, self.clip_max)) + + return value + + +class RandomNoiseSource(NoiseSource): + """Python class representing RandomNoiseSource, inheriting from NoiseSource.""" + + # Constructor for initializing the Python class and the corresponding C++ object + def __init__(self, seed: Optional[str] = None): + # Initialize the C++ RandomNoiseSource object, either with a seed or the default constructor + if seed is not None: + self._cpp_object = _helios.RandomNoiseSourceDouble(seed) + else: + self._cpp_object = _helios.RandomNoiseSourceDouble() + + # Initialize properties for validation + self.clip_min = self._cpp_object.clip_min + self.clip_max = self._cpp_object.clip_max + self.clip_enabled = self._cpp_object.clip_enabled + self.fixed_lifespan = self._cpp_object.fixed_lifespan + self.fixed_value_remaining_uses = self._cpp_object.fixed_value_remaining_uses + + # Properties with validation that link to C++ object + clip_min: Optional[float] = ValidatedCppManagedProperty("clip_min") + clip_max: Optional[float] = ValidatedCppManagedProperty("clip_max") + clip_enabled: Optional[bool] = ValidatedCppManagedProperty("clip_enabled") + fixed_lifespan: Optional[int] = ValidatedCppManagedProperty("fixed_lifespan") + fixed_value_remaining_uses: Optional[int] = ValidatedCppManagedProperty("fixed_value_remaining_uses") + + +class UniformNoiseSource(RandomNoiseSource): + """Python class representing UniformNoiseSource, inheriting from RandomNoiseSource.""" + + def __init__(self, seed: Optional[str] = None, min: Optional[float] = 0.0, max: Optional[float] = 1.0): + """Initialize the Python object, calling the appropriate C++ constructor.""" + if seed is not None: + self._cpp_object = _helios.UniformNoiseSource(seed, min, max) + else: + self._cpp_object = _helios.UniformNoiseSource(min, max) + + # Initialize properties for validation + self.min = self._cpp_object.min + self.max = self._cpp_object.max + + # Initialize randomness generator (in Python) + self.randomness_generator = RandomnessGenerator(seed=seed) + self.randomness_generator.compute_uniform_real_distribution(min, max) + + # Properties with validation that link to the C++ object + min: Optional[float] = ValidatedCppManagedProperty("min") + max: Optional[float] = ValidatedCppManagedProperty("max") + + def noise_function(self) -> float: + """Generate the next value in the uniform distribution.""" + return self.randomness_generator.uniform_real_distribution_next() + + def configure_uniform_noise(self, min_value: float, max_value: float): + """Configure the uniform noise range in the C++ object.""" + if min_value >= max_value: + raise ValueError("min_value must be less than max_value.") + + self._cpp_object.configure_uniform_noise(min_value, max_value) + self.randomness_generator.compute_uniform_real_distribution(min_value, max_value) + + +class SwapOnRepeatHandler(Validatable): + def __init__(self, baseline: Optional['ScenePart'] = None, keep_crs: Optional[bool] = False, discard_on_replay: Optional[bool] = False) -> None: + self._cpp_object = _helios.SwapOnRepeatHandler() + self.baseline = baseline + self.keep_crs = keep_crs + self.times_to_live: deque[int] = deque() + self.discard_on_replay = discard_on_replay + self.num_target_swaps: int = 0 + self.num_current_swaps: int = 0 + + baseline: Optional['ScenePart'] = ValidatedCppManagedProperty("baseline") + keep_crs: Optional[bool] = ValidatedCppManagedProperty("keep_crs") + discard_on_replay: Optional[bool] = ValidatedCppManagedProperty("discard_on_replay") + + def push_time_to_live(self, time_to_live: int): + self.times_to_live.append(time_to_live) + self._cpp_object.push_time_to_live(time_to_live) + + def prepare(self, sp: 'ScenePart', swap_filters: deque): + # Calculate num_target_swaps based on the length of swap_filters + self.num_target_swaps = len(self._cpp_object.swap_filters) + + # Calculate num_target_replays by summing up all time-to-live values + self.num_target_replays = sum(self.times_to_live) + + # Set num_current_swaps to zero + self.num_current_swaps = 0 + + # Clone ScenePart object for baseline and set its primitives' parts to None + self.baseline = sp.clone() + for primitive in self.baseline.primitives: + primitive.part = None + + +class KDTreeFactoryMaker(BaseModel): + loss_nodes_default: int = Field(21, description="Default loss nodes for SAH KDTree factories") + + @staticmethod + def make_simple_kd_tree(): + return _helios.SimpleKDTreeFactory() + + @staticmethod + def make_multithreaded_simple_kd_tree(node_jobs: int, geom_jobs: int): + kdtree_factory = _helios.SimpleKDTreeFactory() + geom_strategy = _helios.SimpleKDTreeGeometricStrategy() + return _helios.MultiThreadKDTreeFactory(kdtree_factory, geom_strategy, node_jobs, geom_jobs) + + @staticmethod + def make_sah_kd_tree_factory(loss_nodes: Optional[int] = None): + loss_nodes = loss_nodes or KDTreeFactoryMaker().loss_nodes_default + return _helios.SAHKDTreeFactory(loss_nodes) + + @staticmethod + def make_multithreaded_sah_kd_tree_factory(node_jobs: int, geom_jobs: int, loss_nodes: Optional[int] = None): + loss_nodes = loss_nodes or KDTreeFactoryMaker().loss_nodes_default + kdtree_factory = _helios.SAHKDTreeFactory(loss_nodes) + geom_strategy = _helios.SAHKDTreeGeometricStrategy(kdtree_factory) + return _helios.MultiThreadKDTreeFactory(kdtree_factory, geom_strategy, node_jobs, geom_jobs) + + @staticmethod + def make_axis_sah_kd_tree_factory(loss_nodes: Optional[int] = None): + loss_nodes = loss_nodes or KDTreeFactoryMaker().loss_nodes_default + return _helios.AxisSAHKDTreeFactory(loss_nodes) + + @staticmethod + def make_multithreaded_axis_sah_kd_tree_factory(node_jobs: int, geom_jobs: int, loss_nodes: Optional[int] = None): + loss_nodes = loss_nodes or KDTreeFactoryMaker().loss_nodes_default + kdtree_factory = _helios.AxisSAHKDTreeFactory(loss_nodes) + geom_strategy = _helios.AxisSAHKDTreeGeometricStrategy(kdtree_factory) + return _helios.MultiThreadKDTreeFactory(kdtree_factory, geom_strategy, node_jobs, geom_jobs) + + @staticmethod + def make_fast_sah_kd_tree_factory(loss_nodes: Optional[int] = 32): + return _helios.FastSAHKDTreeFactory(loss_nodes) + + @staticmethod + def make_multithreaded_fast_sah_kd_tree_factory(node_jobs: int, geom_jobs: int, loss_nodes: Optional[int] = 32): + kdtree_factory = _helios.FastSAHKDTreeFactory(loss_nodes) + geom_strategy = _helios.FastSAHKDTreeGeometricStrategy(kdtree_factory) + return _helios.MultiThreadSAHKDTreeFactory(kdtree_factory, geom_strategy, node_jobs, geom_jobs) + + +class SimulationCycleCallback: + def __init__(self) -> None: + self._cpp_object = _helios.SimulationCycleCallback() + self.is_callback_in_progress = False # Flag to prevent recursion + + def __call__(self, measurements: List[Measurement], trajectories: List[Trajectory], outpath: str): + # Prevent recursion + if self.is_callback_in_progress: + return + self.is_callback_in_progress = True # Set flag to prevent recursion + try: + self.measurements = measurements + self.trajectories = trajectories + self.output_path = outpath + finally: + self.is_callback_in_progress = False \ No newline at end of file diff --git a/python/pyhelios/scanner.py b/python/pyhelios/scanner.py index 311037cdb..f1d927651 100644 --- a/python/pyhelios/scanner.py +++ b/python/pyhelios/scanner.py @@ -1,13 +1,20 @@ -from pyhelios.utils import Validatable, ValidatedCppManagedProperty -from pyhelios.primitives import FWFSettings, AbstractBeamDeflector, FWFSettings +from pyhelios.utils import Validatable, ValidatedCppManagedProperty, AssetManager, calc_propagation_time_legacy, create_property +from pyhelios.primitives import FWFSettings, AbstractBeamDeflector, FWFSettings, Rotation, RisleyBeamDeflector, PolygonMirrorBeamDeflector, FiberArrayBeamDeflector, ConicBeamDeflector, OscillatingMirrorBeamDeflector, EnergyModel, Measurement, Trajectory +from pyhelios.platforms import Platform +import sys +import threading +import math +import numpy as np from typing import Optional, List, Tuple, Annotated, Type, Any - +from types import SimpleNamespace +from pydantic import Field, field_validator, model_validator, ConfigDict +import xml.etree.ElementTree as ET import _helios class ScannerSettings(Validatable): - def __init__(self, id: Optional[str] = "", is_active: Optional[bool] = True, head_rotation: Optional[float] = .0, + def __init__(self, id: Optional[str] = "DEFAULT_TEMPLATE1_HELIOSCPP", is_active: Optional[bool] = True, head_rotation: Optional[float] = .0, rotation_start_angle: Optional[float] = .0, rotation_stop_angle: Optional[float] = .0, pulse_frequency: Optional[int] = 0, - scan_angle: Optional[float] = .0, min_vertical_angle: Optional[float] = .0, max_vertical_angle: Optional[float] = .0, + scan_angle: Optional[float] = .0, min_vertical_angle: Optional[float] = -1, max_vertical_angle: Optional[float] = -1, scan_frequency: Optional[float] = .0, beam_divergence_angle: Optional[float] = .003, trajectory_time_interval: Optional[float] = .0, vertical_resolution: Optional[float] = .0, horizontal_resolution: Optional[float] = .0) -> None: @@ -26,7 +33,7 @@ def __init__(self, id: Optional[str] = "", is_active: Optional[bool] = True, hea self.trajectory_time_interval = trajectory_time_interval self.vertical_resolution = vertical_resolution self.horizontal_resolution = horizontal_resolution - + id: Optional[str] = ValidatedCppManagedProperty("id") is_active: Optional[bool] = ValidatedCppManagedProperty("is_active") head_rotation: Optional[float] = ValidatedCppManagedProperty("head_rotation") @@ -42,10 +49,64 @@ def __init__(self, id: Optional[str] = "", is_active: Optional[bool] = True, hea vertical_resolution: Optional[float] = ValidatedCppManagedProperty("vertical_resolution") horizontal_resolution: Optional[float] = ValidatedCppManagedProperty("horizontal_resolution") + @classmethod + def from_xml_node(cls, node: ET.Element, template: Optional['ScannerSettings'] = None) -> 'ScannerSettings': + + settings = cls.copy_template(template) if template else cls() + + settings.id = node.get('id', settings.id) + settings.is_active = node.get('active', str(settings.is_active)).lower() == 'true' + settings.head_rotation = float(node.get('headRotate_deg', settings.head_rotation)) + settings.rotation_start_angle = float(node.get('headRotateStart_deg', settings.rotation_start_angle)) + settings.rotation_stop_angle = float(node.get('headRotateStop_deg', settings.rotation_stop_angle)) + settings.pulse_frequency = int(node.get('pulseFreq_hz', settings.pulse_frequency)) + settings.scan_angle = float(node.get('scanAngle_deg', settings.scan_angle)) + settings.min_vertical_angle = float(node.get('verticalAngleMin_deg', settings.min_vertical_angle)) + settings.max_vertical_angle = float(node.get('verticalAngleMax_deg', settings.max_vertical_angle)) + settings.scan_frequency = float(node.get('scanFreq_hz', settings.scan_frequency)) + settings.beam_divergence_angle = float(node.get('beamDivergence_rad', settings.beam_divergence_angle)) + settings.vertical_resolution = float(node.get('verticalResolution_deg', settings.vertical_resolution)) + settings.horizontal_resolution = float(node.get('horizontalResolution_deg', settings.horizontal_resolution)) + settings.trajectory_time_interval = float(node.get('trajectoryTimeInterval_s', settings.trajectory_time_interval)) + + if settings.rotation_stop_angle < settings.rotation_start_angle and settings.head_rotation > 0: + raise ValueError("Rotation stop angle must be greater than rotation start angle if head rotation is positive") + + if settings.rotation_start_angle < settings.rotation_stop_angle and settings.head_rotation < 0: + raise ValueError("Rotation start angle must be greater than rotation stop angle if head rotation is negative") + + return cls._validate(settings) + + @classmethod + def copy_template(cls, template: 'ScannerSettings') -> 'ScannerSettings': + """Create a copy of the template to be used""" + + return ScannerSettings( + id=template.id, + is_active=template.is_active, + head_rotation=template.head_rotation, + rotation_start_angle=template.rotation_start_angle, + rotation_stop_angle=template.rotation_stop_angle, + pulse_frequency=template.pulse_frequency, + scan_angle=template.scan_angle, + min_vertical_angle=template.min_vertical_angle, + max_vertical_angle=template.max_vertical_angle, + scan_frequency=template.scan_frequency, + beam_divergence_angle=template.beam_divergence_angle, + trajectory_time_interval=template.trajectory_time_interval, + vertical_resolution=template.vertical_resolution, + horizontal_resolution=template.horizontal_resolution + ) + + class ScannerHead(Validatable): - def __init__(self, rotate_per_sec_max: Optional[float], rotation_axis: Optional[List[float]] = [1., 0., 0.], rotate_per_sec: Optional[float] = .0, rotate_stop: Optional[float] = .0, rotate_start: Optional[float] = .0, + def __init__(self, rotate_per_sec_max: Optional[float] = float('inf'), rotation_axis: Optional[List[float]] = None, rotate_per_sec: Optional[float] = .0, rotate_stop: Optional[float] = .0, rotate_start: Optional[float] = .0, rotate_range: Optional[float] = .0, current_rotate_angle: Optional[float] = .0) -> None: + if rotation_axis is None: + rotation_axis = [1., 0., 0.] + self._cpp_object = _helios.ScannerHead(rotation_axis, rotate_per_sec_max) + self.rotation_axis = rotation_axis self.rotate_per_sec_max = rotate_per_sec_max self.rotate_per_sec = rotate_per_sec self.rotate_stop = rotate_stop @@ -53,6 +114,7 @@ def __init__(self, rotate_per_sec_max: Optional[float], rotation_axis: Optional[ self.rotate_range = rotate_range self.current_rotate_angle = current_rotate_angle + rotation_axis: Optional[List[float]] = ValidatedCppManagedProperty("rotation_axis") rotate_per_sec_max: Optional[float] = ValidatedCppManagedProperty("rotate_per_sec_max") rotate_per_sec: Optional[float] = ValidatedCppManagedProperty("rotate_per_sec") rotate_stop: Optional[float] = ValidatedCppManagedProperty("rotate_stop") @@ -60,29 +122,738 @@ def __init__(self, rotate_per_sec_max: Optional[float], rotation_axis: Optional[ rotate_range: Optional[float] = ValidatedCppManagedProperty("rotate_range") current_rotate_angle: Optional[float] = ValidatedCppManagedProperty("current_rotate_angle") + @classmethod + def from_xml_node(cls, head_node: ET.Element) -> 'ScannerHead': + if head_node is None: + raise ValueError("No head node found") + + head_rotate_axis = [0, 0, 1] + axis_node = head_node.find("headRotateAxis") + + if axis_node is not None: + x = float(axis_node.get('x', 0.0)) + y = float(axis_node.get('y', 0.0)) + z = float(axis_node.get('z', 1.0)) + head_rotate_axis = [x, y, z] + if math.sqrt(sum([coord**2 for coord in head_rotate_axis])) <= 0.1: + head_rotate_axis = [0, 0, 1] + + rotate_per_sec_max = float(head_node.get('headRotatePerSecMax_deg', 0.0)) + return cls._validate(cls(rotation_axis=head_rotate_axis, rotate_per_sec_max=rotate_per_sec_max)) + + def clone(self): + return ScannerHead(self.rotate_per_sec_max, self.rotation_axis, self.rotate_per_sec, self.rotate_stop, self.rotate_start, self.rotate_range, self.current_rotate_angle) + + +class EvalScannerHead(ScannerHead): + def __init__(self, rotation_axis: Optional[List[float]] = None, rotate_per_sec_max: Optional[float] = float('inf')): + if rotation_axis is None: + rotation_axis = [1., 0., 0.] + super().__init__(rotation_axis, rotate_per_sec_max) + self._cpp_object = _helios.EvalScannerHead(rotation_axis, rotate_per_sec_max) + + +class ScanningDevice(Validatable): + def __init__(self, + dev_idx: int, + id: str, + beam_div_rad: float, + emitter_position: List[float], + emitter_attitude: Rotation, + pulse_freqs: List[int], + pulse_length: float, + avg_power: float, + beam_quality: float, + optical_efficiency: float, + receiver_diameter: float, + atmospheric_visibility: float, + wavelength: float, + last_pulse_was_hit: Optional[bool] = False, + fwf_settings: Optional[FWFSettings] = None + ) -> None: + + self._cpp_object = _helios.ScanningDevice( + dev_idx, id, beam_div_rad, emitter_position, emitter_attitude._cpp_object, pulse_freqs, + pulse_length, avg_power, beam_quality, optical_efficiency, + receiver_diameter, atmospheric_visibility, wavelength + ) + + self.dev_idx = dev_idx + self.id = id + self.beam_div_rad = beam_div_rad + self.emitter_position = emitter_position + self.emitter_attitude = emitter_attitude + self.pulse_freqs = pulse_freqs + self.pulse_length = pulse_length + self.avg_power = avg_power + self.beam_quality = beam_quality + self.optical_efficiency = optical_efficiency + self.receiver_diameter = receiver_diameter + self.atmospheric_visibility = atmospheric_visibility + self.wavelength = wavelength + self.last_pulse_was_hit = last_pulse_was_hit or False + self.fwf_settings = fwf_settings or FWFSettings() + + self.max_nor = 0 + self.beam_deflector = AbstractBeamDeflector() + self.detector = AbstractDetector(Scanner("0")) + self.scanner_head = ScannerHead() + + self.num_rays: int = 0 + self.num_time_bins: int = -1 + self.timevawe: List[float] = [] + self.peak_intensity_index: int = -1 + + self.received_energy_min: float = 0.0001 + self.cached_dr2: float = 0.0 + self.cached_bt2: float = 0.0 + self.cached_subray_rotation: List[Rotation] = [Rotation()] + self.cached_div_angles: List[float] = [] + self.cached_radius_steps: List[int] = [] + self.energy_model: Optional[EnergyModel] = None + + fwf_settings: FWFSettings = ValidatedCppManagedProperty("fwf_settings") + received_energy_min: float = ValidatedCppManagedProperty("received_energy_min") + last_pulse_was_hit: bool = ValidatedCppManagedProperty("last_pulse_was_hit") + cached_dr2: float = ValidatedCppManagedProperty("cached_dr2") + cached_bt2: float = ValidatedCppManagedProperty("cached_bt2") + cached_subray_rotation: List[Rotation] = ValidatedCppManagedProperty("cached_subray_rotation") + + def calc_rays_number(self) -> None: + count = 1 + for radius_step in range(self.fwf_settings.beam_sample_quality): + circle_steps = int(2 * math.pi * radius_step) + count += circle_steps + + self.num_rays = count + + def prepare_simulation(self, legacy_energy_model: Optional[bool] = False) -> None: + beam_sample_quality = self.fwf_settings.beam_sample_quality + radius_step = self.beam_div_rad / beam_sample_quality + axis = np.array([1, 0, 0]) + axis2 = np.array([0, 1, 0]) + norm_axis = np.linalg.norm(np.array(axis)) + norm_axis2 = np.linalg.norm(np.array(axis2)) + + for i in range(beam_sample_quality): + div_angle_rad = i * radius_step + self.cached_div_angles.append(div_angle_rad) + half_div_angle_rad = -0.5 * div_angle_rad + coeff_dif_angle = math.sin(half_div_angle_rad) / norm_axis + + r1 = Rotation(math.cos(half_div_angle_rad), + coeff_dif_angle * axis[0], + coeff_dif_angle * axis[1], + coeff_dif_angle * axis[2]) + + circle_steps = int(2 * math.pi) * i + if circle_steps > 0: + circle_step = 2 * math.pi / circle_steps + for j in range(circle_steps): + div_angle_rad2 = j * circle_step + half_div_angle_rad2 = -0.5 * div_angle_rad2 + coeff_dif_angle2 = math.sin(half_div_angle_rad2) / norm_axis2 + r2 = Rotation(math.cos(half_div_angle_rad2), + coeff_dif_angle2 * axis2[0], + coeff_dif_angle2 * axis2[1], + coeff_dif_angle2 * axis2[2]) + + r2= r2.apply_to(r1) + self.cached_subray_rotation.append(r2) + self.cached_radius_steps.append(i) + + if legacy_energy_model: + #TODO call the improved energy model + pass + + else: + self.energy_model = EnergyModel(self) + + def clone(self): + return ScanningDevice(self.dev_idx, self.id, self.beam_div_rad, self.emitter_position, self.emitter_attitude, self.pulse_freqs, self.pulse_length, self.avg_power, self.beam_quality, self.optical_efficiency, self.receiver_diameter, self.atmospheric_visibility, self.wavelength, self.last_pulse_was_hit, self.fwf_settings) + class Scanner(Validatable): - def __init__(self, id: Optional[str], scanner_settings: Optional[ScannerSettings] = None, FWF_settings: Optional[FWFSettings] = None, scanner_head: Optional[ScannerHead] = None, - beam_deflector: Optional[AbstractBeamDeflector] = None, detector: Optional['AbstractDetector'] = None, supported_pulse_freqs_hz: Optional[List[int]] = [0], - num_rays: Optional [int] = 0, pulse_length: Optional[float] = .0) -> None: + def __init__(self, id: Optional[str], supported_pulse_freqs_hz: Optional[List[int]] = None, platform: Optional[Platform] = None, pulse_freq_hz: Optional[int] = 0) -> None: + if supported_pulse_freqs_hz is None: + supported_pulse_freqs_hz = [0] self._cpp_object = _helios.Scanner(id, supported_pulse_freqs_hz) - self.fwf_settings = FWF_settings - self.num_rays = num_rays - self.pulse_length = pulse_length + self.id = id + self.platform = platform + self.supported_pulse_freqs_hz = supported_pulse_freqs_hz + self.trajectory_time_interval: float = 0.0 + self.is_state_active: bool = True + self.pulse_freq_hz = pulse_freq_hz + + self.all_measurements: List[Measurement] = [] + self.all_trajectories: List[Trajectory] = [] + self.all_output_paths: List[str] = [] + + self.write_waveform: Optional[bool] = False + self.calc_echowidth: Optional[bool] = False + self.fullwavenoise: Optional[bool] = False + self.is_platform_noise_disabled: Optional[bool] = False + + self.cycle_measurements: Optional[List[Measurement]] = [] + self.cycle_trajectories: Optional[List[Trajectory]] = [] + self.cycle_measurements_mutex = None + self.all_measurements_mutex = None + +#TODO: print IMPORTANT NOTE: YOU SHOULD CHECK THE LOGIC OF USAGE OF PARENT CLASS INITIALIZATION!!!!!!!!!! + id: Optional[str] = ValidatedCppManagedProperty("id") + trajectory_time_interval: float = ValidatedCppManagedProperty("trajectory_time_interval") + is_state_active: bool = ValidatedCppManagedProperty("is_state_active") + all_output_paths: List[str] = ValidatedCppManagedProperty("all_output_paths") + all_measurements: List[Measurement] = ValidatedCppManagedProperty("all_measurements") + all_trajectories: List[Trajectory] = ValidatedCppManagedProperty("all_trajectories") + cycle_measurements: Optional[List[Measurement]] = ValidatedCppManagedProperty("cycle_measurements") + cycle_trajectories: Optional[List[Trajectory]] = ValidatedCppManagedProperty("cycle_trajectories") + platform: Optional[Platform] = ValidatedCppManagedProperty("platform") + + @classmethod + def from_xml(cls, filename: str, id: Optional[str] = None) -> 'Scanner': + file_path = AssetManager().find_file_by_name(filename, auto_add=True) + tree = ET.parse(file_path) + root = tree.getroot() + + scanner_element = root.find(f".//scanner[@id='{id}']") + if scanner_element is None: + raise ValueError(f"No scanner found with id: {id}") + + emitter_position, emitter_attitude = cls._parse_emitter(scanner_element) + pulse_freqs = cls._parse_pulse_frequencies(scanner_element) + + setting_characteristics = cls._combine_scanner_characteristics(scanner_element) + multi_scanner_element = scanner_element.find('channels') + if multi_scanner_element is None: + scanner = cls._create_single_scanner(scanner_element, pulse_freqs, setting_characteristics, emitter_position, emitter_attitude) + else: + scanner = cls._create_multi_scanner(scanner_element, pulse_freqs, setting_characteristics, emitter_position, emitter_attitude) + + return scanner + + @classmethod + def _parse_emitter(cls, scanner_element) -> Tuple[List[float], Rotation]: + beam_origin = scanner_element.find('beamOrigin') + + emitter_position = [0.0, 0.0, 0.0] + + if beam_origin is not None: + x = float(beam_origin.get('x', '0.0')) + y = float(beam_origin.get('y', '0.0')) + z = float(beam_origin.get('z', '0.0')) + emitter_position = [x, y, z] + + # Parse the rotation + emitter_attitude = Rotation.from_xml_node(beam_origin) + + return emitter_position, emitter_attitude + + @classmethod + def _parse_pulse_frequencies(cls, root) -> List[int]: + pulse_freqs_string = root.get('pulseFreqs_Hz', "0") + return [int(freq) for freq in pulse_freqs_string.split(',')] + + @classmethod + def _combine_scanner_characteristics(cls, root) -> SimpleNamespace: + + settings_characteristics = SimpleNamespace( + beam_div_rad=float(root.get('beamDivergence_rad', '0.0003')), + pulse_length=float(root.get('pulseLength_ns', '4.0')), + average_power=float(root.get('averagePower_w', '4.0')), + beam_quality=float(root.get('beamQualityFactor', '1.0')), + efficiency=float(root.get('opticalEfficiency', '0.99')), + receiver_diameter=float(root.get('receiverDiameter_m', '0.15')), + atmospheric_visibility=float(root.get('atmosphericVisibility_km', '23.0')), + wavelength=int(root.get('wavelength_nm', '1064')) + ) + return settings_characteristics + + def retrieve_current_settings(self, idx: Optional[int] = 0) -> ScannerSettings: + current_settings = ScannerSettings() + current_settings.id = self.id + "_settings" + current_settings.trajectory_time_interval = self.trajectory_time_interval/1000000000 + if isinstance(self, SingleScanner): + # For SingleScanner, use the single scanning device + current_settings.pulse_frequency = self.pulse_freq_hz + current_settings.is_active = self.is_state_active + current_settings.beam_divergence_angle = self.scanning_device.beam_div_rad + current_settings.head_rotation = self.scanning_device.scanner_head.rotate_start + current_settings.rotation_start_angle = self.scanning_device.scanner_head.current_rotate_angle + current_settings.rotation_stop_angle = self.scanning_device.scanner_head.rotate_stop + current_settings.scan_angle = self.scanning_device.beam_deflector.scan_angle + current_settings.scan_frequency = self.scanning_device.beam_deflector.scan_freq_max + current_settings.min_vertical_angle = self.scanning_device.beam_deflector.vertical_angle_min + current_settings.max_vertical_angle = self.scanning_device.beam_deflector.vertical_angle_max + + elif isinstance(self, MultiScanner): + # For MultiScanner, use the scanning device at index idx + current_settings.pulse_frequency = self.scanning_devices[idx].pulse_freqs + current_settings.is_active = self.is_state_active + current_settings.beam_divergence_angle = self.scanning_devices[idx].beam_div_rad + current_settings.head_rotation = self.scanning_devices[idx].scanner_head.rotate_start + current_settings.rotation_start_angle = self.scanning_devices[idx].scanner_head.current_rotate_angle + current_settings.rotation_stop_angle = self.scanning_devices[idx].scanner_head.rotate_stop + current_settings.scan_angle = self.scanning_devices[idx].beam_deflector.scan_angle + current_settings.scan_frequency = self.scanning_devices[idx].beam_deflector.scan_freq_max + current_settings.min_vertical_angle = self.scanning_devices[idx].beam_deflector.vertical_angle_min + current_settings.max_vertical_angle = self.scanning_devices[idx].beam_deflector.vertical_angle_max + + return current_settings + + @classmethod + def _create_single_scanner(cls, scanner_element, pulse_freqs, setting_characteristics, emitter_position, emitter_attitude) -> 'SingleScanner': + # Create the single scanner instance + scanner = SingleScanner( + id=scanner_element.get('id', 'default'), + average_power=setting_characteristics.average_power, + pulse_freqs=pulse_freqs, + beam_quality=setting_characteristics.beam_quality, + efficiency=setting_characteristics.efficiency, + receiver_diameter=setting_characteristics.receiver_diameter, + atmospheric_visibility=setting_characteristics.atmospheric_visibility, + wavelength=setting_characteristics.wavelength, + beam_div_rad=setting_characteristics.beam_div_rad, + beam_origin=emitter_position, + beam_orientation=emitter_attitude, + pulse_length=setting_characteristics.pulse_length + ) + + # Optionally set additional properties (detector, deflector, head, etc.) + scanner.max_NOR = int(scanner_element.get('maxNOR', 0)) + scanner.beam_deflector = AbstractBeamDeflector.from_xml_node(scanner_element) + scanner.detector = AbstractDetector.from_xml_node(scanner_element, scanner) + scanner.scanner_head = ScannerHead.from_xml_node(scanner_element) + + # Apply waveform settings if present + fwf_node = scanner_element.find('FWFSettings') + fwf_settings = FWFSettings() + fwf_settings.pulse_length = setting_characteristics.pulse_length + + scanner.apply_settings_FWF(fwf_settings.from_xml_node(fwf_node)) + return scanner + + + @classmethod + def _create_multi_scanner(cls, scanner_element, pulse_freqs, settings, emitter_position, emitter_attitude) -> 'MultiScanner': + # Handle multi-scanner setup similar to the C++ code + channels = scanner_element.find('channels') + n_channels = sum(1 for _ in channels.findall('channel')) + + scan_devs = [] + for idx in range(n_channels): + scan_dev = ScanningDevice( + idx, + scanner_element.get('id', 'default'), + settings.beam_div_rad, + emitter_position, + emitter_attitude, + pulse_freqs, + settings.pulse_length, + settings.average_power, + settings.beam_quality, + settings.efficiency, + settings.receiver_diameter, + settings.atmospheric_visibility, + settings.wavelength * 1e-9, # Placeholder for range error expression + ) + scan_dev.received_energy_min = float(scanner_element.get('receivedEnergyMin', '0.0001')) + scan_devs.append(scan_dev) + + scanner = MultiScanner(scan_devs, str(scanner_element.get('id', 'default')), pulse_freqs) + + # Set properties like beam deflector, detector, scanner head, etc. + fwf_settings = FWFSettings.from_xml_node(scanner_element.find('FWFSettings')) + abs_beam_def = AbstractBeamDeflector.from_xml_node(scanner_element) + abs_detector = AbstractDetector.from_xml_node(scanner_element, scanner) + scanner_head = ScannerHead.from_xml_node(scanner_element) + + cls._fill_scan_devs_from_channels(scanner, scanner_element, channels, abs_beam_def, abs_detector, + scanner_head, fwf_settings) + return scanner + + @classmethod + def _fill_scan_devs_from_channels(cls, scanner, scanner_element, channels, abs_deflector, abs_detector, scanner_head, fwf_settings) -> None: + # Iterate over the channels and fill the scan devices, also it should have a counter for the index from 0 + for idx, channel in enumerate(channels.findall('channel')): + scanner.active_scanner_index = idx + scanner._cpp_object.set_device_index(idx, idx) + + scanner.device_id = channel.get('id', 'DeviceID') + + emitter_position, emitter_attitude = cls._parse_emitter(channel) + scanner.head_relative_emitter_position = emitter_position + scanner.head_relative_emitter_attitude = emitter_attitude + scanner.apply_settings_FWF(fwf_settings.from_xml_node(channel.find('FWFSettings'))) + + update_deflector = True + + optics_type = channel.get('optics') + if optics_type is not None: + deflectors_match = (optics_type == "oscillating" and isinstance(abs_deflector, OscillatingMirrorBeamDeflector) + ) or ( + optics_type == "conic" and isinstance(abs_deflector, ConicBeamDeflector) + ) or ( + optics_type == "line" and isinstance(abs_deflector, FiberArrayBeamDeflector) + ) or ( + optics_type == "rotating" and isinstance(abs_deflector, PolygonMirrorBeamDeflector) + ) or ( + optics_type == "risley" and isinstance(abs_deflector, RisleyBeamDeflector)) + + if not deflectors_match: + new_deflector = AbstractBeamDeflector.from_xml_node(channel) + scanner.beam_deflector = new_deflector + update_deflector = False # Don't update if we just replaced the deflector + + if update_deflector: + scanner.beam_deflector = abs_deflector.clone() + current_deflector = scanner.beam_deflector + current_deflector.scan_freq_min = float(channel.get('scanFreqMin_Hz', current_deflector.scan_freq_min)) + current_deflector.scan_freq_max = float(channel.get('scanFreqMax_Hz', current_deflector.scan_freq_max)) + + if 'scanAngleMax_deg' in channel.attrib: + current_deflector.scan_angle_max = math.radians(float(channel.get('scanAngleMax_deg', 0.0))) + + # Handle specific updates for Oscillating Mirror Beam Deflector + if isinstance(current_deflector, OscillatingMirrorBeamDeflector): + current_deflector.scan_product = int(channel.get('scanProduct', current_deflector.scan_product)) + + # Handle specific updates for Fiber Array Beam Deflector + elif isinstance(current_deflector, FiberArrayBeamDeflector): + current_deflector.num_fibers = int(channel.get('numFibers', current_deflector.num_fibers)) + + # Handle specific updates for Polygon Mirror Beam Deflector + elif isinstance(current_deflector, PolygonMirrorBeamDeflector): + current_deflector.scan_angle_max = math.radians(float(channel.get('scanAngleEffectiveMax_deg', math.degrees(current_deflector.scan_angle_max)))) + + # Handle specific updates for Risley Beam Deflector + elif isinstance(current_deflector, RisleyBeamDeflector): + current_deflector.rotor_freq_1 = float(channel.get('rotorFreq1_Hz', 7294)) / (2 * math.pi) + current_deflector.rotor_freq_2 = float(channel.get('rotorFreq2_Hz', -4664)) / (2 * math.pi) + + current_detector = abs_detector.clone() + current_detector.range_max = float(channel.get('rangeMax_m', current_detector.range_max)) + current_detector.accuracy = float(channel.get('accuracy_m', current_detector.accuracy)) + current_detector.range_min = float(channel.get('rangeMin_m', current_detector.range_min)) + scanner.detector = current_detector + + current_head = scanner_head.clone() + current_head.rotate_per_sec_max = math.radians(float(channel.get('headRotatePerSecMax_deg', math.degrees(current_head.rotate_per_sec_max)))) + axis_node = channel.find("headRotateAxis") + if axis_node is not None: + axis_str = axis_node.text.split() + axis = [float(coord) for coord in axis_str] + current_head.rotation_axis = axis + scanner.scanner_head = current_head + + scanner.beam_div_rad = float(channel.get('beamDivergence_rad', scanner.beam_div_rad)) + scanner.pulse_length = float(channel.get('pulseLength_ns', scanner.pulse_length)) + if channel.get("wavelength_nm") is not None: + scanner.wavelength = int(channel.get('wavelength_nm', 1064)) + scanner.max_nor = int(channel.get('maxNOR', 0)) + scanner.received_energy_min = float(channel.get('receivedEnergyMin', scanner.received_energy_min)) - FWF_settings: Optional[FWFSettings] = ValidatedCppManagedProperty("fwf_settings") - num_rays: Optional[int] = ValidatedCppManagedProperty("num_rays") - pulse_length: Optional[float] = ValidatedCppManagedProperty("pulse_length") + class Config: + arbitrary_types_allowed = True + + +class SingleScanner(Scanner): + def __init__(self, id: str, average_power: float, pulse_freqs: List[int], + beam_quality: float, efficiency: float, receiver_diameter: float, + atmospheric_visibility: float, wavelength: int, beam_div_rad: Optional[float] = 0, + beam_origin: Optional[List[float]] = None, beam_orientation: Optional[Rotation] = None, + pulse_length: Optional[float] = 0, scanner_settings: Optional[ScannerSettings] = None, write_waveform=False, + write_pulse=False, calc_echowidth=False, full_wave_noise=False, + platform_noise_disabled=False) -> None: + + + beam_origin = beam_origin or [0.0, 0.0, 0.0] + + beam_orientation = beam_orientation or Rotation() + + super().__init__(id, supported_pulse_freqs_hz=pulse_freqs) + self._cpp_object = _helios.SingleScanner( + beam_div_rad, beam_origin, beam_orientation._cpp_object, pulse_freqs, pulse_length, id, average_power, + beam_quality, efficiency, receiver_diameter, atmospheric_visibility, wavelength, + write_waveform, write_pulse, calc_echowidth, full_wave_noise, platform_noise_disabled) + + self.id = id + self.scanner_settings = scanner_settings + self.scanning_device = ScanningDevice(0, id, beam_div_rad, beam_origin, beam_orientation, pulse_freqs, pulse_length, average_power, beam_quality, efficiency, receiver_diameter, atmospheric_visibility, wavelength) + self.write_waveform = write_waveform + self.write_pulse = write_pulse + self.calc_echowidth = calc_echowidth + self.full_wave_noise = full_wave_noise + self.platform_noise_disabled = platform_noise_disabled + self.pulse_freqs = pulse_freqs + self.supported_pulse_freqs_hz = pulse_freqs + + @classmethod + def from_xml(cls, filename: str, id: str = None) -> 'SingleScanner': + file_path = AssetManager().find_file_by_name(filename, auto_add=True) + tree = ET.parse(file_path) + root = tree.getroot() + + id = root.get('id', id) + beam_div_rad = float(root.get('beamDivergence_rad')) + beam_origin = [float(x) for x in root.get('beam_origin').split(',')] + beam_orientation = root.get('beam_orientation') + pulse_freqs = [int(x) for x in root.get('pulseFreqs_Hz').split(',')] + pulse_length = float(root.get('pulseLength_ns')) + average_power = float(root.get('average_power')) + beam_quality = float(root.get('beam_quality')) + efficiency = float(root.get('efficiency')) + receiver_diameter = float(root.get('receiver_diameter')) + atmospheric_visibility = float(root.get('atmospheric_visibility')) + wavelength = int(root.get('wavelength')) + + scanner_instance = cls( + id=id, + beam_div_rad=beam_div_rad, + beam_origin=beam_origin, + beam_orientation=beam_orientation, + pulse_freqs=pulse_freqs, + pulse_length=pulse_length, + average_power=average_power, + beam_quality=beam_quality, + efficiency=efficiency, + receiver_diameter=receiver_diameter, + atmospheric_visibility=atmospheric_visibility, + wavelength=wavelength, + range_err_expr=None + ) + return cls._validate(scanner_instance) + + device_id = create_property('id', 'set_device_id') + average_power = create_property('avg_power', 'set_average_power') + beam_div_rad = create_property('beam_div_rad', 'set_beam_divergence') + + beam_quality = create_property('beam_quality', 'set_beam_quality') + efficiency = create_property('optical_efficiency', 'set_optical_efficiency') + receiver_diameter = create_property('receiver_diameter', 'set_receiver_diameter') + atmospheric_visibility = create_property('atmospheric_visibility', 'set_atmospheric_visibility') + wavelength = create_property('wavelength', 'set_wavelength') + beam_origin = create_property('emitter_position', 'set_head_relative_emitter_position') + beam_orientation = create_property('emitter_attitude', 'set_head_relative_emitter_attitude') + max_NOR = create_property('max_nor', 'set_max_nor') + beam_deflector = create_property('beam_deflector', 'set_beam_deflector') + detector = create_property('detector', 'set_detector') + scanner_head = create_property('scanner_head', 'set_scanner_head') + fwf_settings = create_property('fwf_settings', 'set_fwf_settings') + num_time_bins = create_property('num_time_bins', 'set_num_time_bins') + pulse_length = create_property('pulse_length', 'set_pulse_length') + timewave = create_property('timewave', 'set_time_wave') + peak_intensity_index = create_property('peak_intensity_index', 'set_peak_intensity_index') + + def prepare_discretization(self): + self.num_time_bins = int(self.pulse_length / self.fwf_settings.bin_size) + self.timewave = [0.0] * self.num_time_bins + + self.peak_intensity_index = calc_propagation_time_legacy(self.timewave, self.num_time_bins, self.fwf_settings.bin_size, self.pulse_length, 7.0) + + def apply_settings_FWF(self, settings: FWFSettings): + self._cpp_object.apply_settings_FWF(settings._cpp_object, 0) + self.fwf_settings = settings + self.scanning_device.calc_rays_number() + self.prepare_discretization() + + def prepare_simulation(self, is_legacy_energy_model: bool = False): + self.scanning_device.prepare_simulation(is_legacy_energy_model) + + def clone(self): + new_scanner = SingleScanner(self.id, self.average_power, self.pulse_freqs, self.beam_quality, self.efficiency, self.receiver_diameter, self.atmospheric_visibility, self.wavelength, self.beam_div_rad, self.beam_origin, self.beam_orientation, self.pulse_length, self.scanner_settings, self.write_waveform, self.write_pulse, self.calc_echowidth, self.full_wave_noise, self.platform_noise_disabled) + new_scanner.max_NOR = self.max_NOR + new_scanner.beam_deflector = self.beam_deflector + new_scanner.detector = self.detector + new_scanner.scanner_head = self.scanner_head + new_scanner.fwf_settings = self.fwf_settings + new_scanner.num_time_bins = self.num_time_bins + new_scanner.timewave = self.timewave + new_scanner.peak_intensity_index = self.peak_intensity_index + new_scanner._cpp_object = self._cpp_object.clone() + + +class MultiScanner(Scanner): + def __init__(self, + scanning_devices: List[ScanningDevice], + id: str, + pulse_freqs: Optional[List[int]] = None, + device_rotation: Optional[Rotation] = None, + global_position: Optional[List[float]] = None, + active_scanner_index: Optional[int] = -1, + write_waveform: Optional[bool] = False, + calc_echowidth: Optional[bool] = False, + full_wave_noise: Optional[bool] = False, + platform_noise_disabled: Optional[bool] = False) -> None: + + pulse_freqs = pulse_freqs or [0] + + super().__init__(id) + self._cpp_object = _helios.MultiScanner([sd._cpp_object for sd in scanning_devices], id, pulse_freqs) + + self.id = id + self.scanning_devices = scanning_devices + self.device_rotation = device_rotation or Rotation() + self.global_position = global_position or [0.0, 0.0, 0.0] + self.active_scanner_index = active_scanner_index + self.write_waveform = write_waveform + self.calc_echowidth = calc_echowidth + self.full_wave_noise = full_wave_noise + self.platform_noise_disabled = platform_noise_disabled + + def _get_active_device(self): + return self.scanning_devices[self.active_scanner_index] + + device_id = create_property('id', 'set_device_id', index_function=lambda self: self.active_scanner_index) + head_relative_emitter_position = create_property('emitter_position', 'set_head_relative_emitter_position', index_function=lambda self: self.active_scanner_index) + head_relative_emitter_attitude = create_property('emitter_attitude', 'set_head_relative_emitter_attitude', index_function=lambda self: self.active_scanner_index) + num_time_bins = create_property('num_time_bins', 'set_num_time_bins', index_function=lambda self: self.active_scanner_index) + timewave = create_property('timewave', 'set_time_wave', index_function=lambda self: self.active_scanner_index) + pulse_length = create_property('pulse_length', 'set_pulse_length', index_function=lambda self: self.active_scanner_index) + fwf_settings = create_property('fwf_settings', 'set_fwf_settings', index_function=lambda self: self.active_scanner_index) + peak_intensity_index = create_property('peak_intensity_index', 'set_peak_intensity_index', index_function=lambda self: self.active_scanner_index) + beam_deflector = create_property('beam_deflector', 'set_beam_deflector', index_function=lambda self: self.active_scanner_index) + detector = create_property('detector', 'set_detector', index_function=lambda self: self.active_scanner_index) + scanner_head = create_property('scanner_head', 'set_scanner_head', index_function=lambda self: self.active_scanner_index) + beam_div_rad = create_property('beam_div_rad', 'set_beam_divergence', index_function=lambda self: self.active_scanner_index) + wavelength = create_property('wavelength', 'set_wavelength', index_function=lambda self: self.active_scanner_index) + max_nor = create_property('max_nor', 'set_max_nor', index_function=lambda self: self.active_scanner_index) + received_energy_min = create_property('received_energy_min', 'set_received_energy_min', index_function=lambda self: self.active_scanner_index) + average_power = create_property('average_power', 'set_average_power', index_function=lambda self: self.active_scanner_index) + pulse_freqs = create_property('pulse_freqs', 'set_pulse_freqs', index_function=lambda self: self.active_scanner_index) + beam_quality = create_property('beam_quality', 'set_beam_quality', index_function=lambda self: self.active_scanner_index) + efficiency = create_property('efficiency', 'set_optical_efficiency', index_function=lambda self: self.active_scanner_index) + receiver_diameter = create_property('receiver_diameter', 'set_receiver_diameter', index_function=lambda self: self.active_scanner_index) + + def prepare_discretization(self): + self.num_time_bins = int(self.pulse_length / self.fwf_settings.bin_size) + self.timewave = [0.0] * self.num_time_bins + + self.peak_intensity_index = calc_propagation_time_legacy(self.timewave, self.num_time_bins, self.fwf_settings.bin_size, self.pulse_length, 7.0) + + def apply_settings_FWF(self, settings: FWFSettings): + active_device = self._get_active_device() + self._cpp_object.apply_settings_FWF(settings._cpp_object, self.active_scanner_index) + self.fwf_settings = settings + active_device.calc_rays_number() + self.prepare_discretization() + + def prepare_simulation(self, is_legacy_energy_model: bool = False): + num_devices = len(self.scanning_devices) + for i in range(num_devices): + self.scanning_devices[i].prepare_simulation(is_legacy_energy_model) + + def clone(self): + new_scanner = MultiScanner(self.scanning_devices, self.id, self.pulse_freqs, self.device_rotation, self.global_position, self.active_scanner_index, self.write_waveform, self.calc_echowidth, self.full_wave_noise, self.platform_noise_disabled) + new_scanner._cpp_object = self._cpp_object.clone() + new_scanner.scanning_devices = [sd.clone() for sd in self.scanning_devices] + new_scanner.device_rotation = self.device_rotation + new_scanner.global_position = self.global_position + new_scanner.active_scanner_index = self.active_scanner_index + new_scanner.pulse_freqs = self.pulse_freqs + new_scanner.write_waveform = self.write_waveform + new_scanner.calc_echowidth = self.calc_echowidth + new_scanner.full_wave_noise = self.full_wave_noise + new_scanner.platform_noise_disabled = self.platform_noise_disabled + return new_scanner class AbstractDetector(Validatable): - def __init__(self, scanner: Optional[Scanner], range_max: Optional[float], accuracy: Optional[float] = .0, range_min: Optional[float] = .0) -> None: + def __init__(self, scanner: Optional[Scanner], range_max: Optional[float] = sys.float_info.max, + accuracy: Optional[float] = .0, range_min: Optional[float] = .0) -> None: + scanner = scanner or Scanner() self._cpp_object = _helios.AbstractDetector(scanner._cpp_object, accuracy, range_min, range_max) + self.scanner = scanner self.accuracy = accuracy self.range_min = range_min self.range_max = range_max - accuracy: Optional[float] = ValidatedCppManagedProperty("accuracy") range_min: Optional[float] = ValidatedCppManagedProperty("range_min") range_max: Optional[float] = ValidatedCppManagedProperty("range_max") + + @classmethod + def from_xml_node(cls, detector_node: ET.Element, scanner: Scanner) -> 'AbstractDetector': + + range_max = float(detector_node.get('rangeMax_m', 1e20)) + accuracy = float(detector_node.get('accuracy_m', '0.0')) + range_min = float(detector_node.get('rangeMin_m', '0.0')) + + return FullWaveformPulseDetector._validate(FullWaveformPulseDetector(scanner=scanner, range_max=range_max, accuracy=accuracy, range_min=range_min)) + + def clone(self): + new_detector = AbstractDetector(self.scanner, self.range_max, self.accuracy, self.range_min) + new_detector._cpp_object = self._cpp_object.clone() + return new_detector + + +class FullWaveformPulseDetector(AbstractDetector): + def __init__(self, scanner: Optional[Scanner], range_max: Optional[float], accuracy: Optional[float] = .0, range_min: Optional[float] = .0, pulse_length: Optional[float] = .0) -> None: + scanner = scanner or Scanner() + self._cpp_object = _helios.FullWaveformPulseDetector(scanner._cpp_object, accuracy, range_min, range_max) + self.scanner = scanner + self.accuracy = accuracy + self.range_min = range_min + self.range_max = range_max + self.pulse_length = pulse_length + + def clone(self): + new_detector = FullWaveformPulseDetector(self.scanner, self.range_max, self.accuracy, self.range_min, self.pulse_length) + new_detector._cpp_object = self._cpp_object.clone() + return new_detector + + +LEICAALS50 = Scanner.from_xml("data/scanners_als.xml", id="leica_als50") + +LEICAALS50_II = Scanner.from_xml("data/scanners_als.xml", id="leica_als50-ii") + +OPTECH_2033 = Scanner.from_xml("data/scanners_als.xml", id="optech_2033") + +OPTECH_3100 = Scanner.from_xml("data/scanners_als.xml", id="optech_3100") + +OPTECH_GALAXY = Scanner.from_xml("data/scanners_als.xml", id="optech_galaxy") + +RIEGL_LMS_Q560 = Scanner.from_xml("data/scanners_als.xml", id="riegl_lms-q560") + +RIEGL_LMS_Q780 = Scanner.from_xml("data/scanners_als.xml", id="riegl_lms-q780") + +RIEGL_VQ_780i = Scanner.from_xml("data/scanners_als.xml", id="riegl_vq_780i") + +RIEGL_VUX_1UAV = Scanner.from_xml("data/scanners_als.xml", id="riegl_vux-1uav") + +RIEGL_VUX_1UAV22 = Scanner.from_xml("data/scanners_als.xml", id="riegl_vux-1uav22") + +RIEGL_VUX_1HA22 = Scanner.from_xml("data/scanners_als.xml", id="riegl_vux-1ha22") + +RIEGL_VQ_880g = Scanner.from_xml("data/scanners_als.xml", id="riegl_vq-880g") + +RIEGL_VQ_1560i = Scanner.from_xml("data/scanners_als.xml", id="riegl_vq-1560i") + +LIVOX_MID70 = Scanner.from_xml("data/scanners_als.xml", id="livox_mid-70") + +LIVOX_MID100 = Scanner.from_xml("data/scanners_als.xml", id="livox-mid-100") + +LIVOX_MID100a = Scanner.from_xml("data/scanners_als.xml", id="livox-mid-100a") + +LIVOX_MID100b = Scanner.from_xml("data/scanners_als.xml", id="livox-mid-100b") + +LIVOX_MID100c = Scanner.from_xml("data/scanners_als.xml", id="livox-mid-100c") + + +#TLS + +RIEGL_VZ_400 = Scanner.from_xml("data/scanners_tls.xml", id="riegl_vz400") + +RIEGL_VZ_1000 = Scanner.from_xml("data/scanners_tls.xml", id="riegl_vz1000") + +RIEGL_VQ_450 = Scanner.from_xml("data/scanners_tls.xml", id="riegl_vq-450") + +LIVOX_MID70_TLS = Scanner.from_xml("data/scanners_tls.xml", id="livox_mid-70") + +VLP16 = Scanner.from_xml("data/scanners_tls.xml", id="vlp16") + +VELODYNE_HDL_64E = Scanner.from_xml("data/scanners_tls.xml", id="velodyne_hdl-64e") + +TRACTOR_SCANNER = Scanner.from_xml("data/scanners_tls.xml", id="tractorscanner") + +PANO_SCANNER = Scanner.from_xml("data/scanners_tls.xml", id="panoscanner") \ No newline at end of file diff --git a/python/pyhelios/scene.py b/python/pyhelios/scene.py old mode 100755 new mode 100644 index e7ffaf0b8..539d03992 --- a/python/pyhelios/scene.py +++ b/python/pyhelios/scene.py @@ -1,52 +1,739 @@ -from pyhelios.utils import Validatable, ValidatedCppManagedProperty -from pyhelios.primitives import Rotation, Primitive, AABB +from pyhelios.utils import Validatable, ValidatedCppManagedProperty, AssetManager +from pyhelios.primitives import Rotation, Primitive, AABB, Vertex, Triangle, Material, SwapOnRepeatHandler, PrimitiveType, KDTreeFactoryMaker + +from pydantic import Field, ConfigDict +from osgeo import gdal +from osgeo import osr +from osgeo import ogr +import os +import re +import threading +import numpy as np +from typing import Optional, List, Tuple, Annotated, Type, Any, Dict +import xml.etree.ElementTree as ET +import sys +from collections import defaultdict, deque +from numpy import array, zeros +from numpy.linalg import norm -from typing import Optional, List, Tuple, Annotated, Type, Any import _helios -class ScenePart(Validatable): +gdal.DontUseExceptions() +class ScenePart(Validatable): def __init__(self, id: Optional[str] = "", - origin: Optional[List[float]] = [0.0, 0.0, 0.0], - rotation: Optional[Rotation] = None, + origin: Optional[List[float]] = None, + rotation: Optional[Rotation] = None, scale: Optional[float] = 1.0, bound: Optional[List[float]] = None, - primitives: Optional[List[Primitive]] = None) -> None: - + primitives: Optional[List[Primitive]] = None, + translation: Optional[List[float]] = None, + rotation_method: Optional[str] = 'global') -> None: - self._cpp_object = _helios.ScenePart() - self.id = id - self.origin = origin - self.rotation = rotation - self.scale = scale - self.bound = bound - if primitives is not None: - self.primitives = primitives - else: - self.primitives = [] + self._cpp_object = _helios.ScenePart() + self.id = id + self.origin = origin or [0.0, 0.0, 0.0] + self.rotation = rotation or Rotation(1,0,0,0) + self.scale = scale + self.bound = bound + self.primitives = primitives or [] + self.translation = translation or [0.0, 0.0, 0.0] + self.rotation_method = rotation_method + self.env = None + self.sorh = None + self.is_force_on_ground = 0 + self.primitive_type: PrimitiveType = Field(default=PrimitiveType.NONE) - - id: Optional[str] = ValidatedCppManagedProperty("id") origin: Optional[Tuple[float, float, float]] = ValidatedCppManagedProperty("origin") rotation: Optional[Rotation] = ValidatedCppManagedProperty("rotation") scale: Optional[float] = ValidatedCppManagedProperty("scale") - bound: Optional[tuple[float, float, float]] = ValidatedCppManagedProperty("bound") + bound: Optional[Tuple[float, float, float]] = ValidatedCppManagedProperty("bound") primitives: Optional[List[Primitive]] = ValidatedCppManagedProperty("primitives") + is_force_on_ground: Optional[int] = ValidatedCppManagedProperty("is_force_on_ground") + @classmethod + def read_from_xml(cls: Type['ScenePart'], xml: str) -> 'ScenePart': + root = ET.fromstring(xml) + scene_part = cls() -class Scene(Validatable): + part_element = root.find("part") + if part_element is not None: + filter_element = part_element.find("filter") + if filter_element is not None: + filter_type = filter_element.attrib.get("type") + if filter_type == 'scale': + scene_part.scale = float(filter_element.attrib.get("value")) + elif filter_type == 'translate': + scene_part.translation = [float(v) for v in filter_element.attrib.get("value").split(";")] + elif filter_type == 'rotate': + scene_part._parse_rotation(filter_element) + elif filter_type == 'geotiffloader': + scene_part._load_tiff(xml) + scene_part = cls._validate(scene_part) + + return scene_part + + def _create_params(self, filter_type: ET.Element) -> dict: + params = {} + for param_elem in filter_type.findall('param'): + param_type = param_elem.get("type", "string") + key = param_elem.get("key") + value_str = param_elem.get("value") + + if param_type == "string": + params[key] = value_str + elif param_type in ["boolean", "bool"]: + params[key] = value_str.lower() == "true" + elif param_type == "double": + try: + params[key] = float(value_str) + except ValueError: + raise ValueError(f"Invalid double value for key '{key}': {value_str}") + elif param_type in ["integer", "int"]: + try: + params[key] = int(value_str) + except ValueError: + raise ValueError(f"Invalid integer value for key '{key}': {value_str}") + elif param_type == "vec3": + try: + vec = value_str.split(";") + if len(vec) != 3: + raise ValueError(f"Invalid vec3 value for key '{key}': {value_str}") + params[key] = [float(vec[0]), float(vec[1]), float(vec[2])] + except (ValueError, IndexError): + raise ValueError(f"Invalid vec3 format for key '{key}': {value_str}") + elif param_type == "rotation": + params[key] = Rotation.from_xml_node(param_elem) + else: + raise ValueError(f"Unknown parameter type '{param_type}' for key '{key}'") + + return params + + def _apply_filter(self, filter_type: str, params: dict) -> None: + if filter_type == "scale": + self.scale_loader(params) + elif filter_type == "translate": + self.translate_loader(params) + elif filter_type == "rotate": + self.rotate_loader(params) + if filter_type == "geotiffloader": + self._geotiff_loader(params) + elif filter_type == "objloader": + self._obj_loader(params) + #TODO: implement xyzloader and detailed_voxels_loader + + def load_scene_filter(self, filter_element: ET.Element) -> None: + for filter_elem in filter_element.findall('filter'): + filter_type = filter_elem.attrib.get('type') + params = self._create_params(filter_elem) + + self._apply_filter(filter_type, params) + if filter_type in ["geotiffloader", "detailed_voxels_loader", "xyzloader", "objloader"] and filter_element.find('swap') is not None: + if self.sorh is not None: + raise ValueError("Multiple swap filters are not supported.") + + self.sorh = self.load_scene_part_swaps(filter_elem, params) + + def load_scene_part_swaps(self, filter_elem: ET.Element, params: dict) -> Optional[SwapOnRepeatHandler]: + swap_nodes = filter_elem.findall("swap") + if not swap_nodes: + return None + + sorh = SwapOnRepeatHandler() + + for swap_node in swap_nodes: + # Get swapStep and keepCRS attributes + swap_step = int(swap_node.get("swapStep", 1)) + keep_crs = bool(swap_node.get("keepCRS", sorh.keep_crs)) + + sorh.push_time_to_live(swap_step) + sorh.keep_crs = keep_crs + + # Prepare to collect filters for the swap + swap_filters = deque() + + # Check for null geometry filter + if swap_node.get("force_null", "false") == "true": + swap_filters.append("null") + else: + # Load filters if they exist + for filter_node in swap_node.findall("filter"): + filter_type = filter_node.attrib.get('type') + params = self._create_params(filter_elem) + + self._apply_filter(filter_type, params) + swap_filters.append(params) + + # Add the swap filters to the handler + sorh.swap_filters.append(swap_filters) + + # Prepare the SwapOnRepeatHandler with the scene part + sorh.prepare(self, swap_filters) + return sorh + + def validate_scene_part(self, part_elem: ET.Element) -> bool: + # Check if the scene part is valid + if self.primitive_type == "NONE" and not self.primitives: + path = "#NULL#" + path_type = "path" + found_path = False + for filter_elem in part_elem.findall('filter'): + for param_elem in filter_elem.findall('param'): + if 'key' in param_elem.attrib: + key = param_elem.attrib['key'] + if key in ["filepath", "efilepath"]: + path = param_elem.attrib.get('value', '#NULL#') + path_type = "extended path expression" if key == "efilepath" else "path" + found_path = True + break + if found_path: + break + + # Log a warning or error for invalid scene parts + print(f"XmlSceneLoader::validate_scene_part detected an invalid scene part with id \"{self.id}\".\n" + f"The {path_type} \"{path}\" was given.\n" + "It leads to the loading of an invalid scene part, which is automatically ignored.") + return False + return True + + + def load_scene_part_id(self, part_elem: ET.Element, idx: int) -> bool: + """ + This method loads the `id` attribute of a scene part from an XML element. + If no ID is present, it generates an ID from the index. + + Parameters: + part_elem (ET.Element): The XML element representing the scene part. + idx (int): The index of the part in the scene. + + Returns: + bool: Returns True if the part should be split, False otherwise. + """ + part_id = "" + split_part = True + + # Try to find the 'id' attribute + part_id_attr = part_elem.attrib.get("id") + + if part_id_attr is not None: + part_id = part_id_attr + try: + # Try to convert the ID to an integer (to simulate checking if it's numerical) + int(part_id) + except ValueError: + # If it's not numerical, log a warning and proceed + print(f'Warning: Scene part id "{part_id}" is non-numerical. ' + 'This is not compatible with LAS format specification.') + + # If no id was found, assign the partIndex as the id + if not part_id: + self.id = str(idx) + else: + self.id = part_id + split_part = False + + return split_part + + def scale_loader(self, params: dict) -> None: + for key, value in params.items(): + if key == "scale": + self.scale = float(value) + + def translate_loader(self, params: dict) -> None: + for key, value in params.items(): + if key == "offset": + self.origin = value + if key == "onGround": + self.is_force_on_ground = int(value) + + def rotate_loader(self, params: dict) -> None: + for key, value in params.items(): + if key == "rotation": + rotation = value + self.rotation = rotation.apply_rotation(self.rotation) + + def _parse_rotation(self, filter_element: ET.Element) -> None: + """Helper method to parse rotation data from XML.""" + self.rotation_method = filter_element.attrib.get('rotations', 'global') + rotation_param = filter_element.find('param') + if rotation_param is not None and rotation_param.attrib.get('type') == 'rotation': + self.rotation = [ + (rot.attrib.get('axis', ''), rot.attrib.get('angle_deg', '')) + for rot in rotation_param.findall('rot') + ] + + def _obj_loader(self, params: dict) -> None: + y_is_up = self._determine_up_axis(params) + for key, value in params.items(): + + if 'filepath' in key: + file_path = AssetManager().find_file_by_name(value, auto_add=False) + self._load_obj(file_path, y_is_up) + + def _load_obj(self, file_path: str, y_is_up: bool) -> None: + with open(file_path, 'r') as file: + primitives = [] + vertices = [] + normals = [] + tex_coords = [] + materials = {} + current_mat_type = "default" + mat = Material() + mat.use_vertex_colors = True + mat.mat_file_path = str(file_path) + if current_mat_type not in materials: + materials[current_mat_type] = [] + materials[current_mat_type].append(mat) + + for line in file: + line = line.strip() + if not line or line.startswith("#"): + continue + line_parts = re.split(r"\s+", line) + if line_parts[0] == "v": + vertices.append(self._read_obj_vertex(line_parts, y_is_up)) + + elif line_parts[0] == "vn": + normals.append(self._read_obj_normal_vector(line_parts, y_is_up)) + elif line_parts[0] == "vt": + tex_coords.append([float(line_parts[1]), float(line_parts[2])]) + elif line_parts[0] == "f": + + # We need to check if the name that is in currentmat is in the materials dict, the we need to save to variable the value from the dict. If not we create Material instance set name to currentMat and insert this pair to dict + if current_mat in materials.keys(): + mat = materials[current_mat] + else: + mat = Material() + mat.name = current_mat + materials.update({current_mat_type: [mat]}) + self._read_obj_primitive(primitives, line_parts, vertices, tex_coords, normals, mat, file_path) + elif line_parts[0] == "usemtl": + current_mat = line_parts[1] + + elif line_parts[0] == "mtllib": + file_path_string = os.path.join(os.path.dirname(file_path), line_parts[1]) + new_materials = Material.load_materials(file_path_string) + materials.update(new_materials) + self.primitives.extend(primitives) + + def _determine_up_axis(self, params: dict) -> bool: + up_axis = params.get("up", "z") + if up_axis == "y": + return True + elif up_axis != "z": + raise RuntimeWarning(f"Invalid up axis value: {up_axis}. Defaulting to z-axis.") + return False + + def _read_obj_vertex(self, line_parts: List[str], y_is_up: bool) -> Vertex: + if y_is_up: + position = [float(line_parts[1]), -float(line_parts[3]), float(line_parts[2])] + else: + position = [float(line_parts[1]), float(line_parts[2]), float(line_parts[3])] + + return Vertex(position=position) + + def _read_obj_normal_vector(self, line_parts: List[str], y_is_up: bool): + if y_is_up: + return [float(line_parts[1]), -float(line_parts[3]), float(line_parts[2])] + else: + return [float(line_parts[1]), float(line_parts[2]), float(line_parts[3])] + + def _read_obj_primitive(self, primitives: List[Primitive], line_parts: List[str], vertices: List[Vertex], + texcoords: List[List[float]], normals: List[List[float]], current_mat: Material, mat_file_path: str) -> None: + verts = [] + + for part in line_parts[1:]: + fields = part.split('/') + vi = int(fields[0]) - 1 + ti = int(fields[1]) - 1 if len(fields) > 1 and fields[1] else -1 + ni = int(fields[2]) - 1 if len(fields) > 2 and fields[2] else -1 + vert = self._build_primitive_vertex(vertices[vi], vertices[vi], ti, ni, texcoords, normals) + verts.append(vert) + + if len(verts) == 3: + tri = Triangle(v0=verts[0], v1=verts[1], v2=verts[2]) + tri.material = current_mat + primitives.append(tri) + + elif len(verts) == 4: + tri1 = Triangle(v0=verts[0], v1=verts[1], v2=verts[2]) + tri2 = Triangle(v0=verts[0], v1=verts[2], v2=verts[3]) + tri1.material = current_mat + tri2.material = current_mat + primitives.append(tri1) + primitives.append(tri2) + + def _build_primitive_vertex(self, dst_vert: Vertex, src_vert: Vertex, tex_idx: int, normal_idx: int, + texcoords: List[List[float]], normals: List[List[float]]): + dst_vert = src_vert.clone() + if tex_idx >= 0: + dst_vert.tex_coords = texcoords[tex_idx] + if normal_idx >= 0: + dst_vert.normal = normals[normal_idx] + return dst_vert + + def _geotiff_loader(self, params: dict) -> None: + """Load TIFF file and generate a 3D mesh using GDAL and Open3D.""" + + file_path = AssetManager().find_file_by_name(params["filepath"], auto_add=False) + if file_path is None: + raise FileNotFoundError(f"No filepath was provided for the GeoTIFF file.") + + tiff_dataset = gdal.Open(file_path, gdal.GA_ReadOnly) + if tiff_dataset is None: + raise RuntimeError(f"Failed to open GeoTIFF file at {file_path}") + + coord_ref_sys = tiff_dataset.GetSpatialRef() + if coord_ref_sys is not None: + source_crs = coord_ref_sys.Clone() + else: + source_crs = osr.SpatialReference() + + raster_band = tiff_dataset.GetRasterBand(1) # Get the first raster band (band 1) + if raster_band is None: + raise RuntimeError("Failed to obtain raster band from TIFF file.") + + # Get the raster dimensions (width and height) + raster_width = raster_band.XSize + raster_height = raster_band.YSize + + # envelope data + self.env = {"MinX": 0, "MaxX": 0, "MinY": 0, "MaxY": 0} + layer = tiff_dataset.GetLayer(0) # Assuming only one layer + geom = None + if layer is not None: + # Get spatial filter if available + geom = layer.GetSpatialFilter() + if geom is not None: + # Envelope from geometry + env = geom.GetEnvelope() + self.env["MinX"], self.env["MaxX"], self.env["MinY"], self.env["MaxY"] = env + elif layer is not None: + # Envelope from layer extent + env = ogr.Envelope() + if layer.GetExtent(env, True) == ogr.OGRERR_NONE: + self.env["MinX"], self.env["MaxX"], self.env["MinY"], self.env["MaxY"] = env.MinX, env.MaxX, env.MinY, env.MaxY + else: + raise RuntimeError("Failed to retrieve envelope from layer.") + else: + # Envelope from GeoTransform (fallback) + transform = tiff_dataset.GetGeoTransform() + if transform: + if transform[1] > 0: + self.env["MinX"] = transform[0] + self.env["MaxX"] = self.env["MinX"] + transform[1] * raster_width + else: + self.env["MaxX"] = transform[0] + self.env["MinX"] = self.env["MaxX"] + transform[1] * raster_width + + if transform[5] < 0: # Negative value for pixel height + self.env["MaxY"] = transform[3] + self.env["MinY"] = self.env["MaxY"] + transform[5] * raster_height + else: + self.env["MinY"] = transform[3] + self.env["MaxY"] = self.env["MinY"] + transform[5] * raster_height + + else: + raise RuntimeError("No valid GeoTransform found.") + + width = self.env["MaxX"] - self.env["MinX"] + height = self.env["MaxY"] - self.env["MinY"] + + # Calculate pixel size + pixel_width = width / raster_width + pixel_height = height / raster_height + + #fill Vertices + # Get no-data value from the raster + nodata = raster_band.GetNoDataValue() + if nodata is None: + nodata = float('nan') + + half_pixel_width = pixel_width / 2.0 + half_pixel_height = pixel_height / 2.0 + eps = 1e-6 + vertices = [[None for _ in range(raster_height)] for _ in range(raster_width)] + for x in range(raster_width): + for y in range(raster_height): + z = np.array([0.0], dtype=np.float32) # Buffer to store the pixel value + err = raster_band.ReadRaster( + xoff=x, + yoff=y, + xsize=1, + ysize=1, + buf_xsize=1, + buf_ysize=1, + buf_type=gdal.GDT_Float32, + buf_obj=z + ) + + if err is None: + continue + + z_val = z[0] + if np.abs(z_val - nodata) < eps: + v = None # No data, so no vertex + else: + # Create the vertex with 3D position and texture coordinates + v = Vertex() + v.position = [ + self.env["MinX"]+ x * pixel_width + half_pixel_width, + self.env["MinY"] - y * pixel_height - half_pixel_height + height, + z_val + ] + v.tex_coords = [ + x / raster_width, + (raster_height - y) / raster_height + ] + # Store the vertex in the grid + vertices[x][y] = v + + # Build triangles from the vertices + for x in range(raster_width - 1): + for y in range(raster_height - 1): + # Get the four vertices that form the two triangles in this square + vert0 = vertices[x][y] + vert1 = vertices[x][y + 1] + vert2 = vertices[x + 1][y + 1] + vert3 = vertices[x + 1][y] + + # Create the first triangle (vert0, vert1, vert3) if all vertices are valid + if vert0 is not None and vert1 is not None and vert3 is not None: + tri1 = Triangle(vert0, vert1, vert3) + self.primitives.append(tri1) + + # Create the second triangle (vert1, vert2, vert3) if all vertices are valid + if vert1 is not None and vert2 is not None and vert3 is not None: + tri2 = Triangle(vert1, vert2, vert3) + self.primitives.append(tri2) + + # parse Material + materials = [] + materials = Material.parse_materials(params) + + mat_file_path = params.get("matfile", "") + mat = Material(mat_file_path=mat_file_path, name="default") if not materials else materials[0] + for prim in self.primitives: + prim.material = mat + self.smooth_vertex_normals() + + def compute_transform(self, holistic: Optional[bool] = False) -> None: + for primitive in self.primitives: + primitive.scene_part = self + primitive.rotate(self.rotation) + + # If holistic is True, scale the vertices of the primitive + if holistic: + for vertex in primitive.vertices: + vertex.position[0] *= self.scale + vertex.position[1] *= self.scale + vertex.position[2] *= self.scale + + # Scale the primitive + primitive.scale(self.scale) + # Translate the primitive + primitive.translate(self.origin) + + def smooth_vertex_normals(self): + # Dictionary to store the list of triangles for each vertex + vt_map = defaultdict(list) + + for primitive in self.primitives: + if isinstance(primitive, Triangle): # Check if primitive is a Triangle + for vert in primitive.vertices: # Use the vertices property + vt_map[vert].append(primitive) + + for vertex, triangles in vt_map.items(): + vertex.normal = np.zeros(3) # Initialize vertex normal as a zero vector + for triangle in triangles: + vertex.normal += triangle.get_face_normal() # Accumulate face normals + if len(triangles) > 0: + vertex.normal /= np.linalg.norm(vertex.normal) + + +class Scene(Validatable): + model_config = ConfigDict(arbitrary_types_allowed=True) def __init__(self, scene_parts: Optional[List[ScenePart]], bbox: Optional[AABB] = None, bbox_crs: Optional[AABB] = None) -> None: self._cpp_object = _helios.Scene() - self.scene_parts = scene_parts + self._scene_parts = scene_parts self.bbox = bbox self.bbox_crs = bbox_crs + self._kd_grove_factory: _helios.KDGroveFactory + self.kd_grove: Optional[_helios.KDGrove] + self.kd_factory_type: int = 1 + self.kdt_num_jobs: int = 1 + self.kdt_geom_jobs: int = 1 + self.kdt_sah_loss_nodes: int = 21 + self.primitives: Optional[List[Primitive]] = [] + + @property + def scene_parts(self) -> Optional[List[ScenePart]]: + return self._scene_parts + + @property + def kd_grove_factory(self) -> Optional[_helios.KDGroveFactory]: + return self._kd_grove_factory + + @kd_grove_factory.setter + def kd_grove_factory(self, value: Optional[_helios.KDGroveFactory]) -> None: + self._kd_grove_factory = value + self._cpp_object.kd_grove_factory = value + + def add_scene_part(self, scene_part: ScenePart) -> None: + if scene_part is not None: + self._scene_parts.append(scene_part) + scene_parts: Optional[List[ScenePart]] = ValidatedCppManagedProperty("scene_parts") + kd_grove_factory: Optional[_helios.KDGroveFactory] = ValidatedCppManagedProperty("kd_grove_factory") bbox: Optional[AABB] = ValidatedCppManagedProperty("bbox") - bbox_crs: Optional[AABB] = ValidatedCppManagedProperty + bbox_crs: Optional[AABB] = ValidatedCppManagedProperty("bbox_crs") + primitives: Optional[List[Primitive]] = ValidatedCppManagedProperty("primitives") + + def get_scene_parts_size(self) -> int: + return len(self._scene_parts) + + @classmethod + def read_from_xml(cls: Type['Scene'], filename: str, id: Optional[str] = '', kd_factory_type: Optional[int] = 1, kdt_num_jobs: Optional[int] = 1, kdt_geom_jobs: Optional[int] = 1, kdt_sah_loss_nodes: Optional[int] = 21) -> 'Scene': + + file_path = AssetManager.find_file_by_name(filename, auto_add=True) + tree = ET.parse(file_path) + root = tree.getroot() + + scene_element = root.find(f".//scene[@id='{id}']") + + if scene_element is None: + raise ValueError(f"No scanner found with id: {id}") + + is_dyn_scene = False + holistic = False #TODO Implement holistis for xyzloader + scene_parts = [] + scene = StaticScene._validate(StaticScene()) + for idx, part_elem in enumerate(scene_element.findall('part')): + scene_part = ScenePart() + scene_part.load_scene_filter(part_elem) + is_split_part = scene_part.load_scene_part_id(part_elem, idx) + if not scene_part.validate_scene_part(part_elem): + # If invalid, skip to the next element + continue + + #TODO Implement reading of dynamic scene parts + #TODO: scene loading specification + scene._digest_scene_part(scene_part, idx, holistic, is_split_part, is_dyn_scene) + + scene_parts.append(scene_part) + scene.scene_parts = scene_parts + + scene._cpp_object.primitives = [primitive._cpp_object for primitive in scene.primitives] + + for part in scene.scene_parts: + part._cpp_object.primitives = [primitive._cpp_object for primitive in part.primitives] + for primitive in part.primitives: + if primitive.material is not None: + primitive._cpp_object.material = primitive.material._cpp_object + + if primitive.vertices is not None: + abd = [vertex._cpp_object for vertex in primitive.vertices] + primitive._cpp_object.vertices = abd + + #NOTE: Error starts here. The problem is with Plane class, calculated in Scene::doForceOnGround() + scene._cpp_object.finalize_loading() + + scene.kd_factory_type = kd_factory_type + scene.kdt_num_jobs = kdt_num_jobs + scene.kdt_geom_jobs = kdt_geom_jobs + scene.kdt_sah_loss_nodes = kdt_sah_loss_nodes + kd_tree_type = scene.define_kd_type() + scene._cpp_object.kd_grove_factory = _helios.KDGroveFactory(kd_tree_type) + + return cls._validate(scene) + + + + + def _digest_scene_part(self, scene_part: ScenePart, part_index: int, holistic: Optional[bool] = False, split_part: Optional[bool] = False, dyn_object: Optional[bool] = False) -> None: + scene_part.compute_transform(holistic) + + if not dyn_object: + self.static_objects.append(scene_part) + self._cpp_object.append_static_object_part(scene_part._cpp_object) + + self.primitives.extend(scene_part.primitives) + #TODO: implement splitting of scene parts + ''' + if (split_part): + part_index_offset = len(scene_part.subpart_limit) - 1 + if scene_part.split_subparts(): + part_index += part_index_offset + ''' + + num_vertices = len(scene_part.primitives[0].vertices) + + if num_vertices == 3: + scene_part.primitive_type = PrimitiveType.TRIANGLE + elif num_vertices == 2: + scene_part.primitive_type = PrimitiveType.VOXEL + + def define_kd_type(self): + """Define the KD tree factory based on the factory type and number of jobs.""" + self.kdt_num_jobs = self.kdt_num_jobs or (os.cpu_count() or 1) + self.kdt_geom_jobs = self.kdt_geom_jobs or self.kdt_num_jobs + + # Define a mapping of factory types to corresponding maker functions + factory_makers = { + 1: { + 'single': KDTreeFactoryMaker.make_simple_kd_tree, + 'multi': KDTreeFactoryMaker.make_multithreaded_simple_kd_tree, + }, + 2: { + 'single': KDTreeFactoryMaker.make_sah_kd_tree_factory, + 'multi': KDTreeFactoryMaker.make_multithreaded_sah_kd_tree_factory, + }, + 3: { + 'single': KDTreeFactoryMaker.make_axis_sah_kd_tree_factory, + 'multi': KDTreeFactoryMaker.make_multithreaded_axis_sah_kd_tree_factory, + }, + 4: { + 'single': KDTreeFactoryMaker.make_fast_sah_kd_tree_factory, + 'multi': KDTreeFactoryMaker.make_multithreaded_fast_sah_kd_tree_factory, + } + } + + # Determine whether to use a multi-threaded or single-threaded factory + factory_type = 'multi' if self.kdt_num_jobs > 1 else 'single' + + # Get the appropriate factory maker function based on the type + if self.kd_factory_type in factory_makers: + factory_func = factory_makers[self.kd_factory_type][factory_type] + + # Pass the required parameters to the factory function + if factory_type == 'multi': + # For multithreaded factories, pass node_jobs, geom_jobs, and optionally loss_nodes + if self.kd_factory_type == 1: # SimpleKDTreeFactory doesn't use loss_nodes + return factory_func(self.kdt_num_jobs, self.kdt_geom_jobs) + else: + return factory_func(self.kdt_num_jobs, self.kdt_geom_jobs, self.kdt_sah_loss_nodes) + else: + # For single-threaded factories + if self.kd_factory_type == 1: # SimpleKDTreeFactory doesn't use any arguments + return factory_func() + else: + return factory_func(self.kdt_sah_loss_nodes) + + class Config: + arbitrary_types_allowed = True + +class StaticScene(Scene): + def __init__(self, + static_objects: Optional[List[ScenePart]] = [], + bbox: Optional[AABB] = None, + bbox_crs: Optional[AABB] = None) -> None: + super().__init__(static_objects, bbox, bbox_crs) + self._cpp_object = _helios.StaticScene() + self.static_objects: Optional[List[ScenePart]] = static_objects \ No newline at end of file diff --git a/python/pyhelios/simulation.py b/python/pyhelios/simulation.py index 34cf073a9..dcafd92d1 100644 --- a/python/pyhelios/simulation.py +++ b/python/pyhelios/simulation.py @@ -1,7 +1,7 @@ -from pyhelios.scene import Scene + from pyhelios.utils import Validatable, ValidatedCppManagedProperty -from pyhelios.scanner import Scanner, AbstractDetector from pyhelios.primitives import Rotation, Measurement, Trajectory +from pyhelios.survey import Survey from typing import Optional, List, Callable import _helios @@ -37,15 +37,115 @@ def set_callback(self, callback: Callable): """ self.callback = callback -class Simulation: +class PyheliosSimulation(Validatable): def __init__(self, final_output: Optional[bool] = True, legacy_energy_model: Optional[bool] = False, export_to_file: Optional[bool] = True, num_threads: Optional[int] = 0, num_runs: Optional[int] = 1, callback_frequency: Optional[int] = 0, simulation_frequency: Optional[SimulationCycleCallback] = None, fixed_gps_time: Optional[str] = "", las_output: Optional[bool] = False, las10_output: Optional[bool] = False, zip_output: Optional[bool] = False , split_by_channel: Optional[bool] = False, las_scale: Optional[float] = 0.0001, - kdt_factory: Optional[int] = 4, + kdt_factory_type: Optional[int] = 4, kdt_jobs: Optional[int] = 0, kdt_SAH_loss_nodes: Optional[int] = 32, parallelization_strategy: Optional[int] = 1, chunk_size: Optional[int] = 32, - warehouse_factor: Optional[int] = 1) -> None: - self._cpp_object = _helios.Simulation() + warehouse_factor: Optional[int] = 1, survey: Optional[Survey] = None, survey_path: Optional[str] = "", + assets_path: Optional[str] = "", output_path: Optional[str] = "" + ) -> None: + self._cpp_object = _helios.PyheliosSimulation() + self.survey_path = survey_path + self.assets_path = assets_path + self.output_path = output_path + self.survey = survey + self.final_output = final_output + self.legacy_energy_model = legacy_energy_model + self.export_to_file = export_to_file + self.num_threads = num_threads + self.num_runs = num_runs + self.callback_frequency = callback_frequency + self.simulation_frequency = simulation_frequency + self.fixed_gps_time = fixed_gps_time + self.las_output = las_output + + self.las10_output = las10_output + self.zip_output = zip_output + self.split_by_channel = split_by_channel + self.las_scale = las_scale + self.kdt_factory_type = kdt_factory_type + self.kdt_jobs = kdt_jobs + self.kdt_SAH_loss_nodes = kdt_SAH_loss_nodes + self.parallelization_strategy = parallelization_strategy + self.chunk_size = chunk_size + self.warehouse_factor = warehouse_factor + + + self.is_started = False + self.is_paused = False + self.is_stopped = False + + + final_output: Optional[bool] = ValidatedCppManagedProperty("final_output") + legacy_energy_model: Optional[bool] = ValidatedCppManagedProperty("legacy_energy_model") + export_to_file: Optional[bool] = ValidatedCppManagedProperty("export_to_file") + num_threads: Optional[int] = ValidatedCppManagedProperty("num_threads") + num_runs: Optional[int] = ValidatedCppManagedProperty("num_runs") + callback_frequency: Optional[int] = ValidatedCppManagedProperty("callback_frequency") + simulation_frequency: Optional[SimulationCycleCallback] = ValidatedCppManagedProperty("simulation_frequency") + fixed_gps_time: Optional[str] = ValidatedCppManagedProperty("fixed_gps_time") + las_output: Optional[bool] = ValidatedCppManagedProperty("las_output") + las10_output: Optional[bool] = ValidatedCppManagedProperty("las10_output") + zip_output: Optional[bool] = ValidatedCppManagedProperty("zip_output") + split_by_channel: Optional[bool] = ValidatedCppManagedProperty("split_by_channel") + las_scale: Optional[float] = ValidatedCppManagedProperty("las_scale") + kdt_factory_type: Optional[int] = ValidatedCppManagedProperty("kdt_factory") + kdt_jobs: Optional[int] = ValidatedCppManagedProperty("kdt_jobs") + kdt_SAH_loss_nodes: Optional[int] = ValidatedCppManagedProperty("kdt_SAH_loss_nodes") + parallelization_strategy: Optional[int] = ValidatedCppManagedProperty("parallelization_strategy") + chunk_size: Optional[int] = ValidatedCppManagedProperty("chunk_size") + warehouse_factor: Optional[int] = ValidatedCppManagedProperty("warehouse_factor") + survey: Optional[Survey] = ValidatedCppManagedProperty("survey") + + + + def start(self): + if self.is_started: + raise ValueError("Simulation is already started") + + if self.survey is None: + self.load_survey(self.survey_path) + self.is_started = True + + # Add output here later + + self.build_pulse_thread_pool() + + def pause(self): + pass + + def stop(self): + pass + def resume(self): + pass + + def join(self): + pass + + def load_survey(self, survey_path: str): + self.survey = Survey.from_xml(survey_path) + + def add_rotation(self, rotation: Rotation): + pass + + def add_scale_filter(self, scale_filter: float): + pass + + def add_translate_filter(self, translate_filter: float): + pass + + def build_pulse_thread_pool(self): + + #FOR NOW JUST CALL IT FROM THE C++ SIDE + self._cpp_object.build_pulse_thread_pool() + + + + + diff --git a/python/pyhelios/survey.py b/python/pyhelios/survey.py index 491c26961..6311cc7a8 100644 --- a/python/pyhelios/survey.py +++ b/python/pyhelios/survey.py @@ -1,24 +1,318 @@ from pyhelios.scene import Scene -from pyhelios.utils import Validatable, ValidatedCppManagedProperty -from pyhelios.scanner import Scanner, AbstractDetector -from pyhelios.primitives import Rotation -from typing import Optional, List +from pyhelios.utils import Validatable, ValidatedCppManagedProperty, AssetManager, PyHeliosException +from pyhelios.scanner import Scanner, AbstractDetector, ScannerSettings, SingleScanner, MultiScanner +from pyhelios.platforms import PlatformSettings, Platform +from pyhelios.primitives import Rotation, FWFSettings, SimulationCycleCallback, Measurement, Trajectory +import threading +from pyhelios.leg import Leg + +import os +from threading import Thread +from typing import Optional, List, Dict +import xml.etree.ElementTree as ET import _helios +from datetime import datetime, timezone +import time class Survey(Validatable): def __init__(self, name: Optional[str] = "", num_runs: Optional[int] = -1, - sim_speed_factor: Optional[float] = 1., scanner: Optional[Scanner] = None) -> None: + sim_speed_factor: Optional[float] = 1., scanner: Optional[Scanner] = None, + legs: Optional[List[Leg]] = None) -> None: self._cpp_object = _helios.Survey() self.name = name self.num_runs = num_runs self.sim_speed_factor = sim_speed_factor self.scanner = scanner + self.legs = legs or [] + + self.is_legacy_energy_model: Optional[bool] = False # this is used in c++ Simulation class + self.fixed_gps_time_start: Optional[str] = "" + + self.is_started: Optional[bool] = False + self.is_paused: Optional[bool] = False + self.is_stopped: Optional[bool] = False + self.is_finished: Optional[bool] = False + self.output_path: Optional[str] = "" + self.las_scale: Optional[float] = 0.0001 + self.las_output: Optional[bool] = False + self.las_10: Optional[bool] = False + self.zip_output: Optional[bool] = False + self.split_by_channel: Optional[bool] = False + self.playback: Optional[_helios.SurveyPlayback] = None + self.callback: Optional[_helios.SimulationCycleCallback] = None + + self.kd_factory_type: Optional[int] = 4 + self.kdt_num_jobs: Optional[int] = 0 + self.kdt_geom_jobs: Optional[int] = 0 + self.kdt_sah_loss_nodes: Optional[int] = 32 + self.parallelization_strategy: Optional[int] = 1 + + self.export_to_file: Optional[bool] = False + self.write_waveform: Optional[bool] = False + self.calc_echowidth: Optional[bool] = False + self.fullwavenoise: Optional[bool] = False + self.is_platform_noise_disabled: Optional[bool] = False + self.final_output: Optional[bool] = False + self.scanner_settings_templates: Dict[str, ScannerSettings] = {} + self.platform_settings_templates: Dict[str, PlatformSettings] = {} + self.callback_frequency: Optional[int] = 0 + name: Optional[str] = ValidatedCppManagedProperty("name") num_runs: Optional[int] = ValidatedCppManagedProperty("num_runs") sim_speed_factor: Optional[float] = ValidatedCppManagedProperty("sim_speed_factor") scanner: Optional[Scanner] = ValidatedCppManagedProperty("scanner") + legs: Optional[List[Leg]] = ValidatedCppManagedProperty("legs") + + @classmethod + def from_xml(cls, filename: str) -> 'Survey': + # Locate the XML file using the AssetManager + file_path = AssetManager().find_file_by_name(filename, auto_add=True) + + # Parse the XML file + tree = ET.parse(file_path) + root = tree.getroot() + + # Extract main survey attributes + survey_node = root.find('survey') + if survey_node is None: + raise ValueError("Invalid XML file, missing 'survey' tag") + name = survey_node.get('name', "") + # Optional scanner parsing if specified in XML + scanner_path, scanner_id = survey_node.get('scanner').split('#') if survey_node.get('scanner') else (None, None) + scanner = Scanner.from_xml(scanner_path, scanner_id) if scanner_path else None + + platform_path, platform_id = survey_node.get('platform').split('#') if survey_node.get('scanner') else (None, None) + scanner.platform = Platform.from_xml(platform_path, platform_id) if platform_path else None + + if survey_node.find("FWFSettings") is not None: + scanner.apply_settings_FWF(FWFSettings.from_xml_node(survey_node.find("FWFSettings"))) + + num_runs = int(survey_node.get('numRuns', 1)) + + speed = float(survey_node.get('simSpeedFactor', 1.0)) + + sim_speed_factor = 1 / speed if speed > 0 else 1.0 + + #TODO: add detector overloading - XmlSurveyLoader::handleCoreOverloading + # TODO: add interpolated legs + platform - \ No newline at end of file + platform_settings_templates = {} + for template_node in root.findall('platformSettings'): + template_id = template_node.get('id') + + if template_id: + template = PlatformSettings.from_xml_node(template_node) + platform_settings_templates[template_id] = template + + scanner_settings_templates = {} + for template_node in root.findall('scannerSettings'): + template_id = template_node.get('id') + + if template_id: + template = ScannerSettings.from_xml_node(template_node) + scanner_settings_templates[template_id] = template + + # Extract legs information + legs = [] + for idx, leg_node in enumerate(survey_node.findall('leg')): + + leg = Leg.from_xml(leg_node, idx, platform_settings_templates=platform_settings_templates, scanner_settings_templates = scanner_settings_templates) + legs.append(leg) + # TODO: waypoints for interpolated legs + + # integrateSurveyAndLegs + if(scanner.beam_deflector is not None): + for leg in legs: + if not (leg.scanner_settings.vertical_resolution == 0 and leg.scanner_settings.horizontal_resolution == 0): + leg.scanner_settings.scan_frequency = (leg.scanner_settings.pulse_frequency * leg.scanner_settings.vertical_resolution) / (2 * scanner.beam_deflector.scan_angle_max) + leg.scanner_settings.head_rotation = leg.scanner_settings.horizontal_resolution * leg.scanner_settings.scan_frequency + + # validate Survey to derived func!!!!!!!!!!! + for leg in legs: + scan_settings = leg.scanner_settings + beam_deflector = scanner.beam_deflector + if scan_settings.scan_frequency < beam_deflector.scan_freq_min: + raise ValueError(f"Leg {leg.serial_id} scan frequency is below the minimum frequency of the beam deflector.\n The requested scanning frequency cannot be achieved by this scanner. \n") + if scan_settings.scan_frequency > beam_deflector.scan_freq_max and beam_deflector.scan_freq_max != 0: + raise ValueError(f"Leg {leg.serial_id} scan frequency is above the maximum frequency of the beam deflector.\n The requested scanning frequency cannot be achieved by this scanner.\n") + + #SCene parsing + scene_path, scene_id = survey_node.get('scene').split('#') if survey_node.get('scene') else (None, None) + + kd_factory_type = 4 + kdt_num_jobs = os.cpu_count() + kdt_sah_loss_nodes = 32 + scanner.platform.scene = Scene.read_from_xml(scene_path, scene_id, kd_factory_type=kd_factory_type, kdt_num_jobs=kdt_num_jobs, kdt_sah_loss_nodes=kdt_sah_loss_nodes) + # CHECK AND IF SPECTRAL LIB IS WORKING add it + + for scene_part in scanner.platform.scene.scene_parts: + if scene_part.sorh is None: + continue + + num_primitives = len(scene_part.primitives) + baseline_primitives = scene_part.sorh.baseline.primitives + for i in range(num_primitives): + baseline_primitives.primitives[i].material = scene_part.primitives[i].material + + + #TODO: add shifting of interpolated objects and a random offset + + pos1 = scanner.platform.scene._cpp_object.bbox_crs.vertices[0].position + pos2 = scanner.platform.scene._cpp_object.bbox_crs.vertices[1].position + + # Check that the positions are valid tuples with the same length (x, y, z) + if len(pos1) != len(pos2): + raise ValueError("Positions of the vertices must have the same length (x, y, z).") + + # Calculate the bounding box size (assuming 3D coordinates: x, y, z) + bbox_size = tuple(pos2[i] - pos1[i] for i in range(len(pos1))) + + # Halve the bbox size (center the bounding box) + bbox_size = tuple(coord / 2 for coord in bbox_size) + + # Calculate the center of the bounding box for shifting purposes + shift = tuple(pos1[i] + bbox_size[i] for i in range(len(pos1))) + + for leg in legs: + if leg.platform_settings is not None: + leg.platform_settings.position = (leg.platform_settings.position[0] - shift[0], leg.platform_settings.position[1] - shift[1], leg.platform_settings.position[2] - shift[2]) + + scanner._cpp_object.initialize_sequential_generators() + scanner.platform.scene._cpp_object.build_kd_grove() + + # Create the validated Survey instance + survey_instance = cls( + name=name, + scanner=scanner, + num_runs=num_runs, + sim_speed_factor=sim_speed_factor, + legs=legs + ) + + survey_instance.scanner.write_waveform = survey_instance.write_waveform + survey_instance.scanner.calc_echowidth = survey_instance.calc_echowidth + survey_instance.scanner.fullwavenoise = survey_instance.fullwavenoise + survey_instance.scanner.is_platform_noise_disabled = survey_instance.is_platform_noise_disabled + survey_instance.scanner_settings_templates = scanner_settings_templates + survey_instance.platform_settings_templates = platform_settings_templates + # Validate the created instance using _validate method + survey_instance = cls._validate(survey_instance) + + return survey_instance + + def run(self, num_threads: Optional[int] = 0, chunk_size: Optional[int] = 32, warehouse_factor: Optional[int] = 1, + callback_frequency: Optional[int] = 0, blocking: Optional[bool] = True, export_to_file: Optional[bool] = True): + + if num_threads == 0: + num_threads = os.cpu_count() + + fms = None + + if export_to_file: #TODO: finish with export to file + fms = _helios.FMSFacadeFactory().build_facade(self.output_path, self.las_scale, self.las_output, self.las_10, self.zip_output, self.split_by_channel, self._cpp_object) + + det = self.scanner.scanning_device.detector if isinstance(self.scanner, SingleScanner) else self.scanner.scanning_devices[0].detector + + ptpf = _helios.PulseThreadPoolFactory(self.parallelization_strategy, num_threads-1, det.accuracy, chunk_size, warehouse_factor) + pulse_thread_pool = ptpf.make_pulse_thread_pool() + + current_time = datetime.now(timezone.utc).isoformat(timespec='seconds') + self.playback = _helios.SurveyPlayback(self._cpp_object, _helios.FMSFacade(), self.parallelization_strategy, pulse_thread_pool, chunk_size, current_time, self.is_legacy_energy_model, export_to_file) + self.playback.callback = self.callback._cpp_object + self.playback.callback_frequency = callback_frequency + self.scanner._cpp_object.cycle_measurements_mutex = None + + self.scanner._cpp_object.cycle_measurements = [] + self.scanner._cpp_object.cycle_trajectories = [] + self.scanner._cpp_object.all_measurements = [] + self.scanner._cpp_object.all_trajectories = [] + self.scanner._cpp_object.all_output_paths = [] + #NOTE: threaded version was realised, but hidden for this version, to simplify the usage + self.playback.start() + + self.is_started = True + + def pause(self): + if not self.is_started: + raise PyHeliosException("PyHeliosSimulation was not started so it cannot be paused") + + if self.is_stopped: + raise PyHeliosException("PyHeliosSimulation was stopped so it cannot be paused") + + + if self.is_finished: + raise PyHeliosException("PyHeliosSimulation was finished so it cannot be paused") + + self.playback.pause(True) + self.is_paused = True + + def stop(self): + if not self.is_started: + raise PyHeliosException("PyHeliosSimulation was not started so it cannot be stopped") + + if self.is_stopped: + raise PyHeliosException("PyHeliosSimulation was already stopped") + + if self.is_finished: + raise PyHeliosException("PyHeliosSimulation was finished so it cannot be stopped") + + self.playback.stop() + self.is_stopped = True + + def resume(self): + if not self.is_started: + raise PyHeliosException("PyHeliosSimulation was not started so it cannot be resumed") + + if self.is_stopped: + raise PyHeliosException("PyHeliosSimulation was stopped so it cannot be resumed") + + if self.playback.is_finished: + raise PyHeliosException("PyHeliosSimulation was finished so it cannot be resumed") + if not self.is_paused: + raise PyHeliosException("PyHeliosSimulation was not paused so it cannot be resumed") + + self.playback.resume() + self.is_paused = False + + def join_output(self): + if not self.is_started or self.is_paused: + raise PyHeliosException("PyHeliosSimulation is not running so it cannot be joined") + + outpath = "" + if self.export_to_file: + outpath = str(self.scanner.fms.write.getMeasurementWriterOutputPath()) + + if self.callback_frequency > 0 and self.callback is not None: + if not self.playback.is_finished: + # Return empty vectors if the simulation is not finished yet + return ([], [], outpath, [outpath], False) + else: + # Return collected data if the simulation is finished + self.is_finished = True + return (self.scanner.all_measurements, + self.scanner.all_trajectories, + outpath, + self.scanner.all_output_paths, + True) + + if self.thread and self.thread.is_alive(): + self.thread.join() + + if self.playback.fms is not None: + self.playback.fms.disconnect() + + self.finished = True + + + if not self.final_output: + return ([], [], outpath, [], False) + + return (self.scanner.all_measurements, + self.scanner.all_trajectories, + outpath, + self.scanner.all_output_paths, + True) + \ No newline at end of file diff --git a/python/pyhelios/utils.py b/python/pyhelios/utils.py index 8e5abac6e..5199aa96b 100644 --- a/python/pyhelios/utils.py +++ b/python/pyhelios/utils.py @@ -1,6 +1,11 @@ -from pydantic import validate_call, GetCoreSchemaHandler +import os +import importlib_resources as resources +from pydantic import validate_call, GetCoreSchemaHandler from pydantic_core import core_schema -from typing import Optional, Type, Any +from typing import Optional, Type, Any, Callable +import random +import math +import numpy as np class Validatable: @classmethod @@ -24,7 +29,7 @@ def __get__(self, obj, objtype=None): def __set__(self, obj, value): @validate_call - def _validated_setter(value: obj.__annotations__[self.name]): + def _validated_setter(value: self._get_annotation(obj)): if isinstance(value, list): cpp_value = [getattr(item, "_cpp_object") if hasattr(item, "_cpp_object") else item for item in value] else: @@ -33,4 +38,168 @@ def _validated_setter(value: obj.__annotations__[self.name]): self._values[obj] = value _validated_setter(value) + def _get_annotation(self, obj): + for cls in obj.__class__.__mro__: + if self.name in getattr(cls, '__annotations__', {}): + return cls.__annotations__[self.name] + raise AttributeError(f"'{obj.__class__.__name__}' object has no annotation for '{self.name}'") +class AssetManager: + _instance = None + _assets = set() + + def __new__(cls): + if cls._instance is None: + cls._instance = super(AssetManager, cls).__new__(cls) + cls._initialize_default_assets() + return cls._instance + + @classmethod + def _initialize_default_assets(cls): + # Attempt to initialize default paths and verify they are valid + current_dir = os.getcwd() + pyhelios_dir = str(resources.files("pyhelios")) + pyhelios_data_dir = str(resources.files("pyhelios") / "data") + + cls._assets.add(current_dir) + if os.path.exists(pyhelios_dir): + cls._assets.add(pyhelios_dir) + else: + print(f"Pyhelios directory does not exist: {pyhelios_dir}") + + if os.path.exists(pyhelios_data_dir): + cls._assets.add(pyhelios_data_dir) + else: + print(f"Pyhelios data directory does not exist: {pyhelios_data_dir}") + + @classmethod + def add_asset(cls, path: str): + cls._assets.add(path) + + @classmethod + def locate_asset(cls, filename: str) -> Optional[str]: + # Look for the file in all registered assets + for path in cls._assets: + potential_path = os.path.join(path, filename) + if os.path.exists(potential_path): + return potential_path + return None + + @classmethod + def find_file_by_name(cls, filename: str, auto_add: bool = False) -> str: + if os.path.isabs(filename): + if os.path.exists(filename): + if auto_add: + cls.add_asset(os.path.dirname(filename)) + return filename + else: + raise FileNotFoundError(f"File not found: {filename}") + + located_path = cls.locate_asset(filename) + if located_path: + return located_path + + raise FileNotFoundError(f"File not found in registered assets: {filename} \n existed assets: {cls._assets}") + + + +def calc_propagation_time_legacy(time_wave, num_bins, bin_size, pulse_length, pulse_length_divisor): + # Prepare variables + + tau = pulse_length / pulse_length_divisor + peak_value = 0 + peak_index = 0 + + # Do forward iterative process to compute nodes and pick max + for i in range(num_bins): + t = i * bin_size + t_tau = t / tau + pt = (t_tau ** 2) * math.exp(-t_tau) + time_wave[i] = pt + + if pt > peak_value: + peak_value = pt + peak_index = i + + return peak_index + +class RandomnessGenerator: + """Randomness generator that mimics the behavior of the C++ version.""" + + def __init__(self, mode="AUTO_SEED", seed=None): + self.mode = mode + self.urd_gen = None # The random generator + self.urd = None # The uniform distribution + + if self.mode == "AUTO_SEED": + self.seed = random.SystemRandom().randint(0, 2**32 - 1) # Auto seed + elif self.mode == "FIXED_SEED_DOUBLE" or self.mode == "FIXED_SEED_LONG": + if seed is None: + raise ValueError("Fixed seed mode requires a seed") + self.seed = seed + else: + raise ValueError(f"Unknown mode: {self.mode}") + + random.seed(self.seed) + + def compute_uniform_real_distribution(self, lower_bound=0.0, upper_bound=1.0): + """Compute the uniform real distribution with given bounds.""" + # For Python's random, no need to store the distribution explicitly + self.urd_gen = random.uniform # Directly use random.uniform + + def uniform_real_distribution_next(self): + """Return the next value in the uniform distribution.""" + if self.urd_gen is None: + self.compute_uniform_real_distribution(0.0, 1.0) + + return self.urd_gen(0.0, 1.0) + + +class PyHeliosException(Exception): + """PyHelios exception class""" + + def __init__(self, msg): + """PyHeliosException constructor: Build a new exception. + + Arguments: + msg -- message for the exception + """ + super().__init__(msg) + + +def create_property(property_name, cpp_function_setter, cpp_function_getter=None, index_function=None): + """ + Factory function to create a property getter and setter for scanning_device and _cpp_object. + + Args: + property_name (str): The name of the property. + cpp_function_setter (str): The C++ method to call to set the property on _cpp_object. + cpp_function_getter (str, optional): The C++ method to call to get the property from _cpp_object. + index_function (function, optional): Function to get the index of the active scanning device (if multi-device). + + Returns: + property: A property object with getter and setter methods. + """ + + def getter(self): + if index_function: + # If index_function is provided, use it to get the active device's property + active_device = self.scanning_devices[index_function(self)] + return getattr(active_device, property_name) + return getattr(self.scanning_device, property_name) + + def setter(self, value): + if index_function: + # If index_function is provided, set the value for the active device + active_device = self.scanning_devices[index_function(self)] + setattr(active_device, property_name, value) + if cpp_function_setter: + cpp_value = value._cpp_object if hasattr(value, '_cpp_object') else value + getattr(self._cpp_object, cpp_function_setter)(cpp_value, index_function(self)) + else: + setattr(self.scanning_device, property_name, value) + if cpp_function_setter: + cpp_value = value._cpp_object if hasattr(value, '_cpp_object') else value + getattr(self._cpp_object, cpp_function_setter)(cpp_value, 0) + + return property(getter, setter) diff --git a/src/python/EnergyModelWrap.h b/src/python/EnergyModelWrap.h new file mode 100644 index 000000000..ca396fe09 --- /dev/null +++ b/src/python/EnergyModelWrap.h @@ -0,0 +1,34 @@ +#include +#include +#include +#include "EnergyModel.h" // Include your EnergyModel header +#include "ScanningDevice.h" // Include your ScanningDevice header + +namespace py = pybind11; + +class EnergyModelWrap : public EnergyModel { +public: + // Constructor for wrapping + EnergyModelWrap(ScanningDevice const &sd) : EnergyModel(sd) {} + + // Implement all pure virtual methods + double computeIntensity(double const incidenceAngle, double const targetRange, Material const &mat, int const subrayRadiusStep) override { + PYBIND11_OVERLOAD_PURE(double, EnergyModel, computeIntensity, incidenceAngle, targetRange, mat, subrayRadiusStep); + } + + double computeReceivedPower(ModelArg const &args) override { + PYBIND11_OVERLOAD_PURE(double, EnergyModel, computeReceivedPower, args); + } + + double computeEmittedPower(ModelArg const &args) override { + PYBIND11_OVERLOAD_PURE(double, EnergyModel, computeEmittedPower, args); + } + + double computeTargetArea(ModelArg const &args) override { + PYBIND11_OVERLOAD_PURE(double, EnergyModel, computeTargetArea, args); + } + + double computeCrossSection(ModelArg const &args) override { + PYBIND11_OVERLOAD_PURE(double, EnergyModel, computeCrossSection, args); + } +}; \ No newline at end of file diff --git a/src/python/GLMTypeCaster.h b/src/python/GLMTypeCaster.h index 59c29d6bd..5a9b3c609 100755 --- a/src/python/GLMTypeCaster.h +++ b/src/python/GLMTypeCaster.h @@ -31,6 +31,32 @@ namespace pybind11 { namespace detail { } }; + template <> struct type_caster { + public: + PYBIND11_TYPE_CASTER(glm::dvec2, _("dvec2")); + + // Load function to convert from Python tuple/list to glm::dvec2 + bool load(handle src, bool) { + if (!src) return false; + if (!pybind11::isinstance(src)) return false; + + pybind11::tuple t = pybind11::cast(src); + if (t.size() != 2) return false; // glm::dvec2 needs two elements + + value = glm::dvec2( + pybind11::cast(t[0]), + pybind11::cast(t[1]) + ); + + return true; + } + + // Cast function to convert glm::dvec2 to Python tuple + static handle cast(const glm::dvec2& src, return_value_policy, handle) { + return pybind11::make_tuple(src.x, src.y).release(); + } + }; + } } #endif diff --git a/src/python/KDTreeFactoryWrapper.h b/src/python/KDTreeFactoryWrapper.h new file mode 100644 index 000000000..564991375 --- /dev/null +++ b/src/python/KDTreeFactoryWrapper.h @@ -0,0 +1,30 @@ +#include + +class KDTreeFactoryWrap : public KDTreeFactory { +public: + using KDTreeFactory::KDTreeFactory; // Inherit constructors + + // Override pure virtual clone method + KDTreeFactory* clone() const override { + PYBIND11_OVERLOAD_PURE( + KDTreeFactory*, // Return type + KDTreeFactory, // Parent class + clone // Function name + ); + } + + KDTreeNodeRoot* makeFromPrimitivesUnsafe( + std::vector& primitives, + bool const computeStats=false, + bool const reportStats=false + ) override { + PYBIND11_OVERLOAD_PURE( + KDTreeNodeRoot*, // Return type + KDTreeFactory, // Parent class + makeFromPrimitivesUnsafe, // Method name + primitives, // Arguments + computeStats, + reportStats + ); + } +}; \ No newline at end of file diff --git a/src/python/NoiseSourceWrap.h b/src/python/NoiseSourceWrap.h index 0e8eef1b7..fcb9ca025 100755 --- a/src/python/NoiseSourceWrap.h +++ b/src/python/NoiseSourceWrap.h @@ -9,3 +9,26 @@ class NoiseSourceWrap : public NoiseSource { throw std::runtime_error("Called pure virtual function noiseFunction()"); } }; + +template +class RandomNoiseSourceWrap : public RandomNoiseSource { +public: + using RandomNoiseSource::RandomNoiseSource; + + + // Override the pure virtual method in Python + std::string getRandomNoiseType() override { + PYBIND11_OVERRIDE_PURE( + std::string, // Return type + RandomNoiseSource, // Parent class + getRandomNoiseType // Function name + ); + } + RealType noiseFunction() override { + PYBIND11_OVERLOAD_PURE( + RealType, // Return type + RandomNoiseSource, // Parent class + noiseFunction, // Method name in C++ + ); + } +}; \ No newline at end of file diff --git a/src/python/PulseThreadPoolInterfaceWrap.h b/src/python/PulseThreadPoolInterfaceWrap.h new file mode 100755 index 000000000..b7fd6f8e8 --- /dev/null +++ b/src/python/PulseThreadPoolInterfaceWrap.h @@ -0,0 +1,52 @@ +#include + +class PulseThreadPoolInterfaceWrap : public PulseThreadPoolInterface { + public: + using PulseThreadPoolInterface::PulseThreadPoolInterface; // Inherit constructors + + void run_pulse_task(TaskDropper< + PulseTask, + PulseThreadPoolInterface, + std::vector>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource& +#if DATA_ANALYTICS >= 2 + , std::shared_ptr +#endif + > &dropper) override { + PYBIND11_OVERLOAD_PURE( + void, // Return type + PulseThreadPoolInterface, // Parent class + run_pulse_task, // Name of function in Python + dropper // Arguments + ); + } + + bool try_run_pulse_task(TaskDropper< + PulseTask, + PulseThreadPoolInterface, + std::vector>&, + RandomnessGenerator&, + RandomnessGenerator&, + NoiseSource& +#if DATA_ANALYTICS >= 2 + , std::shared_ptr +#endif + > &dropper) override { + PYBIND11_OVERLOAD_PURE( + bool, // Return type + PulseThreadPoolInterface, // Parent class + try_run_pulse_task, // Name of function in Python + dropper // Arguments + ); + } + + void join() override { + PYBIND11_OVERLOAD_PURE( + void, // Return type + PulseThreadPoolInterface, // Parent class + join, // Name of function in Python + ); + } +}; \ No newline at end of file diff --git a/src/python/PyHeliosSimulation.cpp b/src/python/PyHeliosSimulation.cpp index 69b1ffcdc..c8cdd903d 100644 --- a/src/python/PyHeliosSimulation.cpp +++ b/src/python/PyHeliosSimulation.cpp @@ -34,26 +34,6 @@ PyHeliosSimulation::PyHeliosSimulation( int chunkSize, int warehouseFactor ){ - std::cout << "Constructor called with:" << std::endl; - std::cout << "surveyPath: " << surveyPath << std::endl; - std::cout << "outputPath: " << outputPath << std::endl; - std::cout << "numThreads: " << numThreads << std::endl; - std::cout << "lasOutput: " << lasOutput << std::endl; - std::cout << "las10: " << las10 << std::endl; - std::cout << "zipOutput: " << zipOutput << std::endl; - std::cout << "splitByChannel: " << splitByChannel << std::endl; - std::cout << "kdtFactory: " << kdtFactory << std::endl; - std::cout << "kdtJobs: " << kdtJobs << std::endl; - std::cout << "kdtSAHLossNodes: " << kdtSAHLossNodes << std::endl; - std::cout << "parallelizationStrategy: " << parallelizationStrategy << std::endl; - std::cout << "chunkSize: " << chunkSize << std::endl; - std::cout << "warehouseFactor: " << warehouseFactor << std::endl; - - std::cout << "assetsPath:" << std::endl; - for (const auto& path : assetsPath) { - std::cout << path << std::endl; - } - this->fixedGpsTimeStart = ""; this->lasOutput = lasOutput; this->las10 = las10; diff --git a/src/python/PyHeliosSimulation.h b/src/python/PyHeliosSimulation.h index 99998d53c..ac2127d6c 100644 --- a/src/python/PyHeliosSimulation.h +++ b/src/python/PyHeliosSimulation.h @@ -154,6 +154,8 @@ class PyHeliosSimulation{ * * @return Scanner used by the simulation */ + void setSurvey(Survey & survey) {this->survey = std::make_shared(survey);} + Scanner * getScanner() {return survey->scanner.get();} /** diff --git a/src/python/ScannerWrap.h b/src/python/ScannerWrap.h index 65a9d5640..4c80e2951 100755 --- a/src/python/ScannerWrap.h +++ b/src/python/ScannerWrap.h @@ -20,7 +20,6 @@ class ScannerWrap : public Scanner { public: - // using Scanner::Scanner; // Inherit constructors ScannerWrap() : Scanner("", std::list()) {} @@ -36,14 +35,30 @@ class ScannerWrap : public Scanner { ScannerWrap(Scanner& scanner) : Scanner(scanner) {} - std::shared_ptr clone() override { - PYBIND11_OVERLOAD_PURE( - std::shared_ptr, - Scanner, - clone - ); + std::shared_ptr cycle_measurements_mutex; + + // Getter for cycle_measurements_mutex + std::shared_ptr get_mutex() const { + return cycle_measurements_mutex; } + // Setter for cycle_measurements_mutex + void set_mutex(std::shared_ptr mutex=nullptr) { + if (!mutex) { + cycle_measurements_mutex = std::make_shared(); + } else { + cycle_measurements_mutex = std::move(mutex); + + } + Scanner::cycleMeasurementsMutex = cycle_measurements_mutex; + Scanner::allMeasurementsMutex = cycle_measurements_mutex; + } + + std::shared_ptr clone() override { + // Create a copy of the current ScannerWrap object + // This can be a shallow or deep copy depending on your needs + return std::make_shared(*this); + } void prepareSimulation(bool const legacyEnergyModel) override { PYBIND11_OVERLOAD_PURE( void, diff --git a/src/python/ScanningPulseProcessWrap.h b/src/python/ScanningPulseProcessWrap.h new file mode 100755 index 000000000..dc988f232 --- /dev/null +++ b/src/python/ScanningPulseProcessWrap.h @@ -0,0 +1,34 @@ +#include + +class ScanningPulseProcessWrap : public ScanningPulseProcess { +public: + using ScanningPulseProcess::ScanningPulseProcess; // Inherit constructors + + // Override handlePulseComputation in Python + void handlePulseComputation(SimulatedPulse const &sp) override { + PYBIND11_OVERRIDE_PURE( + void, // Return type + ScanningPulseProcess, // Parent class + handlePulseComputation, // Function name + sp // Argument(s) + ); + } + + // Override onLegComplete in Python + void onLegComplete() override { + PYBIND11_OVERRIDE( + void, // Return type + ScanningPulseProcess, // Parent class + onLegComplete // Function name + ); + } + + // Override onSimulationFinished in Python + void onSimulationFinished() override { + PYBIND11_OVERRIDE( + void, // Return type + ScanningPulseProcess, // Parent class + onSimulationFinished // Function name + ); + } +}; \ No newline at end of file diff --git a/src/python/SimulationCycleCallbackWrap.h b/src/python/SimulationCycleCallbackWrap.h index b5f3b7f94..746ed1f97 100755 --- a/src/python/SimulationCycleCallbackWrap.h +++ b/src/python/SimulationCycleCallbackWrap.h @@ -5,32 +5,40 @@ namespace py = pybind11; -class [[gnu::visibility("default")]] SimulationCycleCallbackWrap : public SimulationCycleCallback { +class PySimulationCycleCallback : public SimulationCycleCallback { public: using SimulationCycleCallback::SimulationCycleCallback; - SimulationCycleCallbackWrap(py::object obj) : SimulationCycleCallback(), py_obj(std::move(obj)) {} - void operator()( - std::vector &measurements, - std::vector &trajectories, - const std::string &outpath) override { + bool is_callback_in_progress = false; // Flag to prevent recursion - py::gil_scoped_acquire acquire; // Acquire GIL before calling Python code - - // Create a tuple with the required components - py::tuple output = py::make_tuple( - measurements, - trajectories, - outpath, - std::vector{outpath}, - false - ); + void operator()(std::vector& measurements, + std::vector& trajectories, + const std::string& outpath) override { + + if (is_callback_in_progress) return; + is_callback_in_progress = true; // Set the flag to prevent recursion + + py::gil_scoped_acquire acquire; // Acquire GIL before calling Python code + py::object py_self = py::cast(this, py::return_value_policy::reference); // Reference to the Python object + + if (py::hasattr(py_self, "__call__")) { + // Convert C++ vectors to Python lists + py::list measurements_list = py::cast(measurements); + py::list trajectories_list = py::cast(trajectories); + + // Call the Python __call__ method with converted arguments + py_self.attr("__call__")(measurements_list, trajectories_list, outpath); + } else { + throw std::runtime_error("Python __call__ method is missing on SimulationCycleCallback."); + } - py_obj(output); + is_callback_in_progress = false; // Reset the flag after callback completes } +}; + + + + -private: - py::object py_obj; -}; diff --git a/src/python/utils.h b/src/python/utils.h index ebff1ef10..a56db7a08 100644 --- a/src/python/utils.h +++ b/src/python/utils.h @@ -29,4 +29,18 @@ int calcTimePropagation(std::vector &timeWave, int numBins, Scanner &sca template py::array_t create_numpy_array(T (&arr)[N]) { return py::array_t(N, arr); +} + +template +void from_numpy_array(py::array_t arr, double (&out)[N]) { + if (arr.size() != N) { + throw std::runtime_error("Input array size does not match expected size."); + } + + // Create a buffer info object to access the data + auto buf = arr.unchecked<1>(); // Unchecked for 1D access + + for (size_t i = 0; i < N; ++i) { + out[i] = buf(i); // Copy each element into the output array + } } \ No newline at end of file diff --git a/src/scanner/SingleScanner.cpp b/src/scanner/SingleScanner.cpp index 52054d9d8..3c4472475 100644 --- a/src/scanner/SingleScanner.cpp +++ b/src/scanner/SingleScanner.cpp @@ -19,12 +19,12 @@ SingleScanner::SingleScanner( double const receiverDiameter, double const atmosphericVisibility, int const wavelength, - std::shared_ptr> rangeErrExpr, bool const writeWaveform, bool const writePulse, bool const calcEchowidth, bool const fullWaveNoise, - bool const platformNoiseDisabled + bool const platformNoiseDisabled, + std::shared_ptr> rangeErrExpr ) : Scanner( id, @@ -135,7 +135,6 @@ void SingleScanner::doSimStep( ){ // Check whether the scanner is active or not bool const _isActive = isActive(); - // Simulate scanning devices scanDev.doSimStep( legIndex, diff --git a/src/scanner/SingleScanner.h b/src/scanner/SingleScanner.h index 13009bac2..92af669b3 100644 --- a/src/scanner/SingleScanner.h +++ b/src/scanner/SingleScanner.h @@ -39,12 +39,12 @@ class SingleScanner : public Scanner{ double const receiverDiameter, double const atmosphericVisibility, int const wavelength, - std::shared_ptr> rangeErrExpr = nullptr, bool const writeWaveform = false, bool const writePulse = false, bool const calcEchowidth = false, bool const fullWaveNoise = false, - bool const platformNoiseDisabled = false + bool const platformNoiseDisabled = false, + std::shared_ptr> rangeErrExpr = nullptr ); /** * @brief Copy constructor for the SingleScanner diff --git a/src/test/EnergyModelsTest.h b/src/test/EnergyModelsTest.h index f365143f8..746a53dda 100644 --- a/src/test/EnergyModelsTest.h +++ b/src/test/EnergyModelsTest.h @@ -168,12 +168,12 @@ bool EnergyModelsTest::testEllipticalFootprintEnergy(){ 0.15, // receiverDiameter_m 9.07603791e-6, // atmosphericExtinction 1.064e-06, // wavelength_m - nullptr, // rangeErrExpr false, // Write waveform false, // Write pulse false, // Calc echowidth false, // Fullwave noise - false // Platform noise disabled + false, // Platform noise disabled + nullptr // rangeErrExpr ); std::shared_ptr detector = std::make_shared< FullWaveformPulseDetector diff --git a/src/test/SurveyCopyTest.h b/src/test/SurveyCopyTest.h index 121f0748b..ae13db588 100644 --- a/src/test/SurveyCopyTest.h +++ b/src/test/SurveyCopyTest.h @@ -54,11 +54,12 @@ bool SurveyCopyTest::run(){ 0.7, 0.8, 100, - nullptr, false, false, false, - true + true, + false, + nullptr ); survey->scanner->setScannerHead(std::make_shared( glm::dvec3(0.4, 0.7, 0.1), 0.067