tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(root): add s210a

Browse Source
This commit is contained in:
kalman 2019-01-02 19:41:34 +08:00
parent 667c53ae6d
commit c6fd9db827
20 changed files with 699 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@ -176,6 +176,9 @@ set(MYNTEYE_SRCS
src/mynteye/device/standard2/channels_adapter_s2.cc
src/mynteye/device/standard2/device_s2.cc
src/mynteye/device/standard2/streams_adapter_s2.cc
src/mynteye/device/standard2/channels_adapter_s210a.cc
src/mynteye/device/standard2/device_s210a.cc
src/mynteye/device/standard2/streams_adapter_s210a.cc
src/mynteye/device/streams.cc
src/mynteye/device/types.cc
src/mynteye/device/utils.cc

6
include/mynteye/types.h
View File

@ -37,8 +37,10 @@ MYNTEYE_BEGIN_NAMESPACE
enum class Model : std::uint8_t {
/** Standard */
STANDARD,
/** Standard 2 generation */
/** Standard 2 */
STANDARD2,
/** Standard 210a */
STANDARD210A,
/** Last guard */
LAST
};
@ -313,6 +315,8 @@ enum class Format : std::uint32_t {
YUYV = MYNTEYE_FOURCC('Y', 'U', 'Y', 'V'),
/** BGR 8:8:8, 24 bits per pixel */
BGR888 = MYNTEYE_FOURCC('B', 'G', 'R', '3'),
/** RGB 8:8:8, 24 bits per pixel */
RGB888 = MYNTEYE_FOURCC('R', 'G', 'B', '3'),
/** Last guard */
LAST
};

4
samples/tutorials/control/auto_exposure.cc
View File

@ -58,8 +58,8 @@ int main(int argc, char *argv[]) {
<< api->GetOptionValue(Option::DESIRED_BRIGHTNESS);
}
// Set auto exposure options fo s210a or s2000
if (model == Model::STANDARD2) {
// Set auto exposure options fo S2000/S2100/S210A
if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
// auto-exposure: 0
api->SetOptionValue(Option::EXPOSURE_MODE, 0);

4
samples/tutorials/control/framerate.cc
View File

@ -49,8 +49,8 @@ int main(int argc, char *argv[]) {
<< api->GetOptionValue(Option::IMU_FREQUENCY);
}
// You should set frame rate for S210A by 'SelectStreamRequest()'
if (model == Model::STANDARD2) {
// You should set frame rate for S2000/S2100/S210A by 'SelectStreamRequest()'
if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
LOG(INFO) << "Please set frame rate by 'SelectStreamRequest()'";
}

4
samples/tutorials/control/imu_low_pass_filter.cc
View File

@ -38,8 +38,8 @@ int main(int argc, char *argv[]) {
return 0;
}
// Set imu low pass filter for s210a
if (model == Model::STANDARD2) {
// Set imu low pass filter for S2000/S2100/S210A
if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
// ACCELEROMETER_RANGE values: 0, 1, 2
api->SetOptionValue(Option::ACCELEROMETER_LOW_PASS_FILTER, 2);
// GYROSCOPE_RANGE values: 23, 64

6
samples/tutorials/control/imu_range.cc
View File

@ -32,7 +32,7 @@ int main(int argc, char *argv[]) {
Model model = api->GetModel();
// Set imu range for s1030
// Set imu range for S1030
if (model == Model::STANDARD) {
// ACCELEROMETER_RANGE values: 4, 8, 16, 32
api->SetOptionValue(Option::ACCELEROMETER_RANGE, 8);
@ -40,8 +40,8 @@ int main(int argc, char *argv[]) {
api->SetOptionValue(Option::GYROSCOPE_RANGE, 1000);
}
// Set imu range for s210a
if (model == Model::STANDARD2) {
// Set imu range for S2000/S2100/S210A
if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
// ACCELEROMETER_RANGE values: 6, 12, 24, 32
api->SetOptionValue(Option::ACCELEROMETER_RANGE, 6);
// GYROSCOPE_RANGE values: 250, 500, 1000, 2000, 4000

5
samples/tutorials/control/infrared.cc
View File

@ -29,8 +29,7 @@ int main(int argc, char *argv[]) {
Model model = api->GetModel();
// Set ir low pass filter for s210a
if (model == Model::STANDARD) {
if (model == Model::STANDARD || model == Model::STANDARD2) {
// ir control: range [0,160], default 0
api->SetOptionValue(Option::IR_CONTROL, 80);
@ -39,7 +38,7 @@ int main(int argc, char *argv[]) {
}
// MYNTEYE-S210A don't support this option
if (model == Model::STANDARD2) {
if (model == Model::STANDARD210A) {
LOG(INFO) << "Sorry,MYNTEYE-S210A don't support ir control";
return 0;
}

4
samples/tutorials/control/manual_exposure.cc
View File

@ -54,8 +54,8 @@ int main(int argc, char *argv[]) {
LOG(INFO) << "Set CONTRAST to " << api->GetOptionValue(Option::CONTRAST);
}
// Set manual exposure options fo s210a or s2000
if (model == Model::STANDARD2) {
// Set manual exposure options fo S2000/S2100/S210A
if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
// manual-exposure: 1
api->SetOptionValue(Option::EXPOSURE_MODE, 1);

25
src/mynteye/device/config.cc
View File

@ -17,12 +17,14 @@ MYNTEYE_BEGIN_NAMESPACE
const std::map<Model, StreamSupports> stream_supports_map = {
{Model::STANDARD, {Stream::LEFT, Stream::RIGHT}},
{Model::STANDARD2, {Stream::LEFT, Stream::RIGHT}}
{Model::STANDARD2, {Stream::LEFT, Stream::RIGHT}},
{Model::STANDARD210A, {Stream::LEFT, Stream::RIGHT}}
};
const std::map<Model, CapabilitiesSupports> capabilities_supports_map = {
{Model::STANDARD, {Capabilities::STEREO, Capabilities::IMU}},
{Model::STANDARD2, {Capabilities::STEREO_COLOR, Capabilities::IMU}}
{Model::STANDARD2, {Capabilities::STEREO_COLOR, Capabilities::IMU}},
{Model::STANDARD210A, {Capabilities::STEREO_COLOR, Capabilities::IMU}}
};
const std::map<Model, OptionSupports> option_supports_map = {
@ -38,6 +40,14 @@ const std::map<Model, OptionSupports> option_supports_map = {
Option::ERASE_CHIP}
},
{Model::STANDARD2, {
Option::BRIGHTNESS,
Option::EXPOSURE_MODE, Option::MAX_GAIN, Option::MAX_EXPOSURE_TIME,
Option::IR_CONTROL, Option::MIN_EXPOSURE_TIME,
Option::DESIRED_BRIGHTNESS, Option::ACCELEROMETER_RANGE,
Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER,
Option::GYROSCOPE_LOW_PASS_FILTER, Option::ERASE_CHIP}
},
{Model::STANDARD210A, {
Option::BRIGHTNESS,
Option::EXPOSURE_MODE, Option::MAX_GAIN, Option::MAX_EXPOSURE_TIME,
Option::MIN_EXPOSURE_TIME, Option::DESIRED_BRIGHTNESS,
@ -55,6 +65,17 @@ stream_requests_map = {
}}
},
{Model::STANDARD2,
{{Capabilities::STEREO_COLOR, {
{1280, 400, Format::YUYV, 10},
{1280, 400, Format::YUYV, 20},
{1280, 400, Format::YUYV, 30},
{1280, 400, Format::YUYV, 60},
{2560, 800, Format::YUYV, 10},
{2560, 800, Format::YUYV, 20},
{2560, 800, Format::YUYV, 30}}
}}
},
{Model::STANDARD210A,
{{Capabilities::STEREO_COLOR, {
{1280, 400, Format::BGR888, 10},
{1280, 400, Format::BGR888, 20},

14
src/mynteye/device/device.cc
View File

@ -26,6 +26,7 @@
#include "mynteye/device/motions.h"
#include "mynteye/device/standard/device_s.h"
#include "mynteye/device/standard2/device_s2.h"
#include "mynteye/device/standard2/device_s210a.h"
#include "mynteye/device/streams.h"
#include "mynteye/device/types.h"
#include "mynteye/util/strings.h"
@ -108,12 +109,17 @@ std::shared_ptr<Device> Device::Create(
VLOG(2) << "MYNE EYE Model: " << model_s;
DeviceModel model(model_s);
if (model.type == 'S') {
switch (model.generation) {
case '1':
if (model.generation == '1') {
return std::make_shared<StandardDevice>(device);
case '2':
} else if (model.generation == '2') {
if (model.custom_code == '0') {
return std::make_shared<Standard2Device>(device);
default:
} else if (model.custom_code == 'A') {
return std::make_shared<Standard210aDevice>(device);
} else {
LOG(FATAL) << "No such custom code now";
}
} else {
LOG(FATAL) << "No such generation now";
}
} else {

173
src/mynteye/device/standard2/channels_adapter_s210a.cc Normal file
View File

@ -0,0 +1,173 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "mynteye/device/standard2/channels_adapter_s210a.h"
#include "mynteye/device/config.h"
#include "mynteye/logger.h"
MYNTEYE_BEGIN_NAMESPACE
namespace {
#pragma pack(push, 1)
struct ImuData {
std::uint32_t frame_id;
std::uint64_t timestamp;
std::uint8_t flag;
std::int16_t temperature;
std::int16_t accel_or_gyro[3];
ImuData() = default;
explicit ImuData(const std::uint8_t *data) {
from_data(data);
}
void from_data(const std::uint8_t *data) {
std::uint32_t timestamp_l;
std::uint32_t timestamp_h;
frame_id = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
*(data + 3);
timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
(*(data + 6) << 8) | *(data + 7);
timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
flag = *(data + 12);
temperature = (*(data + 13) << 8) | *(data + 14);
accel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);
accel_or_gyro[1] = (*(data + 17) << 8) | *(data + 18);
accel_or_gyro[2] = (*(data + 19) << 8) | *(data + 20);
}
};
#pragma pack(pop)
void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
seg->frame_id = imu.frame_id;
seg->timestamp = imu.timestamp;
seg->flag = imu.flag;
seg->temperature = imu.temperature;
seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0;
seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0;
seg->accel[2] = (seg->flag == 1) ? imu.accel_or_gyro[2] : 0;
seg->gyro[0] = (seg->flag == 2) ? imu.accel_or_gyro[0] : 0;
seg->gyro[1] = (seg->flag == 2) ? imu.accel_or_gyro[1] : 0;
seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0;
}
void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
std::size_t data_n = sizeof(ImuData); // 21
for (std::size_t i = 0; i < pkg->count; i++) {
ImuSegment seg;
unpack_imu_segment(ImuData(data + data_n * i), &seg);
pkg->segments.push_back(seg);
}
pkg->serial_number = pkg->segments.back().frame_id;
}
void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
res->header = *data;
res->state = *(data + 1);
res->size = (*(data + 2) << 8) | *(data + 3);
std::size_t data_n = sizeof(ImuData); // 21
ImuPacket packet;
packet.count = res->size / data_n;
unpack_imu_packet(data + 4, &packet);
res->packets.push_back(packet);
res->checksum = *(data + 4 + res->size);
}
} // namespace
Standard210aChannelsAdapter::Standard210aChannelsAdapter() {
}
Standard210aChannelsAdapter::~Standard210aChannelsAdapter() {
}
std::set<Option> Standard210aChannelsAdapter::GetOptionSupports() {
return option_supports_map.at(Model::STANDARD210A);
}
std::int32_t Standard210aChannelsAdapter::GetAccelRangeDefault() {
return 12;
}
std::vector<std::int32_t> Standard210aChannelsAdapter::GetAccelRangeValues() {
return {6, 12, 24, 48};
}
std::int32_t Standard210aChannelsAdapter::GetGyroRangeDefault() {
return 1000;
}
std::vector<std::int32_t> Standard210aChannelsAdapter::GetGyroRangeValues() {
return {250, 500, 1000, 2000, 4000};
}
void Standard210aChannelsAdapter::GetImuResPacket(
const std::uint8_t *data, ImuResPacket *res) {
unpack_imu_res_packet(data, res);
}
std::size_t Standard210aChannelsAdapter::GetImgParamsFromData(
const std::uint8_t *data, const Version *version,
Channels::img_params_t *img_params) {
std::size_t i = 0;
Intrinsics in_left, in_right;
Extrinsics ex_right_to_left;
i += bytes::from_data(&in_left, data + i, version);
i += bytes::from_data(&in_right, data + i, version);
(*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left};
i += bytes::from_data(&in_left, data + i, version);
i += bytes::from_data(&in_right, data + i, version);
(*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left};
i += bytes::from_data(&ex_right_to_left, data + i, version);
(*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left;
(*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left;
return i;
}
std::size_t Standard210aChannelsAdapter::SetImgParamsToData(
const Channels::img_params_t *img_params, const Version *version,
std::uint8_t *data) {
std::size_t i = 3; // skip id, size
{
auto &&params = (*img_params).at({1280, 400});
i += bytes::to_data(&params.in_left, data + i, version);
i += bytes::to_data(&params.in_right, data + i, version);
}
{
auto &&params = (*img_params).at({2560, 800});
i += bytes::to_data(&params.in_left, data + i, version);
i += bytes::to_data(&params.in_right, data + i, version);
i += bytes::to_data(&params.ex_right_to_left, data + i, version);
}
// others
std::size_t size = i - 3;
data[0] = Channels::FID_IMG_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
return size + 3;
}
MYNTEYE_END_NAMESPACE

51
src/mynteye/device/standard2/channels_adapter_s210a.h Normal file
View File

@ -0,0 +1,51 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S210A_H_
#define MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S210A_H_
#pragma once
#include <cstdint>
#include <set>
#include <vector>
#include "mynteye/device/channels.h"
MYNTEYE_BEGIN_NAMESPACE
class Standard210aChannelsAdapter : public ChannelsAdapter {
public:
Standard210aChannelsAdapter();
virtual ~Standard210aChannelsAdapter();
std::set<Option> GetOptionSupports() override;
std::int32_t GetAccelRangeDefault() override;
std::vector<std::int32_t> GetAccelRangeValues() override;
std::int32_t GetGyroRangeDefault() override;
std::vector<std::int32_t> GetGyroRangeValues() override;
void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
std::size_t GetImgParamsFromData(
const std::uint8_t *data, const Version *version,
Channels::img_params_t *img_params) override;
std::size_t SetImgParamsToData(
const Channels::img_params_t *img_params, const Version *version,
std::uint8_t *data) override;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S210A_H_

45
src/mynteye/device/standard2/device_s210a.cc Normal file
View File

@ -0,0 +1,45 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "mynteye/device/standard2/device_s210a.h"
#include "mynteye/logger.h"
#include "mynteye/device/motions.h"
#include "mynteye/device/standard2/channels_adapter_s210a.h"
#include "mynteye/device/standard2/streams_adapter_s210a.h"
MYNTEYE_BEGIN_NAMESPACE
Standard210aDevice::Standard210aDevice(std::shared_ptr<uvc::device> device)
: Device(Model::STANDARD210A, device,
std::make_shared<Standard210aStreamsAdapter>(),
std::make_shared<Standard210aChannelsAdapter>()) {
VLOG(2) << __func__;
}
Standard210aDevice::~Standard210aDevice() {
VLOG(2) << __func__;
}
Capabilities Standard210aDevice::GetKeyStreamCapability() const {
return Capabilities::STEREO_COLOR;
}
void Standard210aDevice::OnStereoStreamUpdate() {
if (motion_tracking_) {
auto &&motions = this->motions();
motions->DoMotionTrack();
}
}
MYNTEYE_END_NAMESPACE

37
src/mynteye/device/standard2/device_s210a.h Normal file
View File

@ -0,0 +1,37 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MYNTEYE_DEVICE_STANDARD2_DEVICE_S210A_H_
#define MYNTEYE_DEVICE_STANDARD2_DEVICE_S210A_H_
#pragma once
#include <memory>
#include <vector>
#include "mynteye/device/device.h"
MYNTEYE_BEGIN_NAMESPACE
class Standard210aDevice : public Device {
public:
explicit Standard210aDevice(std::shared_ptr<uvc::device> device);
virtual ~Standard210aDevice();
Capabilities GetKeyStreamCapability() const override;
void OnStereoStreamUpdate() override;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_STANDARD2_DEVICE_S210A_H_

30
src/mynteye/device/standard2/streams_adapter_s2.cc
View File

@ -61,20 +61,15 @@ struct ImagePacket {
bool unpack_left_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
CHECK_EQ(request.format, Format::YUYV);
CHECK_EQ(frame->format(), Format::YUYV);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
std::size_t n = 2;
std::size_t w = frame->width() * n;
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) * n] =
*(data_new + (2 * i * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + (2 * i * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + (2 * i * w + j) * n);
frame->data()[i * w + j] = *(data_new + 2 * i * w + j);
}
}
return true;
@ -83,20 +78,15 @@ bool unpack_left_img_pixels(
bool unpack_right_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
CHECK_EQ(request.format, Format::YUYV);
CHECK_EQ(frame->format(), Format::YUYV);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
std::size_t n = 2;
std::size_t w = frame->width() * n;
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) * n] =
*(data_new + ((2 * i + 1) * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + ((2 * i + 1) * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + ((2 * i + 1) * w + j) * n);
frame->data()[i * w + j] = *(data_new + (2 * i + 1) * w + j);
}
}
return true;

186
src/mynteye/device/standard2/streams_adapter_s210a.cc Normal file
View File

@ -0,0 +1,186 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "mynteye/device/standard2/streams_adapter_s210a.h"
#include <iomanip>
#include "mynteye/logger.h"
#include "mynteye/device/types.h"
MYNTEYE_BEGIN_NAMESPACE
namespace {
// image info
#pragma pack(push, 1)
struct ImagePacket {
std::uint8_t header;
std::uint8_t size;
std::uint16_t frame_id;
std::uint64_t timestamp;
std::uint16_t exposure_time;
std::uint8_t checksum;
ImagePacket() = default;
explicit ImagePacket(std::uint8_t *data) {
from_data(data);
}
void from_data(std::uint8_t *data) {
std::uint32_t timestamp_l;
std::uint32_t timestamp_h;
header = *data;
size = *(data + 1);
frame_id = (*(data + 2) << 8) | *(data + 3);
timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
(*(data + 6) << 8) | *(data + 7);
timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
exposure_time = (*(data + 12) << 8) | *(data + 13);
checksum = *(data + 14);
}
};
#pragma pack(pop)
// image pixels
bool unpack_left_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
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) * n] =
*(data_new + (2 * i * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + (2 * i * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + (2 * i * w + j) * n);
}
}
return true;
}
bool unpack_right_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
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) * n] =
*(data_new + ((2 * i + 1) * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + ((2 * i + 1) * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + ((2 * i + 1) * w + j) * n);
}
}
return true;
}
bool unpack_stereo_img_data(
const void *data, const StreamRequest &request, ImgData *img) {
CHECK_NOTNULL(img);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t data_n =
request.width * request.height * bytes_per_pixel(request.format);
auto data_end = data_new + data_n;
std::size_t packet_n = sizeof(ImagePacket);
std::vector<std::uint8_t> packet(packet_n);
std::reverse_copy(data_end - packet_n, data_end, packet.begin());
ImagePacket img_packet(packet.data());
// LOG(INFO) << "ImagePacket: header=0x" << std::hex <<
// static_cast<int>(img_packet.header)
// << ", size=0x" << std::hex << static_cast<int>(img_packet.size)
// << ", frame_id="<< std::dec << img_packet.frame_id
// << ", timestamp="<< std::dec << img_packet.timestamp
// << ", exposure_time="<< std::dec << img_packet.exposure_time
// << ", checksum=0x" << std::hex << static_cast<int>(img_packet.checksum);
if (img_packet.header != 0x3B) {
VLOG(2) << "Image packet header must be 0x3B, but 0x" << std::hex
<< std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(img_packet.header) << " now";
return false;
}
std::uint8_t checksum = 0;
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) {
VLOG(2) << "Image packet checksum should be 0x" << std::hex
<< std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(img_packet.checksum) << ", but 0x"
<< std::setw(2) << std::setfill('0') << static_cast<int>(checksum)
<< " now";
return false;
}
*/
img->frame_id = img_packet.frame_id;
img->timestamp = img_packet.timestamp;
img->exposure_time = img_packet.exposure_time;
return true;
}
} // namespace
Standard210aStreamsAdapter::Standard210aStreamsAdapter() {
}
Standard210aStreamsAdapter::~Standard210aStreamsAdapter() {
}
std::vector<Stream> Standard210aStreamsAdapter::GetKeyStreams() {
return {Stream::LEFT, Stream::RIGHT};
}
std::vector<Capabilities> Standard210aStreamsAdapter::GetStreamCapabilities() {
return {Capabilities::STEREO_COLOR};
}
std::map<Stream, Streams::unpack_img_data_t>
Standard210aStreamsAdapter::GetUnpackImgDataMap() {
return {
{Stream::LEFT, unpack_stereo_img_data},
{Stream::RIGHT, unpack_stereo_img_data}
};
}
std::map<Stream, Streams::unpack_img_pixels_t>
Standard210aStreamsAdapter::GetUnpackImgPixelsMap() {
return {
{Stream::LEFT, unpack_left_img_pixels},
{Stream::RIGHT, unpack_right_img_pixels}
};
}
MYNTEYE_END_NAMESPACE

42
src/mynteye/device/standard2/streams_adapter_s210a.h Normal file
View File

@ -0,0 +1,42 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S210A_H_
#define MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S210A_H_
#pragma once
#include <map>
#include <memory>
#include <vector>
#include "mynteye/device/streams.h"
MYNTEYE_BEGIN_NAMESPACE
class Standard210aStreamsAdapter : public StreamsAdapter {
public:
Standard210aStreamsAdapter();
virtual ~Standard210aStreamsAdapter();
std::vector<Stream> GetKeyStreams() override;
std::vector<Capabilities> GetStreamCapabilities() override;
std::map<Stream, Streams::unpack_img_data_t>
GetUnpackImgDataMap() override;
std::map<Stream, Streams::unpack_img_pixels_t>
GetUnpackImgPixelsMap() override;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S210A_H_

1
src/mynteye/types.cc
View File

@ -30,6 +30,7 @@ const char *to_string(const Model &value) {
switch (value) {
CASE(STANDARD)
CASE(STANDARD2)
CASE(STANDARD210A)
default:
CHECK(is_valid(value));
return "Model::UNKNOWN";

78
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -37,22 +37,24 @@
<!-- Request index -->
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 10 -->
<arg name="index_s210a_0" default="0" />
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 20 -->
<arg name="index_s210a_1" default="1" />
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 30 -->
<arg name="index_s210a_2" default="2" />
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 60 -->
<arg name="index_s210a_3" default="3" />
<!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 10 -->
<arg name="index_s210a_4" default="4" />
<!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 20 -->
<arg name="index_s210a_5" default="5" />
<!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 30 -->
<arg name="index_s210a_6" default="6" />
<!-- Attention:The format of MYNTEYE-S2100/S2000 is YUYV.S210A's format is BGR888 -->
<arg name="request_index" default="$(arg index_s210a_2)" />
<!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 10 -->
<arg name="index_s2_0" default="0" />
<!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 20 -->
<arg name="index_s2_1" default="1" />
<!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 30 -->
<arg name="index_s2_2" default="2" />
<!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 60 -->
<arg name="index_s2_3" default="3" />
<!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 10 -->
<arg name="index_s2_4" default="4" />
<!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 20 -->
<arg name="index_s2_5" default="5" />
<!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 30 -->
<arg name="index_s2_6" default="6" />
<arg name="request_index" default="$(arg index_s2_2)" />
<arg name="enable_left_rect" default="false" />
<arg name="enable_right_rect" default="false" />
@ -141,6 +143,10 @@
<arg name="standard2/min_exposure_time" default="-1" />
<!-- <arg name="standard2/min_exposure_time" default="0" /> -->
<!-- standard2/ir_control range: [0,160] -->
<arg name="standard2/ir_control" default="80" />
<!-- <arg name="standard2/ir_control" default="0" /> -->
<!-- standard2/accel_range range: [6,48] -->
<arg name="standard2/accel_range" default="-1" />
<!-- <arg name="standard2/accel_range" default="6" /> -->
@ -157,6 +163,48 @@
<arg name="standard2/gyro_low_filter" default="-1" />
<!-- <arg name="standard2/gyro_low_filter" default="64" /> -->
<!-- device options of standard210a, -1 will not set the value -->
<!-- standard210a/brightness range: [0,240] -->
<arg name="standard210a/brightness" default="-1" />
<!-- <arg name="standard210a/brightness" default="120" /> -->
<!-- standard210a/exposure_mode, 0: auto-exposure, 1: manual-exposure -->
<arg name="standard210a/exposure_mode" default="-1" />
<!-- <arg name="standard210a/exposure_mode" default="0" /> -->
<!-- standard210a/max_gain range: [0,255] -->
<arg name="standard210a/max_gain" default="-1" />
<!-- <arg name="standard210a/max_gain" default="8" /> -->
<!-- standard210a/max_exposure_time range: [0,1000] -->
<arg name="standard210a/max_exposure_time" default="-1" />
<!-- <arg name="standard210a/max_exposure_time" default="333" /> -->
<!-- standard210a/desired_brightness range: [1,255] -->
<arg name="standard210a/desired_brightness" default="-1" />
<!-- <arg name="standard210a/desired_brightness" default="122" /> -->
<!-- standard210a/min_exposure_time range: [0,1000] -->
<arg name="standard210a/min_exposure_time" default="-1" />
<!-- <arg name="standard210a/min_exposure_time" default="0" /> -->
<!-- standard210a/accel_range range: [6,48] -->
<arg name="standard210a/accel_range" default="-1" />
<!-- <arg name="standard210a/accel_range" default="6" /> -->
<!-- standard210a/gyro_range range: [250,4000] -->
<arg name="standard210a/gyro_range" default="-1" />
<!-- <arg name="standard210a/gyro_range" default="1000" /> -->
<!-- standard210a/accel_low_filter range: [0,2] -->
<arg name="standard210a/accel_low_filter" default="-1" />
<!-- <arg name="standard210a/accel_low_filter" default="2" /> -->
<!-- standard210a/gyro_low_filter range: [23,64] -->
<arg name="standard210a/gyro_low_filter" default="-1" />
<!-- <arg name="standard210a/gyro_low_filter" default="64" /> -->
<!-- Push down all topics/nodelets into "mynteye" namespace -->
<group ns="$(arg mynteye)">

52
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -220,6 +220,22 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
gravity_ = 9.8;
private_nh_.getParam("gravity", gravity_);
// device options of standard210a
if (model_ == Model::STANDARD210A) {
option_names_ = {
{Option::BRIGHTNESS, "standard210a/brightness"},
{Option::EXPOSURE_MODE, "standard210a/exposure_mode"},
{Option::MAX_GAIN, "standard210a/max_gain"},
{Option::MAX_EXPOSURE_TIME, "standard210a/max_exposure_time"},
{Option::DESIRED_BRIGHTNESS, "standard210a/desired_brightness"},
{Option::MIN_EXPOSURE_TIME, "standard210a/min_exposure_time"},
{Option::ACCELEROMETER_RANGE, "standard210a/accel_range"},
{Option::GYROSCOPE_RANGE, "standard210a/gyro_range"},
{Option::ACCELEROMETER_LOW_PASS_FILTER,
"standard210a/accel_low_filter"},
{Option::GYROSCOPE_LOW_PASS_FILTER, "standard210a/gyro_low_filter"}};
}
// device options of standard2
if (model_ == Model::STANDARD2) {
option_names_ = {
@ -229,6 +245,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
{Option::MAX_EXPOSURE_TIME, "standard2/max_exposure_time"},
{Option::DESIRED_BRIGHTNESS, "standard2/desired_brightness"},
{Option::MIN_EXPOSURE_TIME, "standard2/min_exposure_time"},
{Option::IR_CONTROL, "STANDARD/ir_control"},
{Option::ACCELEROMETER_RANGE, "standard2/accel_range"},
{Option::GYROSCOPE_RANGE, "standard2/gyro_range"},
{Option::ACCELEROMETER_LOW_PASS_FILTER, "standard2/accel_low_filter"},
@ -237,19 +254,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// device options of standard
if (model_ == Model::STANDARD) {
option_names_ = {
{Option::GAIN, "STANDARD/gain"},
{Option::BRIGHTNESS, "STANDARD/brightness"},
{Option::CONTRAST, "STANDARD/contrast"},
{Option::FRAME_RATE, "STANDARD/frame_rate"},
{Option::IMU_FREQUENCY, "STANDARD/imu_frequency"},
{Option::EXPOSURE_MODE, "STANDARD/exposure_mode"},
{Option::MAX_GAIN, "STANDARD/max_gain"},
{Option::MAX_EXPOSURE_TIME, "STANDARD/max_exposure_time"},
{Option::DESIRED_BRIGHTNESS, "STANDARD/desired_brightness"},
{Option::IR_CONTROL, "STANDARD/ir_control"},
{Option::HDR_MODE, "STANDARD/hdr_mode"},
{Option::ACCELEROMETER_RANGE, "STANDARD/accel_range"},
{Option::GYROSCOPE_RANGE, "STANDARD/gyro_range"}};
{Option::GAIN, "standard/gain"},
{Option::BRIGHTNESS, "standard/brightness"},
{Option::CONTRAST, "standard/contrast"},
{Option::FRAME_RATE, "standard/frame_rate"},
{Option::IMU_FREQUENCY, "standard/imu_frequency"},
{Option::EXPOSURE_MODE, "standard/exposure_mode"},
{Option::MAX_GAIN, "standard/max_gain"},
{Option::MAX_EXPOSURE_TIME, "standard/max_exposure_time"},
{Option::DESIRED_BRIGHTNESS, "standard/desired_brightness"},
{Option::IR_CONTROL, "standard/ir_control"},
{Option::HDR_MODE, "standard/hdr_mode"},
{Option::ACCELEROMETER_RANGE, "standard/accel_range"},
{Option::GYROSCOPE_RANGE, "standard/gyro_range"}};
}
for (auto &&it = option_names_.begin(); it != option_names_.end(); ++it) {
@ -278,8 +295,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized on topic " << topic);
}
// Only STANDARD2 need publish mono_topics
if (model_ == Model::STANDARD2) {
// Only STANDARD2/STANDARD210A need publish mono_topics
if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) {
auto &&topic = mono_topics[it->first];
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {
@ -288,7 +305,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized on topic " << topic);
}
}
if (model_ == Model::STANDARD2) {
if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
camera_encodings_ = {{Stream::LEFT, enc::BGR8},
{Stream::RIGHT, enc::BGR8},
{Stream::LEFT_RECTIFIED, enc::BGR8},
@ -862,7 +880,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
int request_index = 0;
model_ = api_->GetModel();
if (model_ == Model::STANDARD2) {
if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
private_nh_.getParam("request_index", request_index);
switch (request_index) {
case 0: