From 9a13909de29807912cf5262985d4080c63bf6fcf Mon Sep 17 00:00:00 2001 From: Osenberg Date: Thu, 11 Apr 2019 18:15:58 +0800 Subject: [PATCH 1/5] fix(ros): fixed bug of imu align for part of device n --- .../src/wrapper_nodelet.cc | 197 +++++++++--------- 1 file changed, 95 insertions(+), 102 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 3a1807f..d1c8d97 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 @@ -46,6 +46,8 @@ using namespace configuru; // NOLINT #define FULL_PRECISION \ std::fixed << std::setprecision(std::numeric_limits::max_digits10) +static const std::size_t MAXSIZE = 4; + MYNTEYE_BEGIN_NAMESPACE namespace enc = sensor_msgs::image_encodings; @@ -918,121 +920,111 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } void timestampAlign() { - static bool get_first_acc = false; - static bool get_first_gyro = false; - static bool has_gyro = false; - static ImuData acc; - static ImuData gyro; - - if (!get_first_acc && imu_accel_ != nullptr) { - acc = *imu_accel_; - imu_accel_ = nullptr; - get_first_acc = true; - return; - } - - if (!get_first_gyro && imu_gyro_ != nullptr) { - gyro = *imu_gyro_; - imu_gyro_ = nullptr; - get_first_gyro = true; - return; - } + static std::vector acc_buf; + static std::vector gyro_buf; if (imu_accel_ != nullptr) { - if (!has_gyro) { - acc = *imu_accel_; - imu_accel_ = nullptr; - return; - } - - if (acc.timestamp <= gyro.timestamp) { - ImuData acc_temp; - acc_temp = *imu_accel_; - acc_temp.timestamp = gyro.timestamp; - - double k = static_cast(imu_accel_->timestamp - acc.timestamp); - k = static_cast(gyro.timestamp - acc.timestamp) / k; - - acc_temp.accel[0] = acc.accel[0] + - (imu_accel_->accel[0] - acc.accel[0]) * k; - acc_temp.accel[1] = acc.accel[1] + - (imu_accel_->accel[1] - acc.accel[1]) * k; - acc_temp.accel[2] = acc.accel[2] + - (imu_accel_->accel[2] - acc.accel[2]) * k; - - acc = *imu_accel_; - *imu_accel_ = acc_temp; - imu_gyro_.reset(new ImuData(gyro)); - has_gyro = false; - return; - } else { - acc = *imu_accel_; - imu_accel_ = nullptr; - return; - } + acc_buf.push_back(*imu_accel_); } if (imu_gyro_ != nullptr) { - has_gyro = true; - gyro = *imu_gyro_; - imu_gyro_ = nullptr; + gyro_buf.push_back(*imu_gyro_); + } + + imu_accel_ = nullptr; + imu_gyro_ = nullptr; + + imu_align_.clear(); + + if (acc_buf.empty() || gyro_buf.empty()) { return; } + + ImuData imu_temp; + auto itg = gyro_buf.end(); + auto ita = acc_buf.end(); + for (auto it_gyro = gyro_buf.begin(); + it_gyro != gyro_buf.end(); it_gyro++) { + for (auto it_acc = acc_buf.begin(); + it_acc+1 != acc_buf.end(); it_acc++) { + if (it_gyro->timestamp >= it_acc->timestamp + && it_gyro->timestamp <= (it_acc+1)->timestamp) { + double k = static_cast((it_acc+1)->timestamp - it_acc->timestamp); + k = static_cast(it_gyro->timestamp - it_acc->timestamp) / k; + + imu_temp = *it_gyro; + imu_temp.accel[0] = it_acc->accel[0] + ((it_acc+1)->accel[0] - it_acc->accel[0]) * k; + imu_temp.accel[1] = it_acc->accel[1] + ((it_acc+1)->accel[1] - it_acc->accel[1]) * k; + imu_temp.accel[2] = it_acc->accel[2] + ((it_acc+1)->accel[2] - it_acc->accel[2]) * k; + + imu_align_.push_back(imu_temp); + + itg = it_gyro; + ita = it_acc; + } + } + } + + if (itg != gyro_buf.end()) { + gyro_buf.erase(gyro_buf.begin(), itg + 1); + } + + if (ita != acc_buf.end()) { + acc_buf.erase(acc_buf.begin(), ita); + } } void publishImuBySync() { timestampAlign(); - if (imu_accel_ == nullptr || imu_gyro_ == nullptr) { - return; + for (int i = 0; i < imu_align_.size(); i++) { + sensor_msgs::Imu msg; + + msg.header.seq = imu_sync_count_; + ros::Time stamp = checkUpImuTimeStamp(imu_align_[i].timestamp); + msg.header.stamp = stamp; + msg.header.frame_id = imu_frame_id_; + + // acceleration should be in m/s^2 (not in g's) + msg.linear_acceleration.x = imu_align_[i].accel[0] * gravity_; + msg.linear_acceleration.y = imu_align_[i].accel[1] * gravity_; + msg.linear_acceleration.z = imu_align_[i].accel[2] * gravity_; + + msg.linear_acceleration_covariance[0] = 0; + msg.linear_acceleration_covariance[1] = 0; + msg.linear_acceleration_covariance[2] = 0; + + msg.linear_acceleration_covariance[3] = 0; + msg.linear_acceleration_covariance[4] = 0; + msg.linear_acceleration_covariance[5] = 0; + + msg.linear_acceleration_covariance[6] = 0; + msg.linear_acceleration_covariance[7] = 0; + msg.linear_acceleration_covariance[8] = 0; + + // velocity should be in rad/sec + msg.angular_velocity.x = imu_align_[i].gyro[0] * M_PI / 180; + msg.angular_velocity.y = imu_align_[i].gyro[1] * M_PI / 180; + msg.angular_velocity.z = imu_align_[i].gyro[2] * M_PI / 180; + + msg.angular_velocity_covariance[0] = 0; + msg.angular_velocity_covariance[1] = 0; + msg.angular_velocity_covariance[2] = 0; + + msg.angular_velocity_covariance[3] = 0; + msg.angular_velocity_covariance[4] = 0; + msg.angular_velocity_covariance[5] = 0; + + msg.angular_velocity_covariance[6] = 0; + msg.angular_velocity_covariance[7] = 0; + msg.angular_velocity_covariance[8] = 0; + + pub_imu_.publish(msg); + + publishTemperature(imu_align_[i].temperature, imu_sync_count_, stamp); + + ++imu_sync_count_; } - sensor_msgs::Imu msg; - ros::Time stamp = checkUpImuTimeStamp(imu_accel_->timestamp); - msg.header.seq = imu_sync_count_; - msg.header.stamp = stamp; - msg.header.frame_id = imu_frame_id_; - - // acceleration should be in m/s^2 (not in g's) - msg.linear_acceleration.x = imu_accel_->accel[0] * gravity_; - msg.linear_acceleration.y = imu_accel_->accel[1] * gravity_; - msg.linear_acceleration.z = imu_accel_->accel[2] * gravity_; - - msg.linear_acceleration_covariance[0] = 0; - msg.linear_acceleration_covariance[1] = 0; - msg.linear_acceleration_covariance[2] = 0; - - msg.linear_acceleration_covariance[3] = 0; - msg.linear_acceleration_covariance[4] = 0; - msg.linear_acceleration_covariance[5] = 0; - - msg.linear_acceleration_covariance[6] = 0; - msg.linear_acceleration_covariance[7] = 0; - msg.linear_acceleration_covariance[8] = 0; - - // velocity should be in rad/sec - msg.angular_velocity.x = imu_gyro_->gyro[0] * M_PI / 180; - msg.angular_velocity.y = imu_gyro_->gyro[1] * M_PI / 180; - msg.angular_velocity.z = imu_gyro_->gyro[2] * M_PI / 180; - - msg.angular_velocity_covariance[0] = 0; - msg.angular_velocity_covariance[1] = 0; - msg.angular_velocity_covariance[2] = 0; - - msg.angular_velocity_covariance[3] = 0; - msg.angular_velocity_covariance[4] = 0; - msg.angular_velocity_covariance[5] = 0; - - msg.angular_velocity_covariance[6] = 0; - msg.angular_velocity_covariance[7] = 0; - msg.angular_velocity_covariance[8] = 0; - - pub_imu_.publish(msg); - - publishTemperature(imu_accel_->temperature, imu_sync_count_, stamp); - - ++imu_sync_count_; - imu_accel_ = nullptr; - imu_gyro_ = nullptr; } void publishTemperature( @@ -1586,6 +1578,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { bool is_started_; int frame_rate_; bool is_intrinsics_enable_; + std::vector imu_align_; }; MYNTEYE_END_NAMESPACE From 05dc3e99ef5d38c3bcb5f4b7e3468b48ec9c22b7 Mon Sep 17 00:00:00 2001 From: Osenberg Date: Fri, 12 Apr 2019 11:31:53 +0800 Subject: [PATCH 2/5] feat(src): supported set iic address for s2100 --- samples/tutorials/control/iic_address.cc | 8 ++++---- src/mynteye/device/config.cc | 3 ++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/samples/tutorials/control/iic_address.cc b/samples/tutorials/control/iic_address.cc index 1401020..cd9667c 100644 --- a/samples/tutorials/control/iic_address.cc +++ b/samples/tutorials/control/iic_address.cc @@ -28,15 +28,15 @@ int main(int argc, char *argv[]) { api->ConfigStreamRequest(request); Model model = api->GetModel(); - if (model == Model::STANDARD210A) { + if (model == Model::STANDARD210A || model == Model::STANDARD2) { api->SetOptionValue(Option::IIC_ADDRESS_SETTING, 0x31); LOG(INFO) << "Set iic address to " << std::hex << "0x" << api->GetOptionValue(Option::IIC_ADDRESS_SETTING); } - // MYNTEYE-S1030/S2100 don't support this option - if (model == Model::STANDARD2 || model == Model::STANDARD) { - LOG(INFO) << "Sorry,MYNTEYE-S1030/S2100 don't support iic address setting"; + // MYNTEYE-S1030 don't support this option + if (model == Model::STANDARD) { + LOG(INFO) << "Sorry,MYNTEYE-S1030 don't support iic address setting"; return 0; } diff --git a/src/mynteye/device/config.cc b/src/mynteye/device/config.cc index c9c21db..a65489e 100644 --- a/src/mynteye/device/config.cc +++ b/src/mynteye/device/config.cc @@ -45,7 +45,8 @@ const std::map option_supports_map = { 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} + Option::GYROSCOPE_LOW_PASS_FILTER, Option::IIC_ADDRESS_SETTING, + Option::ERASE_CHIP} }, {Model::STANDARD210A, { Option::BRIGHTNESS, From c18a23b0597eef1db29299f1938555987a0268cb Mon Sep 17 00:00:00 2001 From: Osenberg Date: Fri, 12 Apr 2019 14:11:19 +0800 Subject: [PATCH 3/5] feat(doc): supported set address for 2100 --- docs/src/control/iic_address.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/src/control/iic_address.rst b/docs/src/control/iic_address.rst index d9c7c8d..85e921f 100644 --- a/docs/src/control/iic_address.rst +++ b/docs/src/control/iic_address.rst @@ -9,11 +9,11 @@ To set the IIC address, set ``Option::IIC_ADDRESS_SETTING``. .. Attention:: - Only support S210A + Only support S210A/2100 Reference Code: -s210a： +s210a/s2100： .. code-block:: c++ @@ -24,7 +24,7 @@ s210a： if (!ok) return 1; api->ConfigStreamRequest(request); Model model = api->GetModel(); - if (model == Model::STANDARD210A) { + if (model == Model::STANDARD210A || model == Model::STANDARD2) { api->SetOptionValue(Option::IIC_ADDRESS_SETTING, 0x31); LOG(INFO) << "Set iic address to " << std::hex << "0x" << api->GetOptionValue(Option::IIC_ADDRESS_SETTING); @@ -33,7 +33,7 @@ s210a： Reference running results on Linux: -s210a： +s210a/s2100： .. code-block:: bash From 2d8a66eb47a02c890e9ee56892139941d79cc2c7 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 12 Apr 2019 15:18:02 +0800 Subject: [PATCH 4/5] fix: 14.04 complie error --- .../mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 15 +++++---------- 1 file changed, 5 insertions(+), 10 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 d1c8d97..abbc614 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 @@ -1296,14 +1296,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet { if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) { if (info_pair->right.distortion_model == "KANNALA_BRANDT") { - camera_info->distortion_model = - sensor_msgs::distortion_models::EQUIDISTANT; + camera_info->distortion_model = "KANNALA_BRANDT"; for (size_t i; i < 4; i++) { camera_info->D.push_back(info_pair->right.D[i]); } } else if (info_pair->right.distortion_model == "PINHOLE") { - camera_info->distortion_model = - sensor_msgs::distortion_models::PLUMB_BOB; + camera_info->distortion_model = "plumb_bob"; for (size_t i; i < 5; i++) { camera_info->D.push_back(info_pair->right.D[i]); } @@ -1323,21 +1321,18 @@ class ROSWrapperNodelet : public nodelet::Nodelet { bool is_laserscan = false; private_nh_.getParamCached("is_laserscan", is_laserscan); if (!is_laserscan) { - camera_info->distortion_model = - sensor_msgs::distortion_models::EQUIDISTANT; + camera_info->distortion_model = "KANNALA_BRANDT"; for (size_t i; i < 4; i++) { camera_info->D.push_back(info_pair->left.D[i]); } } else { - camera_info->distortion_model = - sensor_msgs::distortion_models::PLUMB_BOB; + camera_info->distortion_model = "plumb_bob"; for (size_t i; i < 4; i++) { camera_info->D.push_back(0.0); } } } else if (info_pair->left.distortion_model == "PINHOLE") { - camera_info->distortion_model = - sensor_msgs::distortion_models::PLUMB_BOB; + camera_info->distortion_model = "plumb_bob"; for (size_t i; i < 5; i++) { camera_info->D.push_back(info_pair->left.D[i]); } From b3f6c82f8d86bdb3275751d2b169fbe4427082ee Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 12 Apr 2019 15:41:25 +0800 Subject: [PATCH 5/5] fix(ros): ros error enum error --- wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 abbc614..4b30a36 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 @@ -1326,7 +1326,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { camera_info->D.push_back(info_pair->left.D[i]); } } else { - camera_info->distortion_model = "plumb_bob"; + camera_info->distortion_model = "KANNALA_BRANDT"; for (size_t i; i < 4; i++) { camera_info->D.push_back(0.0); }