From d15ce48a28b70e213b108a87142b245dbabc5e96 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 12 Apr 2018 11:48:09 +0800 Subject: [PATCH] Add motions & times --- CMakeLists.txt | 1 + samples/device/camera.cc | 36 +++++-- src/device/device.cc | 40 ++++---- src/device/device.h | 8 ++ src/internal/motions.cc | 83 ++++++++++++++++ src/internal/motions.h | 48 +++++++++ src/internal/times.h | 210 +++++++++++++++++++++++++++++++++++++++ 7 files changed, 395 insertions(+), 31 deletions(-) create mode 100644 src/internal/motions.cc create mode 100644 src/internal/motions.h create mode 100644 src/internal/times.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2298fda..6d2fbdc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -94,6 +94,7 @@ set(MYNTEYE_SRCS ${UVC_SRC} src/internal/channels.cc src/internal/config.cc + src/internal/motions.cc src/internal/streams.cc src/internal/strings.cc src/internal/types.cc diff --git a/samples/device/camera.cc b/samples/device/camera.cc index 359c69c..8dea9d9 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -6,6 +6,8 @@ #include "device/context.h" #include "device/device.h" +#include "internal/times.h" + MYNTEYE_USE_NAMESPACE int main(int argc, char *argv[]) { @@ -86,22 +88,23 @@ int main(int argc, char *argv[]) { device->SetMotionCallback([&imu_count](const device::MotionData &data) { CHECK_NOTNULL(data.imu); ++imu_count; - LOG(INFO) << "Imu count: " << imu_count; - LOG(INFO) << " frame_id: " << data.imu->frame_id - << ", timestamp: " << data.imu->timestamp - << ", accel_x: " << data.imu->accel[0] - << ", accel_y: " << data.imu->accel[1] - << ", accel_z: " << data.imu->accel[2] - << ", gyro_x: " << data.imu->gyro[0] - << ", gyro_y: " << data.imu->gyro[1] - << ", gyro_z: " << data.imu->gyro[2] - << ", temperature: " << data.imu->temperature; + VLOG(2) << "Imu count: " << imu_count; + VLOG(2) << " frame_id: " << data.imu->frame_id + << ", timestamp: " << data.imu->timestamp + << ", accel_x: " << data.imu->accel[0] + << ", accel_y: " << data.imu->accel[1] + << ", accel_z: " << data.imu->accel[2] + << ", gyro_x: " << data.imu->gyro[0] + << ", gyro_y: " << data.imu->gyro[1] + << ", gyro_z: " << data.imu->gyro[2] + << ", temperature: " << data.imu->temperature; }); device->Start(Source::ALL); cv::namedWindow("frame"); + auto &&time_beg = times::now(); while (true) { device->WaitForStreams(); @@ -124,7 +127,20 @@ int main(int argc, char *argv[]) { break; } } + auto &&time_end = times::now(); device->Stop(Source::ALL); + + float elapsed_ms = + times::count(time_end - time_beg) * 0.001f; + LOG(INFO) << "Time beg: " << times::to_local_string(time_beg) + << ", end: " << times::to_local_string(time_end) + << ", cost: " << elapsed_ms << "ms"; + LOG(INFO) << "Left count: " << left_count + << ", fps: " << (1000.f * left_count / elapsed_ms); + LOG(INFO) << "Right count: " << right_count + << ", fps: " << (1000.f * right_count / elapsed_ms); + LOG(INFO) << "Imu count: " << imu_count + << ", hz: " << (1000.f * imu_count / elapsed_ms); return 0; } diff --git a/src/device/device.cc b/src/device/device.cc index 4a98243..379c7fc 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -8,6 +8,7 @@ #include "device/device_s.h" #include "internal/channels.h" #include "internal/config.h" +#include "internal/motions.h" #include "internal/streams.h" #include "internal/strings.h" #include "internal/types.h" @@ -45,7 +46,8 @@ Device::Device(const Model &model, std::shared_ptr device) model_(model), device_(device), streams_(nullptr), - channels_(std::make_shared(device)) { + channels_(std::make_shared(device)), + motions_(std::make_shared(channels_)) { VLOG(2) << __func__; ReadDeviceInfo(); } @@ -276,6 +278,17 @@ device::StreamData Device::GetLatestStreamData(const Stream &stream) { return streams_->GetLatestStreamData(stream); } +void Device::EnableMotionDatas(std::size_t max_size) { + CHECK_NOTNULL(motions_); + motions_->EnableMotionDatas(max_size); +} + +std::vector Device::GetMotionDatas() { + CHECK(motion_tracking_); + CHECK_NOTNULL(motions_); + return motions_->GetMotionDatas(); +} + const StreamRequest &Device::GetStreamRequest(const Capabilities &capability) { try { return stream_config_requests_.at(capability); @@ -367,27 +380,12 @@ void Device::StartMotionTracking() { LOG(WARNING) << "Cannot start motion tracking without first stopping it"; return; } - channels_->StartImuTracking([this](const ImuPacket &packet) { - if (!HasMotionCallback()) - return; - for (auto &&seg : packet.segments) { - auto &&imu = std::make_shared(); - imu->frame_id = seg.frame_id; - if (seg.offset < 0 && - static_cast(-seg.offset) > packet.timestamp) { - LOG(WARNING) << "Imu timestamp offset is incorrect"; - } - imu->timestamp = packet.timestamp + seg.offset; - imu->accel[0] = seg.accel[0] * 8.f / 0x10000; - imu->accel[1] = seg.accel[1] * 8.f / 0x10000; - imu->accel[2] = seg.accel[2] * 8.f / 0x10000; - imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000; - imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000; - imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000; - imu->temperature = seg.temperature / 326.8f + 25; - motion_callback_({imu}); + motions_->SetMotionCallback([this](const device::MotionData &data) { + if (motion_callback_) { + motion_callback_(data); } }); + motions_->StartMotionTracking(); motion_tracking_ = true; } @@ -396,7 +394,7 @@ void Device::StopMotionTracking() { LOG(WARNING) << "Cannot stop motion tracking without first starting it"; return; } - channels_->StopImuTracking(); + motions_->StopMotionTracking(); motion_tracking_ = false; } diff --git a/src/device/device.h b/src/device/device.h index f43c734..bcb5b49 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -2,6 +2,7 @@ #define MYNTEYE_DEVICE_H_ #pragma once +#include #include #include #include @@ -24,6 +25,7 @@ struct device; struct DeviceInfo; class Channels; +class Motions; class Streams; class Device { @@ -83,6 +85,10 @@ class Device { std::vector GetStreamDatas(const Stream &stream); device::StreamData GetLatestStreamData(const Stream &stream); + void EnableMotionDatas( + std::size_t max_size = std::numeric_limits::max()); + std::vector GetMotionDatas(); + protected: std::shared_ptr device() const { return device_; @@ -126,6 +132,8 @@ class Device { std::shared_ptr channels_; + std::shared_ptr motions_; + void ReadDeviceInfo(); void WriteImgIntrinsics(const ImgIntrinsics &intrinsics); diff --git a/src/internal/motions.cc b/src/internal/motions.cc new file mode 100644 index 0000000..091d14f --- /dev/null +++ b/src/internal/motions.cc @@ -0,0 +1,83 @@ +#include "internal/motions.h" + +#include + +#include "internal/channels.h" + +MYNTEYE_BEGIN_NAMESPACE + +Motions::Motions(std::shared_ptr channels) + : channels_(channels), + motion_callback_(nullptr), + motion_datas_enabled_(false), + is_imu_tracking(false) { + CHECK_NOTNULL(channels_); + VLOG(2) << __func__; +} + +Motions::~Motions() { + VLOG(2) << __func__; +} + +void Motions::SetMotionCallback(motion_callback_t callback) { + motion_callback_ = callback; +} + +void Motions::StartMotionTracking() { + if (!is_imu_tracking) { + channels_->StartImuTracking([this](const ImuPacket &packet) { + if (!motion_callback_ && !motion_datas_enabled_) { + LOG(WARNING) << ""; + return; + } + for (auto &&seg : packet.segments) { + auto &&imu = std::make_shared(); + imu->frame_id = seg.frame_id; + if (seg.offset < 0 && + static_cast(-seg.offset) > packet.timestamp) { + LOG(WARNING) << "Imu timestamp offset is incorrect"; + } + imu->timestamp = packet.timestamp + seg.offset; + imu->accel[0] = seg.accel[0] * 8.f / 0x10000; + imu->accel[1] = seg.accel[1] * 8.f / 0x10000; + imu->accel[2] = seg.accel[2] * 8.f / 0x10000; + imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000; + imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000; + imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000; + imu->temperature = seg.temperature / 326.8f + 25; + if (motion_callback_) { + motion_callback_({imu}); + } + } + }); + is_imu_tracking = true; + } else { + LOG(WARNING) << "Imu is tracking already"; + } +} + +void Motions::StopMotionTracking() { + if (is_imu_tracking) { + channels_->StopImuTracking(); + is_imu_tracking = false; + } +} + +void Motions::EnableMotionDatas(std::size_t max_size) { + if (max_size <= 0) { + LOG(WARNING) << "Could not enable motion datas with max_size <= 0"; + return; + } + motion_datas_enabled_ = true; + motion_datas_max_size = max_size; +} + +Motions::motion_datas_t Motions::GetMotionDatas() { + if (!motion_datas_enabled_) { + LOG(FATAL) << "Must enable motion datas before getting them, or you set " + "motion callback instead"; + } + return motion_datas_; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/internal/motions.h b/src/internal/motions.h new file mode 100644 index 0000000..4e3d715 --- /dev/null +++ b/src/internal/motions.h @@ -0,0 +1,48 @@ +#ifndef MYNTEYE_INTERNAL_MOTIONS_H_ // NOLINT +#define MYNTEYE_INTERNAL_MOTIONS_H_ +#pragma once + +#include +#include + +#include "mynteye/mynteye.h" + +#include "internal/callbacks.h" + +MYNTEYE_BEGIN_NAMESPACE + +class Channels; + +class Motions { + public: + using motion_data_t = device::MotionData; + using motion_datas_t = std::vector; + + using motion_callback_t = device::MotionCallback; + + explicit Motions(std::shared_ptr channels); + ~Motions(); + + void SetMotionCallback(motion_callback_t callback); + + void StartMotionTracking(); + void StopMotionTracking(); + + void EnableMotionDatas(std::size_t max_size); + motion_datas_t GetMotionDatas(); + + private: + std::shared_ptr channels_; + + motion_callback_t motion_callback_; + + motion_datas_t motion_datas_; + bool motion_datas_enabled_; + std::size_t motion_datas_max_size; + + bool is_imu_tracking; +}; + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_INTERNAL_MOTIONS_H_ NOLINT diff --git a/src/internal/times.h b/src/internal/times.h new file mode 100644 index 0000000..8427aee --- /dev/null +++ b/src/internal/times.h @@ -0,0 +1,210 @@ +#ifndef MYNTEYE_INTERNAL_TIMES_H_ // NOLINT +#define MYNTEYE_INTERNAL_TIMES_H_ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mynteye/mynteye.h" + +MYNTEYE_BEGIN_NAMESPACE + +namespace times { + +using system_clock = std::chrono::system_clock; + +using nanoseconds = std::chrono::nanoseconds; +using microseconds = std::chrono::microseconds; +using milliseconds = std::chrono::milliseconds; +using seconds = std::chrono::seconds; +using minutes = std::chrono::minutes; +using hours = std::chrono::hours; +using days = std::chrono::duration>; + +// to + +template +inline Duration to_duration(const std::int64_t &t) { + return Duration{t}; +} + +template +inline system_clock::time_point to_time_point(const Duration &d) { + return system_clock::time_point(d); +} + +template +inline system_clock::time_point to_time_point(const std::int64_t &t) { + return system_clock::time_point(Duration{t}); +} + +inline system_clock::time_point to_time_point(std::tm *tm) { + return system_clock::from_time_t(std::mktime(tm)); +} + +inline struct std::tm *to_local_tm(const system_clock::time_point &t) { + auto t_c = system_clock::to_time_t(t); + return std::localtime(&t_c); +} + +inline struct std::tm *to_utc_tm(const system_clock::time_point &t) { + auto t_c = system_clock::to_time_t(t); + return std::gmtime(&t_c); +} + +// cast + +template +inline ToDuration cast(const FromDuration &d) { + return std::chrono::duration_cast(d); +} + +template +inline Duration cast(const system_clock::duration &d) { + return cast(d); +} + +template +inline std::int64_t cast(const std::int64_t &t) { + return cast(FromDuration{t}).count(); +} + +template +inline system_clock::time_point cast(const system_clock::time_point &d) { + // C++17, floor + return std::chrono::time_point_cast(d); +} + +template +inline system_clock::duration cast_mod(const system_clock::time_point &t) { + return t - cast(t); +} + +// count + +template +inline std::int64_t count(const FromDuration &d) { + return cast(d).count(); +} + +template +inline std::int64_t count(const system_clock::duration &d) { + return cast(d).count(); +} + +// day + +inline std::tm *day_beg(std::tm *tm) { + tm->tm_hour = 0; + tm->tm_min = 0; + tm->tm_sec = 0; + return tm; +} + +inline std::tm *day_end(std::tm *tm) { + tm->tm_hour = 23; + tm->tm_min = 59; + tm->tm_sec = 59; + return tm; +} + +inline system_clock::time_point day_beg(const system_clock::time_point &t) { + return cast(t); +} + +inline system_clock::time_point day_end(const system_clock::time_point &t) { + return day_beg(t) + days(1) - system_clock::duration(1); +} + +inline system_clock::duration day_time(const system_clock::time_point &t) { + return cast_mod(t); +} + +// between + +template +inline std::int64_t between( + const system_clock::time_point &t1, const system_clock::time_point &t2) { + return count(t2 - t1); +} + +inline std::int64_t between_days( + const system_clock::time_point &t1, const system_clock::time_point &t2) { + return between(day_beg(t1), day_beg(t2)); +} + +template +inline std::int64_t between_days( + const std::int64_t &t1, const std::int64_t &t2) { + return between_days(to_time_point(t1), to_time_point(t2)); +} + +// epoch + +inline system_clock::time_point epoch() { + return system_clock::time_point(system_clock::duration{0}); +} + +template +inline std::int64_t since_epoch(const system_clock::time_point &t) { + return count(t.time_since_epoch()); +} + +// now + +inline system_clock::time_point now() { + return system_clock::now(); +} + +template +inline std::int64_t now() { + return since_epoch(now()); +} + +// string + +inline std::string to_string( + const system_clock::time_point &t, const std::tm *tm, + const char *fmt = "%F %T", std::int32_t precision = 6) { + std::stringstream ss; +#if defined(OS_ANDROID) || defined(OS_LINUX) + char foo[20]; + strftime(foo, sizeof(foo), fmt, tm); + ss << foo; +#else + ss << std::put_time(tm, fmt); +#endif + if (precision > 0) { + if (precision > 6) + precision = 6; + ss << '.' << std::setfill('0') << std::setw(precision) + << static_cast( + count(cast_mod(t)) / + std::pow(10, 6 - precision)); + } + return ss.str(); +} + +inline std::string to_local_string( + const system_clock::time_point &t, const char *fmt = "%F %T", + const std::int32_t &precision = 6) { + return to_string(t, to_local_tm(t), fmt, precision); +} + +inline std::string to_utc_string( + const system_clock::time_point &t, const char *fmt = "%F %T", + const std::int32_t &precision = 6) { + return to_string(t, to_utc_tm(t), fmt, precision); +} + +} // namespace times + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_INTERNAL_TIMES_H_ NOLINT