From 6c25f06005722e0990f2bc9dc3e68acd2081a43a Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Sat, 7 Jul 2018 23:20:57 +0800 Subject: [PATCH 01/96] change the resolution --- samples/uvc/camera.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/samples/uvc/camera.cc b/samples/uvc/camera.cc index ac20e03..ada886e 100644 --- a/samples/uvc/camera.cc +++ b/samples/uvc/camera.cc @@ -108,7 +108,7 @@ int main(int argc, char *argv[]) { const auto frame_empty = [&frame]() { return frame == nullptr; }; uvc::set_device_mode( - *device, 752, 480, static_cast(Format::YUYV), 25, + *device, 1280, 480, static_cast(Format::YUYV), 25, [&mtx, &cv, &frame, &frame_ready]( const void *data, std::function continuation) { // reinterpret_cast(data); @@ -143,7 +143,7 @@ int main(int argc, char *argv[]) { } // only lastest frame is valid - cv::Mat img(480, 752, CV_8UC2, const_cast(frame->data)); + cv::Mat img(480, 1280, CV_8UC2, const_cast(frame->data)); cv::cvtColor(img, img, cv::COLOR_YUV2BGR_YUY2); cv::imshow("frame", img); From 659f03ac18a66e9e8ab6e79da1766d5f6133fd11 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Sun, 8 Jul 2018 18:49:09 +0800 Subject: [PATCH 02/96] change the way of split images --- samples/device/camera.cc | 20 ++++++++++---------- src/device/device.cc | 4 ++-- src/internal/config.cc | 2 +- src/internal/streams.cc | 32 +++++++++++++++++++------------- src/public/utils.cc | 4 ++-- 5 files changed, 34 insertions(+), 28 deletions(-) diff --git a/samples/device/camera.cc b/samples/device/camera.cc index bcd9695..5815cbd 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -46,8 +46,7 @@ int main(int argc, char *argv[]) { device->SetOptionValue(Option::FRAME_RATE, 25); device->SetOptionValue(Option::IMU_FREQUENCY, 500); */ - device->LogOptionInfos(); - + // device->LogOptionInfos(); // device->RunOptionAction(Option::ZERO_DRIFT_CALIBRATION); std::size_t left_count = 0; @@ -72,6 +71,7 @@ int main(int argc, char *argv[]) { }); std::size_t imu_count = 0; +/* device->SetMotionCallback([&imu_count](const device::MotionData &data) { CHECK_NOTNULL(data.imu); ++imu_count; @@ -86,11 +86,10 @@ int main(int argc, char *argv[]) { << ", gyro_z: " << data.imu->gyro[2] << ", temperature: " << data.imu->temperature; }); - +*/ // Enable this will cache the motion datas until you get them. - device->EnableMotionDatas(); - device->Start(Source::ALL); - + // device->EnableMotionDatas(); + device->Start(Source::VIDEO_STREAMING); cv::namedWindow("frame"); std::size_t motion_count = 0; @@ -100,7 +99,7 @@ int main(int argc, char *argv[]) { device::StreamData left_data = device->GetLatestStreamData(Stream::LEFT); device::StreamData right_data = device->GetLatestStreamData(Stream::RIGHT); - +/* auto &&motion_datas = device->GetMotionDatas(); motion_count += motion_datas.size(); for (auto &&data : motion_datas) { @@ -114,16 +113,17 @@ int main(int argc, char *argv[]) { << ", gyro_z: " << data.imu->gyro[2] << ", temperature: " << data.imu->temperature; } - +*/ cv::Mat left_img( - left_data.frame->height(), left_data.frame->width(), CV_8UC1, + left_data.frame->height(), left_data.frame->width(), CV_8UC2, left_data.frame->data()); cv::Mat right_img( - right_data.frame->height(), right_data.frame->width(), CV_8UC1, + right_data.frame->height(), right_data.frame->width(), CV_8UC2, right_data.frame->data()); cv::Mat img; cv::hconcat(left_img, right_img, img); + cv::cvtColor(img, img, cv::COLOR_YUV2BGR_YUY2); cv::imshow("frame", img); char key = static_cast(cv::waitKey(1)); diff --git a/src/device/device.cc b/src/device/device.cc index 576f0ae..0b08461 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -87,7 +87,7 @@ Device::Device(const Model &model, std::shared_ptr device) channels_(std::make_shared(device)), motions_(std::make_shared(channels_)) { VLOG(2) << __func__; - ReadAllInfos(); + //ReadAllInfos(); } Device::~Device() { @@ -96,7 +96,7 @@ Device::~Device() { std::shared_ptr Device::Create( const std::string &name, std::shared_ptr device) { - if (name == "MYNTEYE") { + if (name == "MYNTEYE" || name == "CX3-UVC") { return std::make_shared(device); } else if (strings::starts_with(name, "MYNT-EYE-")) { // TODO(JohnZhao): Create different device by name, such as MYNT-EYE-S1000 diff --git a/src/internal/config.cc b/src/internal/config.cc index f9d5a53..9b3188d 100644 --- a/src/internal/config.cc +++ b/src/internal/config.cc @@ -31,6 +31,6 @@ const std::map option_supports_map = { const std::map> stream_requests_map = { {Model::STANDARD, - {{Capabilities::STEREO, {{752, 480, Format::YUYV, 25}}}}}}; + {{Capabilities::STEREO, {{1280, 480, Format::YUYV, 25}}}}}}; MYNTEYE_END_NAMESPACE diff --git a/src/internal/streams.cc b/src/internal/streams.cc index 4b049e3..d97a656 100644 --- a/src/internal/streams.cc +++ b/src/internal/streams.cc @@ -48,7 +48,7 @@ bool unpack_stereo_img_data( // << ", timestamp="<< std::dec << img_packet.timestamp // << ", exposure_time="<< std::dec << img_packet.exposure_time // << ", checksum=0x" << std::hex << static_cast(img_packet.checksum); - +/* if (img_packet.header != 0x3B) { LOG(WARNING) << "Image packet header must be 0x3B, but 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') @@ -68,7 +68,7 @@ bool unpack_stereo_img_data( << static_cast(checksum) << " now"; return false; } - +*/ img->frame_id = img_packet.frame_id; img->timestamp = img_packet.timestamp; img->exposure_time = img_packet.exposure_time; @@ -79,11 +79,14 @@ bool unpack_left_img_pixels( const void *data, const StreamRequest &request, Streams::frame_t *frame) { CHECK_NOTNULL(frame); CHECK_EQ(request.format, Format::YUYV); - CHECK_EQ(frame->format(), Format::GREY); + CHECK_EQ(frame->format(), Format::YUYV); auto data_new = reinterpret_cast(data); - std::size_t n = frame->width() * frame->height(); - for (std::size_t i = 0; i < n; i++) { - frame->data()[i] = *(data_new + (i * 2)); + std::size_t w = frame->width() * 2; + std::size_t h = frame->height(); + for(std::size_t i = 0; i < h; i++) { + for(std::size_t j = 0; j < w; j++) { + frame->data()[i * w + j] = *(data_new + 2 * i * w + j); + } } return true; } @@ -92,11 +95,14 @@ bool unpack_right_img_pixels( const void *data, const StreamRequest &request, Streams::frame_t *frame) { CHECK_NOTNULL(frame); CHECK_EQ(request.format, Format::YUYV); - CHECK_EQ(frame->format(), Format::GREY); + CHECK_EQ(frame->format(), Format::YUYV); auto data_new = reinterpret_cast(data); - std::size_t n = frame->width() * frame->height(); - for (std::size_t i = 0; i < n; i++) { - frame->data()[i] = *(data_new + (i * 2 + 1)); + std::size_t w = frame->width() * 2; + std::size_t h = frame->height(); + for(std::size_t i = 0; i < h; i++) { + for(std::size_t j = 0; j < w; j++) { + frame->data()[i * w + j] = *(data_new + (2 * i + 1) * w + j); + } } return true; } @@ -142,13 +148,13 @@ bool Streams::PushStream(const Capabilities &capability, const void *data) { switch (capability) { case Capabilities::STEREO: { // alloc left - AllocStreamData(Stream::LEFT, request, Format::GREY); + AllocStreamData(Stream::LEFT, request, Format::YUYV); auto &&left_data = stream_datas_map_[Stream::LEFT].back(); // unpack img data if (unpack_img_data_map_[Stream::LEFT]( data, request, left_data.img.get())) { // alloc right - AllocStreamData(Stream::RIGHT, request, Format::GREY); + AllocStreamData(Stream::RIGHT, request, Format::YUYV); auto &&right_data = stream_datas_map_[Stream::RIGHT].back(); *right_data.img = *left_data.img; // unpack frame @@ -281,7 +287,7 @@ void Streams::AllocStreamData( } if (!data.frame) { data.frame = std::make_shared( - request.width, request.height, format, nullptr); + request.width / 2, request.height, format, nullptr); } stream_datas_map_[stream].push_back(data); } diff --git a/src/public/utils.cc b/src/public/utils.cc index ad8b97a..c8d2d77 100644 --- a/src/public/utils.cc +++ b/src/public/utils.cc @@ -32,7 +32,7 @@ std::shared_ptr select() { LOG(ERROR) << "No MYNT EYE devices :("; return nullptr; } - +/* LOG(INFO) << "MYNT EYE devices:"; for (size_t i = 0; i < n; i++) { auto &&device = devices[i]; @@ -40,7 +40,7 @@ std::shared_ptr select() { << ", name: " << device->GetInfo(Info::DEVICE_NAME) << ", sn: " << device->GetInfo(Info::SERIAL_NUMBER); } - +*/ std::shared_ptr device = nullptr; if (n <= 1) { device = devices[0]; From 25220a27ed09a964a3c54ddd16680cc8d45e4875 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Fri, 13 Jul 2018 14:58:18 +0800 Subject: [PATCH 03/96] creat device based on custom code --- src/device/device.cc | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/device/device.cc b/src/device/device.cc index 0b08461..2c37017 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -100,14 +100,20 @@ std::shared_ptr Device::Create( return std::make_shared(device); } else if (strings::starts_with(name, "MYNT-EYE-")) { // TODO(JohnZhao): Create different device by name, such as MYNT-EYE-S1000 - std::string model_s = name.substr(9); + std::string model_s = name.substr(9,5); VLOG(2) << "MYNE EYE Model: " << model_s; DeviceModel model(model_s); - switch (model.type) { - case 'S': - return std::make_shared(device); - default: - LOG(FATAL) << "MYNT EYE model is not supported now"; + if(model.type == 's') { + switch (model.custom_code) { + case '0': + return std::make_shared(device); + case 'A': + return std::make_shared(device); + default: + LOG(FATAL) << "No such custom code now" + } + } else { + LOG(FATAL) << "MYNT EYE model is not supported now"; } } return nullptr; From b33fc86a4595306b268afdd4cbd3f4f54b0fdd30 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Sat, 21 Jul 2018 15:34:07 +0800 Subject: [PATCH 04/96] change the data protocol of imu --- include/mynteye/types.h | 8 +-- samples/api/camera.cc | 8 +-- samples/device/camera.cc | 22 +++--- samples/tutorials/util/cv_painter.cc | 2 +- src/api/api.h | 3 +- src/device/device.cc | 4 +- src/internal/channels.cc | 2 +- src/internal/motions.cc | 22 +++--- src/internal/streams.cc | 6 +- src/internal/types.h | 72 +++++++++---------- wrappers/python/src/mynteye_py.cc | 4 +- .../launch/mynteye.launch | 2 - .../src/wrapper_nodelet.cc | 5 +- 13 files changed, 76 insertions(+), 84 deletions(-) diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 0ab79fd..7f3f9b3 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -441,10 +441,10 @@ struct MYNTEYE_API ImgData { * IMU data. */ struct MYNTEYE_API ImuData { - /** Image frame id */ - std::uint16_t frame_id; + /** accel or gyro flag:1 for accel,2 for gyro,3 for both */ + std::uint8_t flag; /** IMU timestamp in 0.01ms */ - std::uint32_t timestamp; + std::uint64_t timestamp; /** IMU accelerometer data for 3-axis: X, Y, Z. */ double accel[3]; /** IMU gyroscope data for 3-axis: X, Y, Z. */ @@ -453,7 +453,7 @@ struct MYNTEYE_API ImuData { double temperature; void Reset() { - frame_id = 0; + flag = 0; timestamp = 0; std::fill(accel, accel + 3, 0); std::fill(gyro, gyro + 3, 0); diff --git a/samples/api/camera.cc b/samples/api/camera.cc index 5cddd85..02ad7c9 100644 --- a/samples/api/camera.cc +++ b/samples/api/camera.cc @@ -56,8 +56,7 @@ int main(int argc, char *argv[]) { CHECK_NOTNULL(data.imu); ++imu_count; VLOG(2) << "Imu count: " << imu_count; - VLOG(2) << " frame_id: " << data.imu->frame_id - << ", timestamp: " << data.imu->timestamp + VLOG(2) << ", timestamp: " << data.imu->timestamp << ", accel_x: " << data.imu->accel[0] << ", accel_y: " << data.imu->accel[1] << ", accel_z: " << data.imu->accel[2] @@ -106,9 +105,8 @@ int main(int argc, char *argv[]) { auto &&motion_datas = api->GetMotionDatas(); motion_count += motion_datas.size(); - for (auto &&data : motion_datas) { - LOG(INFO) << "Imu frame_id: " << data.imu->frame_id - << ", timestamp: " << data.imu->timestamp + for (auto &&data : motion_datas) { + LOG(INFO) << ", timestamp: " << data.imu->timestamp << ", accel_x: " << data.imu->accel[0] << ", accel_y: " << data.imu->accel[1] << ", accel_z: " << data.imu->accel[2] diff --git a/samples/device/camera.cc b/samples/device/camera.cc index 5815cbd..65d3cee 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -71,13 +71,12 @@ int main(int argc, char *argv[]) { }); std::size_t imu_count = 0; -/* + device->SetMotionCallback([&imu_count](const device::MotionData &data) { CHECK_NOTNULL(data.imu); ++imu_count; - VLOG(2) << "Imu count: " << imu_count; - VLOG(2) << " frame_id: " << data.imu->frame_id - << ", timestamp: " << data.imu->timestamp + VLOG(2) << "Imu count: " << imu_count; + VLOG(2) << ", timestamp: " << data.imu->timestamp << ", accel_x: " << data.imu->accel[0] << ", accel_y: " << data.imu->accel[1] << ", accel_z: " << data.imu->accel[2] @@ -86,7 +85,7 @@ int main(int argc, char *argv[]) { << ", gyro_z: " << data.imu->gyro[2] << ", temperature: " << data.imu->temperature; }); -*/ + // Enable this will cache the motion datas until you get them. // device->EnableMotionDatas(); device->Start(Source::VIDEO_STREAMING); @@ -99,12 +98,11 @@ int main(int argc, char *argv[]) { device::StreamData left_data = device->GetLatestStreamData(Stream::LEFT); device::StreamData right_data = device->GetLatestStreamData(Stream::RIGHT); -/* + auto &&motion_datas = device->GetMotionDatas(); motion_count += motion_datas.size(); - for (auto &&data : motion_datas) { - LOG(INFO) << "Imu frame_id: " << data.imu->frame_id - << ", timestamp: " << data.imu->timestamp + for (auto &&data : motion_datas) { + LOG(INFO) << ", timestamp: " << data.imu->timestamp << ", accel_x: " << data.imu->accel[0] << ", accel_y: " << data.imu->accel[1] << ", accel_z: " << data.imu->accel[2] @@ -113,7 +111,7 @@ int main(int argc, char *argv[]) { << ", gyro_z: " << data.imu->gyro[2] << ", temperature: " << data.imu->temperature; } -*/ + cv::Mat left_img( left_data.frame->height(), left_data.frame->width(), CV_8UC2, left_data.frame->data()); @@ -146,7 +144,7 @@ int main(int argc, char *argv[]) { << ", fps: " << (1000.f * right_count / elapsed_ms); LOG(INFO) << "Imu count: " << imu_count << ", hz: " << (1000.f * imu_count / elapsed_ms); - // LOG(INFO) << "Motion count: " << motion_count - // << ", hz: " << (1000.f * motion_count / elapsed_ms); + LOG(INFO) << "Motion count: " << motion_count + << ", hz: " << (1000.f * motion_count / elapsed_ms); return 0; } diff --git a/samples/tutorials/util/cv_painter.cc b/samples/tutorials/util/cv_painter.cc index c0c6f6a..83fa767 100644 --- a/samples/tutorials/util/cv_painter.cc +++ b/samples/tutorials/util/cv_painter.cc @@ -118,7 +118,7 @@ cv::Rect CVPainter::DrawImuData( if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT) sign = -1; - Clear(ss) << "frame_id: " << data.frame_id << ", stamp: " << data.timestamp + Clear(ss) << "stamp: " << data.timestamp << ", temp: " << fmt_temp << data.temperature; cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5); diff --git a/src/api/api.h b/src/api/api.h index a7f0ff8..8a259a3 100644 --- a/src/api/api.h +++ b/src/api/api.h @@ -69,8 +69,7 @@ struct MYNTEYE_API MotionData { bool operator==(const MotionData &other) const { if (imu && other.imu) { - return imu->frame_id == other.imu->frame_id && - imu->timestamp == other.imu->timestamp; + return imu->timestamp == other.imu->timestamp; } return false; } diff --git a/src/device/device.cc b/src/device/device.cc index 2c37017..c38db1d 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -103,14 +103,14 @@ std::shared_ptr Device::Create( std::string model_s = name.substr(9,5); VLOG(2) << "MYNE EYE Model: " << model_s; DeviceModel model(model_s); - if(model.type == 's') { + if(model.type == 'S') { switch (model.custom_code) { case '0': return std::make_shared(device); case 'A': return std::make_shared(device); default: - LOG(FATAL) << "No such custom code now" + LOG(FATAL) << "No such custom code now"; } } else { LOG(FATAL) << "MYNT EYE model is not supported now"; diff --git a/src/internal/channels.cc b/src/internal/channels.cc index d09d976..88558ef 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -306,7 +306,7 @@ void Channels::DoImuTrack() { return n; }(); - auto &&sn = res_packet.packets.back().serial_number; + auto &&sn = res_packet.packets.back().segments.back().serial_number; if (imu_sn_ == sn) { VLOG(2) << "New imu not ready, dropped"; return; diff --git a/src/internal/motions.cc b/src/internal/motions.cc index 7183c6a..881ac5f 100644 --- a/src/internal/motions.cc +++ b/src/internal/motions.cc @@ -42,20 +42,26 @@ void Motions::SetMotionCallback(motion_callback_t callback) { } for (auto &&seg : packet.segments) { auto &&imu = std::make_shared(); - imu->frame_id = seg.frame_id; + // 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->timestamp = seg.timestamp; + imu->flag = seg.flag; imu->temperature = seg.temperature / 326.8f + 25; + if((imu->flag) & 0x01 != 0) { + imu->accel[0] = seg.aceel_or_gyro[0] * 8.f / 0x10000; + imu->accel[1] = seg.aceel_or_gyro[1] * 8.f / 0x10000; + imu->accel[2] = seg.aceel_or_gyro[2] * 8.f / 0x10000; + } + if((imu->flag) & 0x02 != 0) { + imu->gyro[0] = seg.aceel_or_gyro[0] * 1000.f / 0x10000; + imu->gyro[1] = seg.aceel_or_gyro[1] * 1000.f / 0x10000; + imu->gyro[2] = seg.aceel_or_gyro[2] * 1000.f / 0x10000; + } + std::lock_guard _(mtx_datas_); motion_data_t data = {imu}; motion_datas_.push_back(data); diff --git a/src/internal/streams.cc b/src/internal/streams.cc index d97a656..1a78513 100644 --- a/src/internal/streams.cc +++ b/src/internal/streams.cc @@ -48,7 +48,7 @@ bool unpack_stereo_img_data( // << ", timestamp="<< std::dec << img_packet.timestamp // << ", exposure_time="<< std::dec << img_packet.exposure_time // << ", checksum=0x" << std::hex << static_cast(img_packet.checksum); -/* + if (img_packet.header != 0x3B) { LOG(WARNING) << "Image packet header must be 0x3B, but 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') @@ -60,6 +60,7 @@ bool unpack_stereo_img_data( for (std::size_t i = 2, n = packet_n - 2; i <= n; i++) { // content: [2,9] checksum = (checksum ^ packet[i]); } + /* if (img_packet.checksum != checksum) { LOG(WARNING) << "Image packet checksum should be 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') @@ -68,7 +69,8 @@ bool unpack_stereo_img_data( << static_cast(checksum) << " now"; return false; } -*/ + */ + img->frame_id = img_packet.frame_id; img->timestamp = img_packet.timestamp; img->exposure_time = img_packet.exposure_time; diff --git a/src/internal/types.h b/src/internal/types.h index 216fe89..ac5992d 100644 --- a/src/internal/types.h +++ b/src/internal/types.h @@ -148,7 +148,7 @@ struct ImagePacket { std::uint8_t header; std::uint8_t size; std::uint16_t frame_id; - std::uint32_t timestamp; + std::uint64_t timestamp; std::uint16_t exposure_time; std::uint8_t checksum; @@ -161,10 +161,11 @@ struct ImagePacket { header = *data; size = *(data + 1); frame_id = (*(data + 2) << 8) | *(data + 3); - timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) | - *(data + 7); - exposure_time = (*(data + 8) << 8) | *(data + 9); - checksum = *(data + 10); + timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) | + (*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) | + (*(data + 10) << 8) | *(data + 11); + exposure_time = (*(data + 12) << 8) | *(data + 13); + checksum = *(data + 14); } }; #pragma pack(pop) @@ -199,11 +200,11 @@ struct ImuReqPacket { */ #pragma pack(push, 1) struct ImuSegment { - std::int16_t offset; - std::uint16_t frame_id; - std::int16_t accel[3]; + std::uint32_t serial_number; + std::uint64_t timestamp; + std::uint8_t flag; std::int16_t temperature; - std::int16_t gyro[3]; + std::int16_t aceel_or_gyro[3]; ImuSegment() = default; explicit ImuSegment(std::uint8_t *data) { @@ -211,15 +212,16 @@ struct ImuSegment { } void from_data(std::uint8_t *data) { - offset = (*(data) << 8) | *(data + 1); - frame_id = (*(data + 2) << 8) | *(data + 3); - accel[0] = (*(data + 4) << 8) | *(data + 5); - accel[1] = (*(data + 6) << 8) | *(data + 7); - accel[2] = (*(data + 8) << 8) | *(data + 9); - temperature = (*(data + 10) << 8) | *(data + 11); - gyro[0] = (*(data + 12) << 8) | *(data + 13); - gyro[1] = (*(data + 14) << 8) | *(data + 15); - gyro[2] = (*(data + 16) << 8) | *(data + 17); + serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) | + *(data + 3); + timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) | + (*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) | + (*(data + 10) << 8) | *(data + 11); + flag = *(data + 12); + temperature = (*(data + 13) << 8) | *(data + 14); + aceel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16); + aceel_or_gyro[1] = (*(data + 17) << 8) | *(data + 18); + aceel_or_gyro[2] = (*(data + 19) << 8) | *(data + 20); } }; #pragma pack(pop) @@ -230,27 +232,21 @@ struct ImuSegment { */ #pragma pack(push, 1) struct ImuPacket { - std::uint32_t serial_number; - std::uint32_t timestamp; std::uint8_t count; std::vector segments; ImuPacket() = default; - explicit ImuPacket(std::uint8_t *data) { + + explicit ImuPacket(std::uint8_t seg_count,std::uint8_t *data) { + count = seg_count; from_data(data); } - + void from_data(std::uint8_t *data) { - serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) | - *(data + 3); - timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) | - *(data + 7); - count = *(data + 8); - - std::size_t seg_n = sizeof(ImuSegment); // 18 - for (std::size_t i = 0; i < count; i++) { - segments.push_back(ImuSegment(data + 9 + (seg_n * i))); - } + std::size_t seg_n = sizeof(ImuSegment); // 21 + for(std::size_t i = 0; i < count; i++) { + segments.push_back(ImuSegment(data + seg_n * i)); + } } }; #pragma pack(pop) @@ -277,13 +273,11 @@ struct ImuResPacket { state = *(data + 1); size = (*(data + 2) << 8) | *(data + 3); - std::size_t seg_n = sizeof(ImuSegment); // 18 - for (std::size_t i = 4; i < size;) { - ImuPacket packet(data + i); - packets.push_back(packet); - i += 9 + (packet.count * seg_n); - } - + std::size_t seg_n = sizeof(ImuSegment); // 21 + std::uint8_t seg_count = (size - 4) / seg_n; + ImuPacket packet(seg_count,data + 4); + packets.push_back(packet); + //packet(2); checksum = *(data + 4 + size); } }; diff --git a/wrappers/python/src/mynteye_py.cc b/wrappers/python/src/mynteye_py.cc index 8107f55..3bcccf4 100644 --- a/wrappers/python/src/mynteye_py.cc +++ b/wrappers/python/src/mynteye_py.cc @@ -105,8 +105,7 @@ struct MYNTEYE_API MotionData { ImuData imu; bool operator==(const MotionData &other) const { - return imu.frame_id == other.imu.frame_id && - imu.timestamp == other.imu.timestamp; + return imu.timestamp == other.imu.timestamp; } }; @@ -365,7 +364,6 @@ BOOST_PYTHON_MODULE(mynteye_py) { // bp::register_ptr_to_python>(); bp::class_("ImuData") - .def_readonly("frame_id", &ImuData::frame_id) .def_readonly("timestamp", &ImuData::timestamp) .add_property( "accel", +[](ImuData *o) { return array_ref{o->accel}; }) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index 83e1e70..c906c32 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -26,7 +26,6 @@ - @@ -116,7 +115,6 @@ - diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 5e7d153..cb2a9bb 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -325,9 +325,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ++imu_count_; publishImu(data, imu_count_, stamp); publishTemp(data.imu->temperature, imu_count_, stamp); - NODELET_DEBUG_STREAM( - "Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id - << ", timestamp: " << data.imu->timestamp + NODELET_DEBUG_STREAM( + "Imu count: " << imu_count_<< ", timestamp: " << data.imu->timestamp << ", accel_x: " << data.imu->accel[0] << ", accel_y: " << data.imu->accel[1] << ", accel_z: " << data.imu->accel[2] From 259dc968aa103e9c464a6cec5c5dd5a8ee2df2b3 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Sat, 21 Jul 2018 15:43:02 +0800 Subject: [PATCH 05/96] Enable imu data --- samples/device/camera.cc | 4 ++-- src/device/device.cc | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/samples/device/camera.cc b/samples/device/camera.cc index 65d3cee..e225ccc 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -87,8 +87,8 @@ int main(int argc, char *argv[]) { }); // Enable this will cache the motion datas until you get them. - // device->EnableMotionDatas(); - device->Start(Source::VIDEO_STREAMING); + device->EnableMotionDatas(); + device->Start(Source::ALL); cv::namedWindow("frame"); std::size_t motion_count = 0; diff --git a/src/device/device.cc b/src/device/device.cc index c38db1d..7ec23b8 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -96,7 +96,7 @@ Device::~Device() { std::shared_ptr Device::Create( const std::string &name, std::shared_ptr device) { - if (name == "MYNTEYE" || name == "CX3-UVC") { + if (name == "MYNTEYE") { return std::make_shared(device); } else if (strings::starts_with(name, "MYNT-EYE-")) { // TODO(JohnZhao): Create different device by name, such as MYNT-EYE-S1000 From 01c3c7151689852938c868ea45abfe4b263f990c Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Sat, 21 Jul 2018 17:09:47 +0800 Subject: [PATCH 06/96] fix tool bug --- src/internal/channels.cc | 4 ++++ src/internal/motions.cc | 13 ++++++++++--- tools/dataset/dataset.cc | 11 +++++------ 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/internal/channels.cc b/src/internal/channels.cc index 88558ef..0afbbba 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -298,6 +298,10 @@ void Channels::DoImuTrack() { return; } + if (res_packet.packets.back().count == 0) { + return; + } + VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() { std::size_t n = 0; for (auto &&packet : res_packet.packets) { diff --git a/src/internal/motions.cc b/src/internal/motions.cc index 881ac5f..b091f72 100644 --- a/src/internal/motions.cc +++ b/src/internal/motions.cc @@ -51,15 +51,22 @@ void Motions::SetMotionCallback(motion_callback_t callback) { imu->flag = seg.flag; imu->temperature = seg.temperature / 326.8f + 25; - if((imu->flag) & 0x01 != 0) { + if(imu->flag == 1) { imu->accel[0] = seg.aceel_or_gyro[0] * 8.f / 0x10000; imu->accel[1] = seg.aceel_or_gyro[1] * 8.f / 0x10000; imu->accel[2] = seg.aceel_or_gyro[2] * 8.f / 0x10000; - } - if((imu->flag) & 0x02 != 0) { + imu->gyro[0] = 0; + imu->gyro[1] = 0; + imu->gyro[2] = 0; + } else if(imu->flag == 2) { + imu->accel[0] = 0; + imu->accel[1] = 0; + imu->accel[2] = 0; imu->gyro[0] = seg.aceel_or_gyro[0] * 1000.f / 0x10000; imu->gyro[1] = seg.aceel_or_gyro[1] * 1000.f / 0x10000; imu->gyro[2] = seg.aceel_or_gyro[2] * 1000.f / 0x10000; + } else { + imu->Reset(); } std::lock_guard _(mtx_datas_); diff --git a/tools/dataset/dataset.cc b/tools/dataset/dataset.cc index d156c37..1fea9c9 100644 --- a/tools/dataset/dataset.cc +++ b/tools/dataset/dataset.cc @@ -79,12 +79,11 @@ void Dataset::SaveStreamData( void Dataset::SaveMotionData(const device::MotionData &data) { auto &&writer = GetMotionWriter(); auto seq = motion_count_; - writer->ofs << seq << ", " << data.imu->frame_id << ", " - << data.imu->timestamp << ", " << data.imu->accel[0] << ", " - << data.imu->accel[1] << ", " << data.imu->accel[2] << ", " - << data.imu->gyro[0] << ", " << data.imu->gyro[1] << ", " - << data.imu->gyro[2] << ", " << data.imu->temperature - << std::endl; + writer->ofs << seq << ", " << data.imu->timestamp << ", " + << data.imu->accel[0] << ", " << data.imu->accel[1] << ", " + << data.imu->accel[2] << ", " << data.imu->gyro[0] << ", " + << data.imu->gyro[1] << ", " << data.imu->gyro[2] << ", " + << data.imu->temperature << std::endl; ++motion_count_; } From 08fb1a425ffaeb9f4fcbf8962a1eb34ae580a394 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Sat, 21 Jul 2018 17:40:56 +0800 Subject: [PATCH 07/96] update img/imu spc --- doc/zh-Hans/spec_image_data.md | 13 ++++++------- doc/zh-Hans/spec_imu_data.md | 22 +++++++++++----------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/doc/zh-Hans/spec_image_data.md b/doc/zh-Hans/spec_image_data.md index 77efb87..bb5261d 100644 --- a/doc/zh-Hans/spec_image_data.md +++ b/doc/zh-Hans/spec_image_data.md @@ -3,8 +3,8 @@ | 名称 | 字段 | 单位 | 字节数 | 说明 | | :----- | :----- | :----- | :-------- | :----- | | 帧 ID | frame_id | - | 2 | uint16_t; [0,65535] | -| 时间戳 | timestamp | 10 us | 4 | uint32_t | -| 曝光时间 | exposure_time | 10 us | 2 | uint16_t | +| 时间戳 | timestamp | 1 us | 8 | uint64_t | +| 曝光时间 | exposure_time | 1 us | 2 | uint16_t | > 图像数据传输方式:倒序排在图像尾部。 @@ -12,11 +12,10 @@ | Name | Header | Size | FrameID | Timestamp | ExposureTime | Checksum | | :--- | :----- | :--- | :------ | :-------- | :----------- | :------- | -| 字节数 | 1 | 1 | 2 | 4 | 2 | 1 | -| 类型 | uint8_t | uint8_t | uint16_t | uint32_t | uint16_t | uint8_t | -| 描述 | 0x3B | 0x08 (数据内容大小) | 帧 ID | 时间戳 | 曝光时间 | 校验码(数据内容所有字节异或) | +| 字节数 | 1 | 1 | 2 | 8 | 2 | 1 | +| 类型 | uint8_t | uint8_t | uint16_t | uint64_t | uint16_t | uint8_t | +| 描述 | 0x3B | 0x10 (数据内容大小) | 帧 ID | 时间戳 | 曝光时间 | 校验码(数据内容所有字节异或) | * 数据包校验不过,会丢弃该帧。 -* 时间的单位精度为: 0.01 ms / 10 us 。 - * 4 字节能表示的最大时间约是 11.9 小时,溢出后将重累计。 +* 时间的单位精度为: 1 us 。 * 时间累计是从上电时从开始,而不是从打开时开始。 diff --git a/doc/zh-Hans/spec_imu_data.md b/doc/zh-Hans/spec_imu_data.md index d6fbda1..38050f8 100644 --- a/doc/zh-Hans/spec_imu_data.md +++ b/doc/zh-Hans/spec_imu_data.md @@ -10,7 +10,7 @@ ## IMU 响应数据包 -IMU 响应数据包里会包含多个 IMU 包,而每个 IMU 包又带有多个 IMU 段。 +IMU 响应数据包里会包含1个 IMU 包,而每个 IMU 包又带有多个 IMU 段。 | Name | Header | State | Size | IMU Packets | Checksum | | :--- | :----- | :---- | :--- | :---------- | :------- | @@ -22,19 +22,19 @@ IMU 响应数据包里会包含多个 IMU 包,而每个 IMU 包又带有多个 IMU 包/小包,是一组 IMU 数据。 -| Name | Serial Number | Timestamp | Count | IMU Datas | -| :--- | :------------ | :-------- | :---- | :-------- | -| 字节数 | 4 | 4 | 1 | ... | -| 类型 | uint32_t | uint32_t | uint8_t | - | -| 描述 | 序列号 | IMU 基准时间戳 | IMU 段数量 | 所包含的 IMU 段 | +| Name | Count | IMU Datas | +| :--- | :-----| :-------- | +| 字节数 | 2 | ... | +| 类型 | uint16_t | - | +| 描述 | IMU 段数量 | 所包含的 IMU 段 | ### IMU 段 -| Name | Offset | FrameID | Accelerometer | Temperature | Gyroscope | -| :--- | :----- | :------ | :------------ | :---------- | :-------- | -| 字节数 | 2 | 2 | 6 | 2 | 6 | -| 类型 | int16_t | uint16_t | int16_t * 3 | int16_t | int16_t * 3 | -| Description | 相对基准时间戳的偏移量 | 图像帧 ID | 加速度计 x y z 三轴的值 | IMU 的温度 | 陀螺仪 x y z 三轴的值 | +| Name | Serial Number | Timestamp | flag | Temperature | Accelerometer or Gyroscope | +| :--- | :------------ | :-------- | :----| :----------- | :------------------------- | +| 字节数 | 4 | 8 | 1 | 2 | 6 | +| 类型 | uint32_t | uint64_t | int8_t | int16_t | int16_t * 3 | +| Description | 序列号 | 时间戳 | 指定传感器类型 | IMU 的温度 | 陀螺仪或陀螺仪 x y z 三轴的值 | * 加速度计和陀螺仪的计量值换算成物理值公式: **real = data * range / 0x10000** 。 * 加速度计量程默认值为 **8 g** ,陀螺仪量程默认值为 **1000 deg/s** 。 From c5a4161b9c9282e61a267a79f081cd374f8a2b55 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Mon, 23 Jul 2018 11:23:44 +0800 Subject: [PATCH 08/96] change the range of accel --- doc/zh-Hans/spec_imu_data.md | 2 +- src/internal/motions.cc | 6 +++--- third_party/glog | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/zh-Hans/spec_imu_data.md b/doc/zh-Hans/spec_imu_data.md index 38050f8..34f3490 100644 --- a/doc/zh-Hans/spec_imu_data.md +++ b/doc/zh-Hans/spec_imu_data.md @@ -37,6 +37,6 @@ IMU 包/小包,是一组 IMU 数据。 | Description | 序列号 | 时间戳 | 指定传感器类型 | IMU 的温度 | 陀螺仪或陀螺仪 x y z 三轴的值 | * 加速度计和陀螺仪的计量值换算成物理值公式: **real = data * range / 0x10000** 。 - * 加速度计量程默认值为 **8 g** ,陀螺仪量程默认值为 **1000 deg/s** 。 + * 加速度计量程默认值为 **12 g** ,陀螺仪量程默认值为 **1000 deg/s** 。 * 温度计量值换算成物理值公式: **real = data / ratio + offset** 。 * ``ratio`` 默认值为 **326.8** , ``offset`` 默认值为 **25℃** 。 diff --git a/src/internal/motions.cc b/src/internal/motions.cc index b091f72..3828b54 100644 --- a/src/internal/motions.cc +++ b/src/internal/motions.cc @@ -52,9 +52,9 @@ void Motions::SetMotionCallback(motion_callback_t callback) { imu->temperature = seg.temperature / 326.8f + 25; if(imu->flag == 1) { - imu->accel[0] = seg.aceel_or_gyro[0] * 8.f / 0x10000; - imu->accel[1] = seg.aceel_or_gyro[1] * 8.f / 0x10000; - imu->accel[2] = seg.aceel_or_gyro[2] * 8.f / 0x10000; + imu->accel[0] = seg.aceel_or_gyro[0] * 12.f / 0x10000; + imu->accel[1] = seg.aceel_or_gyro[1] * 12.f / 0x10000; + imu->accel[2] = seg.aceel_or_gyro[2] * 12.f / 0x10000; imu->gyro[0] = 0; imu->gyro[1] = 0; imu->gyro[2] = 0; diff --git a/third_party/glog b/third_party/glog index a97d6b0..8d7a107 160000 --- a/third_party/glog +++ b/third_party/glog @@ -1 +1 @@ -Subproject commit a97d6b0e1ce34e4e1956c63de8ddd325ff164512 +Subproject commit 8d7a107d68c127f3f494bb7807b796c8c5a97a82 From d4547c5525e434cd990e18284542b2a63aa86dcb Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Mon, 23 Jul 2018 17:15:20 +0800 Subject: [PATCH 09/96] fix imu count bug --- src/internal/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/internal/types.h b/src/internal/types.h index ac5992d..69d7d59 100644 --- a/src/internal/types.h +++ b/src/internal/types.h @@ -274,7 +274,7 @@ struct ImuResPacket { size = (*(data + 2) << 8) | *(data + 3); std::size_t seg_n = sizeof(ImuSegment); // 21 - std::uint8_t seg_count = (size - 4) / seg_n; + std::uint8_t seg_count = size / seg_n; ImuPacket packet(seg_count,data + 4); packets.push_back(packet); //packet(2); From 900c44179dae1bed99ccaf29bc873b74fcd9272c Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Mon, 23 Jul 2018 20:28:15 +0800 Subject: [PATCH 10/96] fix the record's bug --- samples/device/camera.cc | 3 ++- tools/dataset/dataset.cc | 3 ++- tools/dataset/record.cc | 6 ++++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/samples/device/camera.cc b/samples/device/camera.cc index e225ccc..dd5afb8 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -120,8 +120,9 @@ int main(int argc, char *argv[]) { right_data.frame->data()); cv::Mat img; + cv::cvtColor(left_img, left_img, cv::COLOR_YUV2BGR_YUY2); + cv::cvtColor(right_img, right_img, cv::COLOR_YUV2BGR_YUY2); cv::hconcat(left_img, right_img, img); - cv::cvtColor(img, img, cv::COLOR_YUV2BGR_YUY2); cv::imshow("frame", img); char key = static_cast(cv::waitKey(1)); diff --git a/tools/dataset/dataset.cc b/tools/dataset/dataset.cc index 1fea9c9..95a92f6 100644 --- a/tools/dataset/dataset.cc +++ b/tools/dataset/dataset.cc @@ -70,7 +70,8 @@ void Dataset::SaveStreamData( ss << writer->outdir << OS_SEP << std::dec << std::setw(IMAGE_FILENAME_WIDTH) << std::setfill('0') << seq << ".png"; cv::Mat img( - data.frame->height(), data.frame->width(), CV_8UC1, data.frame->data()); + data.frame->height(), data.frame->width(), CV_8UC2, data.frame->data()); + cv::cvtColor(img, img, cv::COLOR_YUV2BGR_YUY2); cv::imwrite(ss.str(), img); } ++stream_counts_[stream]; diff --git a/tools/dataset/record.cc b/tools/dataset/record.cc index e3de362..5ecdc14 100644 --- a/tools/dataset/record.cc +++ b/tools/dataset/record.cc @@ -80,12 +80,14 @@ int main(int argc, char *argv[]) { auto &&left_frame = left_datas.back().frame; auto &&right_frame = right_datas.back().frame; cv::Mat left_img( - left_frame->height(), left_frame->width(), CV_8UC1, left_frame->data()); + left_frame->height(), left_frame->width(), CV_8UC2, left_frame->data()); cv::Mat right_img( - right_frame->height(), right_frame->width(), CV_8UC1, + right_frame->height(), right_frame->width(), CV_8UC2, right_frame->data()); cv::Mat img; + cv::cvtColor(left_img, left_img, cv::COLOR_YUV2BGR_YUY2); + cv::cvtColor(right_img, right_img, cv::COLOR_YUV2BGR_YUY2); cv::hconcat(left_img, right_img, img); cv::imshow("frame", img); From f1003b63b31d91eab045bf8a1b7ea601e8322134 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Wed, 25 Jul 2018 17:04:39 +0800 Subject: [PATCH 11/96] include opencv2 in dataset.cc --- tools/dataset/dataset.cc | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/tools/dataset/dataset.cc b/tools/dataset/dataset.cc index 95a92f6..1604c57 100644 --- a/tools/dataset/dataset.cc +++ b/tools/dataset/dataset.cc @@ -12,13 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "dataset/dataset.h" - -#ifdef USE_OPENCV2 #include -#else -#include -#endif - +#include #include #include From 5ef5e8d8663ee9dff72b6f102cfa048cf0d5bf85 Mon Sep 17 00:00:00 2001 From: KalmanSLightech Date: Fri, 27 Jul 2018 18:06:55 +0800 Subject: [PATCH 12/96] read info --- src/api/synthetic.cc | 8 +++++--- src/device/device.cc | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/api/synthetic.cc b/src/api/synthetic.cc index 3c5a194..9323975 100644 --- a/src/api/synthetic.cc +++ b/src/api/synthetic.cc @@ -18,7 +18,7 @@ #include #include #include - +#include #include "api/plugin.h" #include "api/processor/depth_processor.h" #include "api/processor/disparity_normalized_processor.h" @@ -41,8 +41,10 @@ namespace { cv::Mat frame2mat(const std::shared_ptr &frame) { // TODO(JohnZhao) Support different format frame to cv::Mat - CHECK_EQ(frame->format(), Format::GREY); - return cv::Mat(frame->height(), frame->width(), CV_8UC1, frame->data()); + CHECK_EQ(frame->format(), Format::YUYV); + cv::Mat img(frame->height(), frame->width(), CV_8UC2,frame->data()); + cv::cvtColor(img, img, cv::COLOR_YUV2BGR_YUY2); + return img; } api::StreamData data2api(const device::StreamData &data) { diff --git a/src/device/device.cc b/src/device/device.cc index 7ec23b8..b8e973c 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -87,7 +87,7 @@ Device::Device(const Model &model, std::shared_ptr device) channels_(std::make_shared(device)), motions_(std::make_shared(channels_)) { VLOG(2) << __func__; - //ReadAllInfos(); + ReadAllInfos(); } Device::~Device() { From 27c8ccaac34cb33c0848ae3ea9d27bbf5fe2116c Mon Sep 17 00:00:00 2001 From: Kalman Date: Sat, 28 Jul 2018 01:17:56 +0800 Subject: [PATCH 13/96] delete space --- .../src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index cb2a9bb..054e484 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -174,11 +174,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet { NODELET_INFO_STREAM("Advertized on topic " << topic); } - camera_encodings_ = {{Stream::LEFT, enc::MONO8}, - {Stream::RIGHT, enc::MONO8}}; + camera_encodings_ = {{Stream::LEFT, enc::BGR8}, {Stream::RIGHT, enc::BGR8}}; - image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::MONO8}, - {Stream::RIGHT_RECTIFIED, enc::MONO8}, + image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::BGR8}, + {Stream::RIGHT_RECTIFIED, enc::BGR8}, {Stream::DISPARITY, enc::MONO8}, // float {Stream::DISPARITY_NORMALIZED, enc::MONO8}, {Stream::DEPTH, enc::MONO16}}; @@ -325,8 +324,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ++imu_count_; publishImu(data, imu_count_, stamp); publishTemp(data.imu->temperature, imu_count_, stamp); - NODELET_DEBUG_STREAM( - "Imu count: " << imu_count_<< ", timestamp: " << data.imu->timestamp + NODELET_DEBUG_STREAM( + "Imu count: " << imu_count_ << ", timestamp: " << data.imu->timestamp << ", accel_x: " << data.imu->accel[0] << ", accel_y: " << data.imu->accel[1] << ", accel_z: " << data.imu->accel[2] From 94053b8e357cb5c096b7ed6a597cbaac2fa4f68a Mon Sep 17 00:00:00 2001 From: Kalman Date: Mon, 30 Jul 2018 14:45:48 +0800 Subject: [PATCH 14/96] change UpdateControlInfos() --- src/internal/channels.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/internal/channels.cc b/src/internal/channels.cc index 0afbbba..f4a065a 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -130,14 +130,14 @@ void Channels::LogControlInfos() const { void Channels::UpdateControlInfos() { for (auto &&option : std::vector