From 0e4896f788bc3086f5c0872e68cf8575dd4dc097 Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 28 Dec 2018 17:56:12 +0800 Subject: [PATCH] fix(ros): fix option setting bug --- samples/tutorials/control/auto_exposure.cc | 5 +- samples/tutorials/control/manual_exposure.cc | 5 +- samples/tutorials/util/cv_painter.cc | 2 +- .../launch/mynteye.launch | 141 +++++++---- .../mynt_eye_ros_wrapper/rviz/mynteye.rviz | 28 --- .../src/wrapper_nodelet.cc | 231 +++++++++++------- 6 files changed, 242 insertions(+), 170 deletions(-) diff --git a/samples/tutorials/control/auto_exposure.cc b/samples/tutorials/control/auto_exposure.cc index 72dcae6..ba5ae65 100644 --- a/samples/tutorials/control/auto_exposure.cc +++ b/samples/tutorials/control/auto_exposure.cc @@ -24,6 +24,7 @@ int main(int argc, char *argv[]) { auto &&api = API::Create(argc, argv); if (!api) return 1; + std::int32_t frame_rate = 0; bool ok; auto &&request = api->SelectStreamRequest(&ok); if (!ok) return 1; @@ -43,6 +44,8 @@ int main(int argc, char *argv[]) { // desired_brightness: range [0,255], default 192 api->SetOptionValue(Option::DESIRED_BRIGHTNESS, 192); + frame_rate = api->GetOptionValue(Option::FRAME_RATE); + LOG(INFO) << "Enable auto-exposure"; LOG(INFO) << "Set EXPOSURE_MODE to " << api->GetOptionValue(Option::EXPOSURE_MODE); @@ -83,7 +86,7 @@ int main(int argc, char *argv[]) { api->Start(Source::VIDEO_STREAMING); - CVPainter painter(30); + CVPainter painter(frame_rate); cv::namedWindow("frame"); diff --git a/samples/tutorials/control/manual_exposure.cc b/samples/tutorials/control/manual_exposure.cc index 8c84deb..51f1a44 100644 --- a/samples/tutorials/control/manual_exposure.cc +++ b/samples/tutorials/control/manual_exposure.cc @@ -24,6 +24,7 @@ int main(int argc, char *argv[]) { auto &&api = API::Create(argc, argv); if (!api) return 1; + std::int32_t frame_rate = 0; bool ok; auto &&request = api->SelectStreamRequest(&ok); if (!ok) return 1; @@ -42,6 +43,8 @@ int main(int argc, char *argv[]) { // contrast/black_level_calibration: range [0,255], default 127 api->SetOptionValue(Option::CONTRAST, 127); + frame_rate = api->GetOptionValue(Option::FRAME_RATE); + LOG(INFO) << "Enable manual-exposure"; LOG(INFO) << "Set EXPOSURE_MODE to " << api->GetOptionValue(Option::EXPOSURE_MODE); @@ -68,7 +71,7 @@ int main(int argc, char *argv[]) { api->Start(Source::VIDEO_STREAMING); - CVPainter painter(30); + CVPainter painter(frame_rate); cv::namedWindow("frame"); diff --git a/samples/tutorials/util/cv_painter.cc b/samples/tutorials/util/cv_painter.cc index cc9fefe..d5a1271 100644 --- a/samples/tutorials/util/cv_painter.cc +++ b/samples/tutorials/util/cv_painter.cc @@ -82,7 +82,7 @@ cv::Rect CVPainter::DrawImgData( std::ostringstream ss; 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: "; if (frame_rate_ == 0) { ss << data.exposure_time; 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 d08d91f..0487305 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -52,9 +52,6 @@ - - - @@ -64,47 +61,101 @@ - + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -155,18 +206,18 @@ - + - - - - - - - - - - + + + + + + + + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz index 7ad1d90..c448a18 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz @@ -78,34 +78,6 @@ Visualization Manager: Transport Hint: raw Unreliable: false 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 Displays: - Class: rviz/Image 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 634133c..e74f322 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 @@ -104,31 +104,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet { static_cast(_hard_time - hard_time_begin) * 0.000001f)); } - inline bool is_overflow(std::uint32_t now, - std::uint32_t pre) { + inline bool is_overflow(std::uint64_t now, + std::uint64_t pre) { static std::uint64_t unit = std::numeric_limits::max(); return (now < pre) && ((pre - now) > (unit / 2)); } - inline bool is_repeated(std::uint32_t now, - std::uint32_t pre) { + inline bool is_repeated(std::uint64_t now, + std::uint64_t pre) { return now == pre; } - inline bool is_annormal(std::uint32_t now, - std::uint32_t pre) { + inline bool is_annormal(std::uint64_t now, + std::uint64_t pre) { static std::uint64_t unit = std::numeric_limits::max(); 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) { - static std::map hard_time_now; - static std::map acc; + static std::map hard_time_now; + static std::map acc; static std::uint64_t unit_hard_time = std::numeric_limits::max(); @@ -145,18 +145,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet { acc[stream] * unit_hard_time + _hard_time); } - ros::Time checkUpImuTimeStamp(std::uint32_t _hard_time) { - static std::uint32_t hard_time_now(0), acc(0); + ros::Time checkUpImuTimeStamp(std::uint64_t _hard_time) { + static std::uint64_t hard_time_now(0), acc(0); static std::uint64_t unit_hard_time = std::numeric_limits::max(); if (is_overflow(_hard_time, hard_time_now)) { 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."); } else if (is_annormal(_hard_time, hard_time_now)) { NODELET_INFO_STREAM("WARNING:: Imu time stamp is annormal."); } + */ hard_time_now = _hard_time; return hardTimeToSoftTime( @@ -166,10 +169,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { void onInit() override { nh_ = getMTNodeHandle(); 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 :("); // node params @@ -225,20 +226,39 @@ class ROSWrapperNodelet : public nodelet::Nodelet { gravity_ = 9.8; 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_names = { - {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) { + for (auto &&it = option_names_.begin(); it != option_names_.end(); ++it) { if (!api_->Supports(it->first)) continue; int value = -1; @@ -264,22 +284,34 @@ class ROSWrapperNodelet : public nodelet::Nodelet { NODELET_INFO_STREAM("Advertized on topic " << topic); } - 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) { // camera - mono_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1); + // Only STANDARD2 need publish mono_topics + if (model_ == Model::STANDARD2) { + 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) { + mono_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1); + } + NODELET_INFO_STREAM("Advertized on topic " << topic); } - NODELET_INFO_STREAM("Advertized on topic " << topic); } - - camera_encodings_ = {{Stream::LEFT, enc::BGR8}, - {Stream::RIGHT, enc::BGR8}, - {Stream::LEFT_RECTIFIED, enc::BGR8}, - {Stream::RIGHT_RECTIFIED, enc::BGR8}, - {Stream::DISPARITY, enc::MONO8}, // float - {Stream::DISPARITY_NORMALIZED, enc::MONO8}, - {Stream::DEPTH, enc::MONO16}}; - + if (model_ == Model::STANDARD2) { + camera_encodings_ = {{Stream::LEFT, enc::BGR8}, + {Stream::RIGHT, enc::BGR8}, + {Stream::LEFT_RECTIFIED, enc::BGR8}, + {Stream::RIGHT_RECTIFIED, enc::BGR8}, + {Stream::DISPARITY, enc::MONO8}, // float + {Stream::DISPARITY_NORMALIZED, enc::MONO8}, + {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(imu_topic, 100); NODELET_INFO_STREAM("Advertized on topic " << imu_topic); @@ -443,29 +475,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet { !is_published_[Stream::LEFT]) { api_->SetStreamCallback( 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_; - 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); + if (left_count_ > 10) { + // 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; + 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(); is_published_[Stream::LEFT] = true; @@ -476,11 +510,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet { !is_published_[Stream::RIGHT]) { api_->SetStreamCallback( Stream::RIGHT, [this](const api::StreamData &data) { + ++right_count_; + if (right_count_ > 10) { // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); ros::Time stamp = checkUpTimeStamp( data.img->timestamp, Stream::RIGHT); - - ++right_count_; publishCamera(Stream::RIGHT, data, right_count_, stamp); publishMono(Stream::RIGHT, data, right_count_, stamp); NODELET_DEBUG_STREAM( @@ -488,6 +522,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { << ", count: " << right_count_ << ", frame_id: " << data.img->frame_id << ", timestamp: " << data.img->timestamp << ", exposure_time: " << data.img->exposure_time); + } }); right_time_beg_ = ros::Time::now().toSec(); is_published_[Stream::RIGHT] = true; @@ -518,38 +553,40 @@ class ROSWrapperNodelet : public nodelet::Nodelet { // << ((imu_time_prev < 0) ? 0 // : (data.imu->timestamp - imu_time_prev) * 0.01f) << " ms"); // imu_time_prev = data.imu->timestamp; - ++imu_count_; - if (publish_imu_by_sync_) { - if (data.imu) { - if (data.imu->flag == 1) { // accelerometer - imu_accel_ = data.imu; - publishImuBySync(stamp); - } else if (data.imu->flag == 2) { // gyroscope - imu_gyro_ = data.imu; - publishImuBySync(stamp); + if (imu_count_ > 50) { + if (publish_imu_by_sync_) { + if (data.imu) { + if (data.imu->flag == 1) { // accelerometer + imu_accel_ = data.imu; + publishImuBySync(stamp); + } else if (data.imu->flag == 2) { // gyroscope + imu_gyro_ = data.imu; + publishImuBySync(stamp); + } else { + publishImu(data, imu_count_, stamp); + publishTemp(data.imu->temperature, imu_count_, stamp); + } } else { - publishImu(data, imu_count_, stamp); - publishTemp(data.imu->temperature, imu_count_, stamp); + NODELET_WARN_STREAM("Motion data is empty"); } } else { - NODELET_WARN_STREAM("Motion data is empty"); + publishImu(data, imu_count_, stamp); + publishTemp(data.imu->temperature, imu_count_, stamp); } - } else { - publishImu(data, imu_count_, stamp); - publishTemp(data.imu->temperature, imu_count_, stamp); + 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(); } - 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(); is_motion_published_ = true; @@ -789,7 +826,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } private: - void initDevice(int request_index) { + void initDevice() { NODELET_INFO_STREAM("Detecting MYNT EYE devices"); Context context; @@ -827,6 +864,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet { api_ = API::Create(device); auto &&requests = device->GetStreamRequests(); 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 :("); if (m <= 1) { @@ -837,7 +879,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet { NODELET_WARN_STREAM("Resquest_index out of range"); api_->ConfigStreamRequest(requests[0]); } else { - NODELET_WARN_STREAM("request_index: " << request_index); api_->ConfigStreamRequest(requests[request_index]); } } @@ -1096,6 +1137,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ros::NodeHandle nh_; ros::NodeHandle private_nh_; + Model model_; + std::map option_names_; // camera: // LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED, // DISPARITY, DISPARITY_NORMALIZED,