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

fix(ros): fix option setting bug

Browse Source
This commit is contained in:
kalman 2018-12-28 17:56:12 +08:00
parent ea05d35bf2
commit 0e4896f788
6 changed files with 242 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -24,6 +24,7 @@ int main(int argc, char *argv[]) {
auto &&api = API::Create(argc, argv); auto &&api = API::Create(argc, argv);
if (!api) return 1; if (!api) return 1;
std::int32_t frame_rate = 0;
bool ok; bool ok;
auto &&request = api->SelectStreamRequest(&ok); auto &&request = api->SelectStreamRequest(&ok);
if (!ok) return 1; if (!ok) return 1;
@ -43,6 +44,8 @@ int main(int argc, char *argv[]) {
// desired_brightness: range [0,255], default 192 // desired_brightness: range [0,255], default 192
api->SetOptionValue(Option::DESIRED_BRIGHTNESS, 192); api->SetOptionValue(Option::DESIRED_BRIGHTNESS, 192);
frame_rate = api->GetOptionValue(Option::FRAME_RATE);
LOG(INFO) << "Enable auto-exposure"; LOG(INFO) << "Enable auto-exposure";
LOG(INFO) << "Set EXPOSURE_MODE to " LOG(INFO) << "Set EXPOSURE_MODE to "
<< api->GetOptionValue(Option::EXPOSURE_MODE); << api->GetOptionValue(Option::EXPOSURE_MODE);
@ -83,7 +86,7 @@ int main(int argc, char *argv[]) {
api->Start(Source::VIDEO_STREAMING); api->Start(Source::VIDEO_STREAMING);
CVPainter painter(30); CVPainter painter(frame_rate);
cv::namedWindow("frame"); cv::namedWindow("frame");

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

@ -24,6 +24,7 @@ int main(int argc, char *argv[]) {
auto &&api = API::Create(argc, argv); auto &&api = API::Create(argc, argv);
if (!api) return 1; if (!api) return 1;
std::int32_t frame_rate = 0;
bool ok; bool ok;
auto &&request = api->SelectStreamRequest(&ok); auto &&request = api->SelectStreamRequest(&ok);
if (!ok) return 1; if (!ok) return 1;
@ -42,6 +43,8 @@ int main(int argc, char *argv[]) {
// contrast/black_level_calibration: range [0,255], default 127 // contrast/black_level_calibration: range [0,255], default 127
api->SetOptionValue(Option::CONTRAST, 127); api->SetOptionValue(Option::CONTRAST, 127);
frame_rate = api->GetOptionValue(Option::FRAME_RATE);
LOG(INFO) << "Enable manual-exposure"; LOG(INFO) << "Enable manual-exposure";
LOG(INFO) << "Set EXPOSURE_MODE to " LOG(INFO) << "Set EXPOSURE_MODE to "
<< api->GetOptionValue(Option::EXPOSURE_MODE); << api->GetOptionValue(Option::EXPOSURE_MODE);
@ -68,7 +71,7 @@ int main(int argc, char *argv[]) {
api->Start(Source::VIDEO_STREAMING); api->Start(Source::VIDEO_STREAMING);
CVPainter painter(30); CVPainter painter(frame_rate);
cv::namedWindow("frame"); cv::namedWindow("frame");

2
samples/tutorials/util/cv_painter.cc
View File

@ -82,7 +82,7 @@ cv::Rect CVPainter::DrawImgData(
std::ostringstream ss; std::ostringstream ss;
ss << "frame_id: " << data.frame_id; ss << "frame_id: " << data.frame_id;
ss << ", stamp: " << fmt_time << (0.01f * data.timestamp); // ms ss << ", stamp: " << fmt_time << (0.001f * data.timestamp); // ms
ss << ", expo: "; ss << ", expo: ";
if (frame_rate_ == 0) { if (frame_rate_ == 0) {
ss << data.exposure_time; ss << data.exposure_time;

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

@ -52,9 +52,6 @@
<!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 30 --> <!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 30 -->
<arg name="index_s210a_6" default="6" /> <arg name="index_s210a_6" default="6" />
<!-- MYNTEYE-S1030, Reslution: 752x480, Format: YUYV, Fps: 25 -->
<arg name="index_s1030_0" default="0" />
<arg name="request_index" default="$(arg index_s210a_2)" /> <arg name="request_index" default="$(arg index_s210a_2)" />
<arg name="enable_left_rect" default="false" /> <arg name="enable_left_rect" default="false" />
@ -64,47 +61,101 @@
<arg name="enable_points" default="false" /> <arg name="enable_points" default="false" />
<arg name="enable_depth" default="false" /> <arg name="enable_depth" default="false" />
<!-- device options, -1 will not set the value --> <!-- device options of standard, -1 will not set the value -->
<!-- brightness range: [0,240] --> <!-- standard/gain range: [0,48] -->
<arg name="brightness" default="-1" /> <arg name="standard/gain" default="-1" />
<!-- <arg name="brightness" default="120" /> --> <!-- <arg name="standard/gain" default="24" /> -->
<!-- exposure_mode, 0: auto-exposure, 1: manual-exposure --> <!-- standard/brightness range: [0,240] -->
<arg name="exposure_mode" default="-1" /> <arg name="standard/brightness" default="-1" />
<!-- <arg name="exposure_mode" default="0" /> --> <!-- <arg name="standard/brightness" default="120" /> -->
<!-- max_gain range: [0,255] --> <!-- contrast range: [0,255] -->
<arg name="max_gain" default="-1" /> <arg name="standard/contrast" default="-1" />
<!-- <arg name="max_gain" default="8" /> --> <!-- <arg name="standard/contrast" default="127" /> -->
<!-- max_exposure_time range: [0,1000] --> <!-- standard/frame_rate range: {10,15,20,25,30,35,40,45,50,55,60} -->
<arg name="max_exposure_time" default="-1" /> <arg name="standard/frame_rate" default="-1" />
<!-- <arg name="max_exposure_time" default="333" /> --> <!-- <arg name="standard/frame_rate" default="25" /> -->
<!-- desired_brightness range: [1,255] --> <!-- standard/imu_frequency range: {100,200,250,333,500} -->
<arg name="desired_brightness" default="-1" /> <arg name="standard/imu_frequency" default="-1" />
<!-- <arg name="desired_brightness" default="122" /> --> <!-- <arg name="standard/imu_frequency" default="200" /> -->
<!-- min_exposure_time range: [0,1000] --> <!-- standard/exposure_mode, 0: auto-exposure, 1: manual-exposure -->
<arg name="min_exposure_time" default="-1" /> <arg name="standard/exposure_mode" default="-1" />
<!-- <arg name="min_exposure_time" default="0" /> --> <!-- <arg name="standard/exposure_mode" default="0" /> -->
<!-- accel_range range: [6,48] --> <!-- standard/max_gain range: [0,48] -->
<arg name="accel_range" default="-1" /> <arg name="standard/max_gain" default="-1" />
<!-- <arg name="accel_range" default="6" /> --> <!-- <arg name="standard/max_gain" default="48" /> -->
<!-- gyro_range range: [250,4000] --> <!-- standard/max_exposure_time range: [0,240] -->
<arg name="gyro_range" default="-1" /> <arg name="standard/max_exposure_time" default="-1" />
<!-- <arg name="gyro_range" default="1000" /> --> <!-- <arg name="standard/max_exposure_time" default="240" /> -->
<!-- accel_low_filter range: [0,2] --> <!-- standard/desired_brightness range: [0,255] -->
<arg name="accel_low_filter" default="-1" /> <arg name="standard/desired_brightness" default="-1" />
<!-- <arg name="accel_low_filter" default="2" /> --> <!-- <arg name="standard/desired_brightness" default="192" /> -->
<!-- gyro_low_filter range: [23,64] --> <!-- standard/ir_control range: [0,160] -->
<arg name="gyro_low_filter" default="-1" /> <arg name="standard/ir_control" default="80" />
<!-- <arg name="gyro_low_filter" default="64" /> --> <!-- <arg name="standard/ir_control" default="0" /> -->
<!-- standard/hdr_mode, 0: 10-bit, 1: 12-bit -->
<arg name="standard/hdr_mode" default="-1" />
<!-- <arg name="standard/hdr_mode" default="0" /> -->
<!-- standard/accel_range range: {4,8,16,32} -->
<arg name="standard/accel_range" default="-1" />
<!-- <arg name="standard/accel_range" default="8" /> -->
<!-- standard/gyro_range range: {500,1000,2000,4000} -->
<arg name="standard/gyro_range" default="-1" />
<!-- <arg name="standard/gyro_range" default="1000" /> -->
<!-- device options of standard2, -1 will not set the value -->
<!-- standard2/brightness range: [0,240] -->
<arg name="standard2/brightness" default="-1" />
<!-- <arg name="standard2/brightness" default="120" /> -->
<!-- standard2/exposure_mode, 0: auto-exposure, 1: manual-exposure -->
<arg name="standard2/exposure_mode" default="-1" />
<!-- <arg name="standard2/exposure_mode" default="0" /> -->
<!-- standard2/max_gain range: [0,255] -->
<arg name="standard2/max_gain" default="-1" />
<!-- <arg name="standard2/max_gain" default="8" /> -->
<!-- standard2/max_exposure_time range: [0,1000] -->
<arg name="standard2/max_exposure_time" default="-1" />
<!-- <arg name="standard2/max_exposure_time" default="333" /> -->
<!-- standard2/desired_brightness range: [1,255] -->
<arg name="standard2/desired_brightness" default="-1" />
<!-- <arg name="standard2/desired_brightness" default="122" /> -->
<!-- standard2/min_exposure_time range: [0,1000] -->
<arg name="standard2/min_exposure_time" default="-1" />
<!-- <arg name="standard2/min_exposure_time" default="0" /> -->
<!-- standard2/accel_range range: [6,48] -->
<arg name="standard2/accel_range" default="-1" />
<!-- <arg name="standard2/accel_range" default="6" /> -->
<!-- standard2/gyro_range range: [250,4000] -->
<arg name="standard2/gyro_range" default="-1" />
<!-- <arg name="standard2/gyro_range" default="1000" /> -->
<!-- standard2/accel_low_filter range: [0,2] -->
<arg name="standard2/accel_low_filter" default="-1" />
<!-- <arg name="standard2/accel_low_filter" default="2" /> -->
<!-- standard2/gyro_low_filter range: [23,64] -->
<arg name="standard2/gyro_low_filter" default="-1" />
<!-- <arg name="standard2/gyro_low_filter" default="64" /> -->
<!-- Push down all topics/nodelets into "mynteye" namespace --> <!-- Push down all topics/nodelets into "mynteye" namespace -->
<group ns="$(arg mynteye)"> <group ns="$(arg mynteye)">
@ -155,18 +206,18 @@
<!-- stream request index --> <!-- stream request index -->
<param name="request_index" value="$(arg request_index)" /> <param name="request_index" value="$(arg request_index)" />
<!-- device options --> <!-- device options of standard2-->
<param name="brightness" value="$(arg brightness)" /> <param name="standard2/brightness" value="$(arg standard2/brightness)" />
<param name="exposure_mode" value="$(arg exposure_mode)" /> <param name="standard2/exposure_mode" value="$(arg standard2/exposure_mode)" />
<param name="max_gain" value="$(arg max_gain)" /> <param name="standard2/max_gain" value="$(arg standard2/max_gain)" />
<param name="max_exposure_time" value="$(arg max_exposure_time)" /> <param name="standard2/max_exposure_time" value="$(arg standard2/max_exposure_time)" />
<param name="desired_brightness" value="$(arg desired_brightness)" /> <param name="standard2/desired_brightness" value="$(arg standard2/desired_brightness)" />
<param name="min_exposure_time" value="$(arg max_exposure_time)" /> <param name="standard2/min_exposure_time" value="$(arg standard2/max_exposure_time)" />
<param name="accel_range" value="$(arg accel_range)" /> <param name="standard2/accel_range" value="$(arg standard2/accel_range)" />
<param name="gyro_range" value="$(arg gyro_range)" /> <param name="standard2/gyro_range" value="$(arg standard2/gyro_range)" />
<param name="accel_low_filter" value="$(arg accel_low_filter)" /> <param name="standard2/accel_low_filter" value="$(arg standard2/accel_low_filter)" />
<param name="gyro_low_filter" value="$(arg gyro_low_filter)" /> <param name="standard2/gyro_low_filter" value="$(arg standard2/gyro_low_filter)" />
</node> </node>
<!-- disable compressed depth plugin for image topics --> <!-- disable compressed depth plugin for image topics -->

28
wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz
View File

@ -78,34 +78,6 @@ Visualization Manager:
Transport Hint: raw Transport Hint: raw
Unreliable: false Unreliable: false
Value: true Value: true
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: true
Image Topic: /mynteye/left/image_mono
Max Value: 1
Median window: 5
Min Value: 0
Name: LeftMono
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /mynteye/right/image_mono
Max Value: 1
Median window: 5
Min Value: 0
Name: RightMono
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Name: Mono
- Class: rviz/Group - Class: rviz/Group
Displays: Displays:
- Class: rviz/Image - Class: rviz/Image

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

@ -104,31 +104,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
static_cast<double>(_hard_time - hard_time_begin) * 0.000001f)); static_cast<double>(_hard_time - hard_time_begin) * 0.000001f));
} }
inline bool is_overflow(std::uint32_t now, inline bool is_overflow(std::uint64_t now,
std::uint32_t pre) { std::uint64_t pre) {
static std::uint64_t unit = static std::uint64_t unit =
std::numeric_limits<std::uint32_t>::max(); std::numeric_limits<std::uint32_t>::max();
return (now < pre) && ((pre - now) > (unit / 2)); return (now < pre) && ((pre - now) > (unit / 2));
} }
inline bool is_repeated(std::uint32_t now, inline bool is_repeated(std::uint64_t now,
std::uint32_t pre) { std::uint64_t pre) {
return now == pre; return now == pre;
} }
inline bool is_annormal(std::uint32_t now, inline bool is_annormal(std::uint64_t now,
std::uint32_t pre) { std::uint64_t pre) {
static std::uint64_t unit = static std::uint64_t unit =
std::numeric_limits<std::uint32_t>::max(); std::numeric_limits<std::uint32_t>::max();
return (now < pre) && ((pre - now) < (unit / 4)); return (now < pre) && ((pre - now) < (unit / 4));
} }
ros::Time checkUpTimeStamp(std::uint32_t _hard_time, ros::Time checkUpTimeStamp(std::uint64_t _hard_time,
const Stream &stream) { const Stream &stream) {
static std::map<Stream, std::uint32_t> hard_time_now; static std::map<Stream, std::uint64_t> hard_time_now;
static std::map<Stream, std::uint32_t> acc; static std::map<Stream, std::uint64_t> acc;
static std::uint64_t unit_hard_time = static std::uint64_t unit_hard_time =
std::numeric_limits<std::uint32_t>::max(); std::numeric_limits<std::uint32_t>::max();
@ -145,18 +145,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
acc[stream] * unit_hard_time + _hard_time); acc[stream] * unit_hard_time + _hard_time);
} }
ros::Time checkUpImuTimeStamp(std::uint32_t _hard_time) { ros::Time checkUpImuTimeStamp(std::uint64_t _hard_time) {
static std::uint32_t hard_time_now(0), acc(0); static std::uint64_t hard_time_now(0), acc(0);
static std::uint64_t unit_hard_time = static std::uint64_t unit_hard_time =
std::numeric_limits<std::uint32_t>::max(); std::numeric_limits<std::uint32_t>::max();
if (is_overflow(_hard_time, hard_time_now)) { if (is_overflow(_hard_time, hard_time_now)) {
acc++; acc++;
} else if (is_repeated(_hard_time, hard_time_now)) { }
/*
else if (is_repeated(_hard_time, hard_time_now)) {
NODELET_INFO_STREAM("WARNING:: Imu time stamp is repeated."); NODELET_INFO_STREAM("WARNING:: Imu time stamp is repeated.");
} else if (is_annormal(_hard_time, hard_time_now)) { } else if (is_annormal(_hard_time, hard_time_now)) {
NODELET_INFO_STREAM("WARNING:: Imu time stamp is annormal."); NODELET_INFO_STREAM("WARNING:: Imu time stamp is annormal.");
} }
*/
hard_time_now = _hard_time; hard_time_now = _hard_time;
return hardTimeToSoftTime( return hardTimeToSoftTime(
@ -166,10 +169,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
void onInit() override { void onInit() override {
nh_ = getMTNodeHandle(); nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle(); private_nh_ = getMTPrivateNodeHandle();
int request_index = 0;
private_nh_.getParam("request_index", request_index);
initDevice(request_index); initDevice();
NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :("); NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
// node params // node params
@ -225,20 +226,39 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
gravity_ = 9.8; gravity_ = 9.8;
private_nh_.getParam("gravity", gravity_); private_nh_.getParam("gravity", gravity_);
// device options // device options of standard2
if (model_ == Model::STANDARD2) {
option_names_ = {
{Option::BRIGHTNESS, "standard2/brightness"},
{Option::EXPOSURE_MODE, "standard2/exposure_mode"},
{Option::MAX_GAIN, "standard2/max_gain"},
{Option::MAX_EXPOSURE_TIME, "standard2/max_exposure_time"},
{Option::DESIRED_BRIGHTNESS, "standard2/desired_brightness"},
{Option::MIN_EXPOSURE_TIME, "standard2/min_exposure_time"},
{Option::ACCELEROMETER_RANGE, "standard2/accel_range"},
{Option::GYROSCOPE_RANGE, "standard2/gyro_range"},
{Option::ACCELEROMETER_LOW_PASS_FILTER, "standard2/accel_low_filter"},
{Option::GYROSCOPE_LOW_PASS_FILTER, "standard2/gyro_low_filter"}};
}
// 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"}};
}
std::map<Option, std::string> option_names = { for (auto &&it = option_names_.begin(); it != option_names_.end(); ++it) {
{Option::BRIGHTNESS, "brightness"},
{Option::EXPOSURE_MODE, "exposure_mode"},
{Option::MAX_GAIN, "max_gain"},
{Option::MAX_EXPOSURE_TIME, "max_exposure_time"},
{Option::DESIRED_BRIGHTNESS, "desired_brightness"},
{Option::MIN_EXPOSURE_TIME, "min_exposure_time"},
{Option::ACCELEROMETER_RANGE, "accel_range"},
{Option::GYROSCOPE_RANGE, "gyro_range"},
{Option::ACCELEROMETER_LOW_PASS_FILTER, "accel_low_filter"},
{Option::GYROSCOPE_LOW_PASS_FILTER, "gyro_low_filter"}};
for (auto &&it = option_names.begin(); it != option_names.end(); ++it) {
if (!api_->Supports(it->first)) if (!api_->Supports(it->first))
continue; continue;
int value = -1; int value = -1;
@ -264,22 +284,34 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized on topic " << topic); NODELET_INFO_STREAM("Advertized on topic " << topic);
} }
for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) { // Only STANDARD2 need publish mono_topics
auto &&topic = mono_topics[it->first]; if (model_ == Model::STANDARD2) {
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) {
mono_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1); auto &&topic = mono_topics[it->first];
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {
mono_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
}
NODELET_INFO_STREAM("Advertized on topic " << topic);
} }
NODELET_INFO_STREAM("Advertized on topic " << topic);
} }
if (model_ == Model::STANDARD2) {
camera_encodings_ = {{Stream::LEFT, enc::BGR8}, camera_encodings_ = {{Stream::LEFT, enc::BGR8},
{Stream::RIGHT, enc::BGR8}, {Stream::RIGHT, enc::BGR8},
{Stream::LEFT_RECTIFIED, enc::BGR8}, {Stream::LEFT_RECTIFIED, enc::BGR8},
{Stream::RIGHT_RECTIFIED, enc::BGR8}, {Stream::RIGHT_RECTIFIED, enc::BGR8},
{Stream::DISPARITY, enc::MONO8}, // float {Stream::DISPARITY, enc::MONO8}, // float
{Stream::DISPARITY_NORMALIZED, enc::MONO8}, {Stream::DISPARITY_NORMALIZED, enc::MONO8},
{Stream::DEPTH, enc::MONO16}}; {Stream::DEPTH, enc::MONO16}};
}
if (model_ == Model::STANDARD) {
camera_encodings_ = {{Stream::LEFT, enc::MONO8},
{Stream::RIGHT, enc::MONO8},
{Stream::LEFT_RECTIFIED, enc::MONO8},
{Stream::RIGHT_RECTIFIED, enc::MONO8},
{Stream::DISPARITY, enc::MONO8}, // float
{Stream::DISPARITY_NORMALIZED, enc::MONO8},
{Stream::DEPTH, enc::MONO16}};
}
pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 100); pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 100);
NODELET_INFO_STREAM("Advertized on topic " << imu_topic); NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
@ -443,29 +475,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
!is_published_[Stream::LEFT]) { !is_published_[Stream::LEFT]) {
api_->SetStreamCallback( api_->SetStreamCallback(
Stream::LEFT, [this](const api::StreamData &data) { Stream::LEFT, [this](const api::StreamData &data) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::LEFT);
// static double img_time_prev = -1;
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
// ros_time_beg
// << ", img_time_elapsed: " << FULL_PRECISION
// << ((data.img->timestamp - img_time_beg) * 0.00001f)
// << ", img_time_diff: " << FULL_PRECISION
// << ((img_time_prev < 0) ? 0
// : (data.img->timestamp - img_time_prev) * 0.01f) << "
// ms");
// img_time_prev = data.img->timestamp;
++left_count_; ++left_count_;
publishCamera(Stream::LEFT, data, left_count_, stamp); if (left_count_ > 10) {
publishMono(Stream::LEFT, data, left_count_, stamp); // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
NODELET_DEBUG_STREAM( ros::Time stamp = checkUpTimeStamp(
Stream::LEFT << ", count: " << left_count_ data.img->timestamp, Stream::LEFT);
<< ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp // static double img_time_prev = -1;
<< ", exposure_time: " << data.img->exposure_time); // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
// ros_time_beg
// << ", img_time_elapsed: " << FULL_PRECISION
// << ((data.img->timestamp - img_time_beg) * 0.00001f)
// << ", img_time_diff: " << FULL_PRECISION
// << ((img_time_prev < 0) ? 0
// : (data.img->timestamp - img_time_prev) * 0.01f) << "
// ms");
// img_time_prev = data.img->timestamp;
publishCamera(Stream::LEFT, data, left_count_, stamp);
publishMono(Stream::LEFT, data, left_count_, stamp);
NODELET_DEBUG_STREAM(
Stream::LEFT << ", count: " << left_count_
<< ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: "
<< data.img->exposure_time);
}
}); });
left_time_beg_ = ros::Time::now().toSec(); left_time_beg_ = ros::Time::now().toSec();
is_published_[Stream::LEFT] = true; is_published_[Stream::LEFT] = true;
@ -476,11 +510,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
!is_published_[Stream::RIGHT]) { !is_published_[Stream::RIGHT]) {
api_->SetStreamCallback( api_->SetStreamCallback(
Stream::RIGHT, [this](const api::StreamData &data) { Stream::RIGHT, [this](const api::StreamData &data) {
++right_count_;
if (right_count_ > 10) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp( ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::RIGHT); data.img->timestamp, Stream::RIGHT);
++right_count_;
publishCamera(Stream::RIGHT, data, right_count_, stamp); publishCamera(Stream::RIGHT, data, right_count_, stamp);
publishMono(Stream::RIGHT, data, right_count_, stamp); publishMono(Stream::RIGHT, data, right_count_, stamp);
NODELET_DEBUG_STREAM( NODELET_DEBUG_STREAM(
@ -488,6 +522,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
<< ", count: " << right_count_ << ", frame_id: " << ", count: " << right_count_ << ", frame_id: "
<< data.img->frame_id << ", timestamp: " << data.img->timestamp << data.img->frame_id << ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time); << ", exposure_time: " << data.img->exposure_time);
}
}); });
right_time_beg_ = ros::Time::now().toSec(); right_time_beg_ = ros::Time::now().toSec();
is_published_[Stream::RIGHT] = true; is_published_[Stream::RIGHT] = true;
@ -518,38 +553,40 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// << ((imu_time_prev < 0) ? 0 // << ((imu_time_prev < 0) ? 0
// : (data.imu->timestamp - imu_time_prev) * 0.01f) << " ms"); // : (data.imu->timestamp - imu_time_prev) * 0.01f) << " ms");
// imu_time_prev = data.imu->timestamp; // imu_time_prev = data.imu->timestamp;
++imu_count_; ++imu_count_;
if (publish_imu_by_sync_) { if (imu_count_ > 50) {
if (data.imu) { if (publish_imu_by_sync_) {
if (data.imu->flag == 1) { // accelerometer if (data.imu) {
imu_accel_ = data.imu; if (data.imu->flag == 1) { // accelerometer
publishImuBySync(stamp); imu_accel_ = data.imu;
} else if (data.imu->flag == 2) { // gyroscope publishImuBySync(stamp);
imu_gyro_ = data.imu; } else if (data.imu->flag == 2) { // gyroscope
publishImuBySync(stamp); imu_gyro_ = data.imu;
publishImuBySync(stamp);
} else {
publishImu(data, imu_count_, stamp);
publishTemp(data.imu->temperature, imu_count_, stamp);
}
} else { } else {
publishImu(data, imu_count_, stamp); NODELET_WARN_STREAM("Motion data is empty");
publishTemp(data.imu->temperature, imu_count_, stamp);
} }
} else { } else {
NODELET_WARN_STREAM("Motion data is empty"); publishImu(data, imu_count_, stamp);
publishTemp(data.imu->temperature, imu_count_, stamp);
} }
} else { NODELET_DEBUG_STREAM(
publishImu(data, imu_count_, stamp); "Imu count: " << imu_count_
publishTemp(data.imu->temperature, imu_count_, stamp); << ", 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);
// Sleep 1ms, otherwise publish may drop some datas.
ros::Duration(0.001).sleep();
} }
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]
<< ", gyro_x: " << data.imu->gyro[0]
<< ", gyro_y: " << data.imu->gyro[1]
<< ", gyro_z: " << data.imu->gyro[2]
<< ", temperature: " << data.imu->temperature);
// Sleep 1ms, otherwise publish may drop some datas.
ros::Duration(0.001).sleep();
}); });
imu_time_beg_ = ros::Time::now().toSec(); imu_time_beg_ = ros::Time::now().toSec();
is_motion_published_ = true; is_motion_published_ = true;
@ -789,7 +826,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
private: private:
void initDevice(int request_index) { void initDevice() {
NODELET_INFO_STREAM("Detecting MYNT EYE devices"); NODELET_INFO_STREAM("Detecting MYNT EYE devices");
Context context; Context context;
@ -827,6 +864,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
api_ = API::Create(device); api_ = API::Create(device);
auto &&requests = device->GetStreamRequests(); auto &&requests = device->GetStreamRequests();
std::size_t m = requests.size(); std::size_t m = requests.size();
int request_index = 0;
model_ = api_->GetModel();
if (model_ == Model::STANDARD2)
private_nh_.getParam("request_index", request_index);
NODELET_FATAL_COND(m <= 0, "No MYNT EYE devices :("); NODELET_FATAL_COND(m <= 0, "No MYNT EYE devices :(");
if (m <= 1) { if (m <= 1) {
@ -837,7 +879,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_WARN_STREAM("Resquest_index out of range"); NODELET_WARN_STREAM("Resquest_index out of range");
api_->ConfigStreamRequest(requests[0]); api_->ConfigStreamRequest(requests[0]);
} else { } else {
NODELET_WARN_STREAM("request_index: " << request_index);
api_->ConfigStreamRequest(requests[request_index]); api_->ConfigStreamRequest(requests[request_index]);
} }
} }
@ -1096,6 +1137,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
ros::NodeHandle nh_; ros::NodeHandle nh_;
ros::NodeHandle private_nh_; ros::NodeHandle private_nh_;
Model model_;
std::map<Option, std::string> option_names_;
// camera: // camera:
// LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED, // LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED,
// DISPARITY, DISPARITY_NORMALIZED, // DISPARITY, DISPARITY_NORMALIZED,