From e9ab6cece66dadf30f02bf525ddf5e35bb919061 Mon Sep 17 00:00:00 2001 From: Devyesh Date: Sun, 1 Sep 2019 22:27:06 +0530 Subject: [PATCH 1/5] gitignore fix --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8feeb1f --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +include/ST/ +lib/ From 660689acf0b34e3a7881caf2901d598c2d9c0812 Mon Sep 17 00:00:00 2001 From: Devyesh Date: Mon, 2 Sep 2019 19:40:41 +0530 Subject: [PATCH 2/5] Added params --- config/params.yaml | 26 ++++++++++++++++++++++++++ launch/main.launch | 3 +++ 2 files changed, 29 insertions(+) create mode 100644 config/params.yaml create mode 100644 launch/main.launch diff --git a/config/params.yaml b/config/params.yaml new file mode 100644 index 0000000..c8f188c --- /dev/null +++ b/config/params.yaml @@ -0,0 +1,26 @@ +sensorSettings: + frameSyncEnabled: true + lowLatencyIMU: true + applyExpensiveCorrection: true + depthEnabled: true + infraredEnabled: false + visibleEnabled: true + accelerometerEnabled: false + gyroscopeEnabled: true + depthRangeMode: 0 + infraredResolution: 0 + visibleResolution: 0 + visibleApplyGammaCorrection: true + infraredAutoExposureEnabled: true + imuUpdateRate: 0 + sensorSerial: 0 + sensorInitializationTimeout: 6000 + infraredFramerate: 15.0 + depthFramerate: 15.0 + visibleFramerate: 16.0 + initialInfraredExposure: 0.0146 + initialInfraredGain: 3.0 + disableInfraredIntensityBalance: true + latencyReducerEnabled: false + initialProjectorPower: 1.0 + diff --git a/launch/main.launch b/launch/main.launch new file mode 100644 index 0000000..90c9478 --- /dev/null +++ b/launch/main.launch @@ -0,0 +1,3 @@ + + + From 9d50c0af2335d8b7fa2f086889d72b3c3fbd6bd5 Mon Sep 17 00:00:00 2001 From: Devyesh Date: Mon, 2 Sep 2019 19:40:53 +0530 Subject: [PATCH 3/5] Added params --- src/structure_driver.cpp | 98 +++++++++++++++++++++++++++++----------- 1 file changed, 72 insertions(+), 26 deletions(-) diff --git a/src/structure_driver.cpp b/src/structure_driver.cpp index 9613801..0fcaa07 100644 --- a/src/structure_driver.cpp +++ b/src/structure_driver.cpp @@ -2,12 +2,14 @@ #include #include #include +#include #include #include "register.hpp" #include +//#include #include #include #include @@ -300,6 +302,7 @@ class SessionDelegate : public ST::CaptureSessionDelegate { case ST::CaptureSessionEventId::Ready: { std::unique_lock u(lock); ready = true; + printf("HELLO It's me"); cond.notify_all(); } break; case ST::CaptureSessionEventId::Disconnected: @@ -315,7 +318,7 @@ class SessionDelegate : public ST::CaptureSessionDelegate { } void captureSessionDidOutputSample(ST::CaptureSession *, const ST::CaptureSessionSample& sample) { - //printf("Received capture session sample of type %d (%s)\n", (int)sample.type, ST::CaptureSessionSample::toString(sample.type)); + printf("Received capture session sample of type %d (%s)\n", (int)sample.type, ST::CaptureSessionSample::toString(sample.type)); switch (sample.type) { case ST::CaptureSessionSample::Type::DepthFrame: //printf("Depth frame: size %dx%d\n", sample.depthFrame.width(), sample.depthFrame.height()); @@ -353,6 +356,7 @@ class SessionDelegate : public ST::CaptureSessionDelegate { void waitUntilReady() { std::unique_lock u(lock); + printf("I am waitign"); cond.wait(u, [this]() { return ready; }); @@ -360,6 +364,7 @@ class SessionDelegate : public ST::CaptureSessionDelegate { void waitUntilDone() { std::unique_lock u(lock); + printf("I am waitign until DONE"); cond.wait(u, [this]() { return done; }); @@ -378,65 +383,100 @@ int main(int argc, char **argv) { settings.source = ST::CaptureSessionSourceId::StructureCore; /** @brief Set to true to enable frame synchronization between visible or color and depth. */ - settings.frameSyncEnabled = true; + pnh.getParam("/sensorSettings/frameSyncEnabled", settings.frameSyncEnabled) ; + /** @brief Set to true to deliver IMU events on a separate, dedicated background thread. Only supported for Structure Core, currently. */ - settings.lowLatencyIMU = true; + pnh.getParam("/sensorSettings/lowLatencyIMU", settings.lowLatencyIMU); + /** @brief Set to true to apply a correction filter to the depth before streaming. This may effect performance. */ - settings.applyExpensiveCorrection = true; + pnh.getParam("/sensorSettings/applyExpensiveCorrection", settings.applyExpensiveCorrection); + /** @brief Set to true to enable depth streaming. */ - settings.structureCore.depthEnabled = true; + pnh.getParam("/sensorSettings/depthEnabled", settings.structureCore.depthEnabled); + /** @brief Set to true to enable infrared streaming. */ - settings.structureCore.infraredEnabled = false; + pnh.getParam("/sensorSettings/infraredEnabled", settings.structureCore.infraredEnabled); + /** @brief Set to true to enable visible streaming. */ - settings.structureCore.visibleEnabled = true; + pnh.getParam("/sensorSettings/visibleEnabled", settings.structureCore.visibleEnabled); + /** @brief Set to true to enable accelerometer streaming. */ - settings.structureCore.accelerometerEnabled = false; + pnh.getParam("/sensorSettings/accelerometerEnabled", settings.structureCore.accelerometerEnabled); + /** @brief Set to true to enable gyroscope streaming. */ - settings.structureCore.gyroscopeEnabled = false; + pnh.getParam("/sensorSettings/gyroscopeEnabled", settings.structureCore.gyroscopeEnabled); + /** @brief The target resolution for streamed depth frames. @see StructureCoreDepthResolution */ - settings.structureCore.depthResolution = ST::StructureCoreDepthResolution::SXGA; + //pnh.getParam("/sensorSettings/depthResolution", settings.structureCore.depthResolution); + settings.structureCore.depthResolution = ST::StructureCoreDepthResolution::SXGA; + + /** @brief The preset depth range mode for streamed depth frames. Modifies the min/max range of the depth values. */ + //pnh.getParam("/sensorSettings/depthRangeMode", settings.structureCore.depthRangeMode); settings.structureCore.depthRangeMode = ST::StructureCoreDepthRangeMode::Default; + /** @brief The target resolution for streamed depth frames. @see StructureCoreInfraredResolution Non-default infrared and visible resolutions are currently unavailable. */ + //pnh.getParam("/sensorSettings/infraredResolution", settings.structureCore.infraredResolution); settings.structureCore.infraredResolution = ST::StructureCoreInfraredResolution::Default; + /** @brief The target resolution for streamed visible frames. @see StructureCoreVisibleResolution Non-default infrared and visible resolutions are currently unavailable. */ - settings.structureCore.visibleResolution = ST::StructureCoreVisibleResolution::Default; - /** @brief Set to true to apply gamma correction to incoming visible frames. */ - settings.structureCore.visibleApplyGammaCorrection = true; - /** @brief Enable auto-exposure for infrared frames. */ - settings.structureCore.infraredAutoExposureEnabled = true; + //pnh.getParam("/sensorSettings/visibleResolution", settings.structureCore.visibleResolution); + settings.structureCore.visibleResolution = ST::StructureCoreVisibleResolution::Default; + /** @brief Specifies how to stream the infrared frames. @see StructureCoreInfraredMode */ - settings.structureCore.infraredMode = ST::StructureCoreInfraredMode::BothCameras; + //pnh.getParam("/sensorSettings/infraredMode", settings.structureCore.infraredMode); + settings.structureCore.infraredMode = ST::StructureCoreInfraredMode::BothCameras; + /** @brief The target stream rate for IMU data. (gyro and accel) */ - settings.structureCore.imuUpdateRate = ST::StructureCoreIMUUpdateRate::Default; + //pnh.getParam("/sensorSettings/imuUpdateRate", settings.structureCore.imuUpdateRate); + settings.structureCore.imuUpdateRate = ST::StructureCoreIMUUpdateRate::Default; + /** @brief Serial number of sensor to stream. If null, the first connected sensor will be used. */ settings.structureCore.sensorSerial = nullptr; + //pnh.getParam("/sensorSettings/sensorSerial", settings.structureCore.sensorSerial); + + /** @brief Set to true to apply gamma correction to incoming visible frames. */ + pnh.getParam("/sensorSettings/visibleApplyGammaCorrection", settings.structureCore.visibleApplyGammaCorrection); + + /** @brief Enable auto-exposure for infrared frames. */ + pnh.getParam("/sensorSettings/infraredAutoExposureEnabled", settings.structureCore.infraredAutoExposureEnabled); + /** @brief Maximum amount of time (in milliseconds) to wait for a sensor to connect before throwing a timeout error. */ - settings.structureCore.sensorInitializationTimeout = 6000; + pnh.getParam("/sensorSettings/sensorInitializationTimeout", settings.structureCore.sensorInitializationTimeout); + /** @brief The target framerate for the infrared camera. If the value is not supported, the default is 30. */ - settings.structureCore.infraredFramerate = 15.f; + pnh.getParam("/sensorSettings/infraredFramerate", settings.structureCore.infraredFramerate); + /** @brief The target framerate for the depth sensor. If the value is not supported, the default is 30. */ - settings.structureCore.depthFramerate = 15.f; + pnh.getParam("/sensorSettings/depthFramerate", settings.structureCore.depthFramerate); + + /** @brief The target framerate for the visible camera. If the value is not supported, the default is 30. */ - settings.structureCore.visibleFramerate = 15.f; + pnh.getParam("/sensorSettings/visibleFramerate", settings.structureCore.visibleFramerate); + /** @brief The initial visible exposure to start streaming with (milliseconds, but set in seconds). */ //settings.structureCore.initialVisibleExposure = 0.033f; + /** @brief The initial visible gain to start streaming with. Can be any number between 1 and 8. */ //settings.structureCore.initialVisibleGain = 4.0f; + /** @brief The initial infrared exposure to start streaming with. */ - settings.structureCore.initialInfraredExposure = 0.0146f; + pnh.getParam("/sensorSettings/initialInfraredExposure", settings.structureCore.initialInfraredExposure); + /** @brief The initial infrared gain to start streaming with. Can be 0, 1, 2, or 3. */ - settings.structureCore.initialInfraredGain = 3; + pnh.getParam("/sensorSettings/initialInfraredGain", settings.structureCore.initialInfraredGain); + /** @brief Setting this to true will eliminate saturation issues, but might result in sparser depth. */ - settings.structureCore.disableInfraredIntensityBalance = true; + pnh.getParam("/sensorSettings/disableInfraredIntensityBalance", settings.structureCore.disableInfraredIntensityBalance); + /** @brief Setting this to true will reduce latency, but might drop more frame */ - settings.structureCore.latencyReducerEnabled = false; + pnh.getParam("/sensorSettings/latencyReducerEnabled", settings.structureCore.latencyReducerEnabled); /** @brief Laser projector power setting from 0.0 to 1.0 inclusive. Projector will only activate if required by streaming configuration. */ - settings.structureCore.initialProjectorPower = 1.0f; + //pnh.getParam("/sensorSettings/settings.structureCore.initialProjectorPower); SessionDelegate delegate(n, pnh); ST::CaptureSession session; @@ -450,6 +490,12 @@ int main(int argc, char **argv) { delegate.waitUntilReady(); session.startStreaming(); + /*while (true) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + printf("I am here"); + }*/ + + printf("I am outta here"); ros::spin(); session.stopStreaming(); From a743570fa946026d36e61200265ac3ad6c3817c2 Mon Sep 17 00:00:00 2001 From: Devyesh Date: Mon, 2 Sep 2019 19:45:05 +0530 Subject: [PATCH 4/5] modif --- src/structure_driver.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/structure_driver.cpp b/src/structure_driver.cpp index 0fcaa07..5e716e5 100644 --- a/src/structure_driver.cpp +++ b/src/structure_driver.cpp @@ -2,8 +2,7 @@ #include #include #include -#include - +` #include #include "register.hpp" @@ -356,7 +355,6 @@ class SessionDelegate : public ST::CaptureSessionDelegate { void waitUntilReady() { std::unique_lock u(lock); - printf("I am waitign"); cond.wait(u, [this]() { return ready; }); @@ -364,7 +362,6 @@ class SessionDelegate : public ST::CaptureSessionDelegate { void waitUntilDone() { std::unique_lock u(lock); - printf("I am waitign until DONE"); cond.wait(u, [this]() { return done; }); @@ -490,12 +487,6 @@ int main(int argc, char **argv) { delegate.waitUntilReady(); session.startStreaming(); - /*while (true) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - printf("I am here"); - }*/ - - printf("I am outta here"); ros::spin(); session.stopStreaming(); From d724dac549e4899ced52f3dc478d42a86a3c60a3 Mon Sep 17 00:00:00 2001 From: Devyesh Date: Mon, 2 Sep 2019 19:47:35 +0530 Subject: [PATCH 5/5] modif --- src/structure_driver.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/structure_driver.cpp b/src/structure_driver.cpp index 5e716e5..8ebade4 100644 --- a/src/structure_driver.cpp +++ b/src/structure_driver.cpp @@ -2,7 +2,7 @@ #include #include #include -` + #include #include "register.hpp" @@ -301,7 +301,6 @@ class SessionDelegate : public ST::CaptureSessionDelegate { case ST::CaptureSessionEventId::Ready: { std::unique_lock u(lock); ready = true; - printf("HELLO It's me"); cond.notify_all(); } break; case ST::CaptureSessionEventId::Disconnected: @@ -317,7 +316,7 @@ class SessionDelegate : public ST::CaptureSessionDelegate { } void captureSessionDidOutputSample(ST::CaptureSession *, const ST::CaptureSessionSample& sample) { - printf("Received capture session sample of type %d (%s)\n", (int)sample.type, ST::CaptureSessionSample::toString(sample.type)); + //printf("Received capture session sample of type %d (%s)\n", (int)sample.type, ST::CaptureSessionSample::toString(sample.type)); switch (sample.type) { case ST::CaptureSessionSample::Type::DepthFrame: //printf("Depth frame: size %dx%d\n", sample.depthFrame.width(), sample.depthFrame.height()); @@ -405,7 +404,7 @@ int main(int argc, char **argv) { /** @brief The target resolution for streamed depth frames. @see StructureCoreDepthResolution */ //pnh.getParam("/sensorSettings/depthResolution", settings.structureCore.depthResolution); - settings.structureCore.depthResolution = ST::StructureCoreDepthResolution::SXGA; + settings.structureCore.depthResolution = ST::StructureCoreDepthResolution::SXGA; /** @brief The preset depth range mode for streamed depth frames. Modifies the min/max range of the depth values. */