Merge branch 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk into develop

CMakeLists.txt

@@ -14,7 +14,7 @@
 
 cmake_minimum_required(VERSION 3.0)
 
-project(mynteye VERSION 2.3.5 LANGUAGES C CXX)
+project(mynteye VERSION 2.3.6 LANGUAGES C CXX)
 
 include(cmake/Common.cmake)
 

README.md

@@ -1,6 +1,6 @@
 # MYNT® EYE S SDK
 
-[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.5-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
+[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.6-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
 
 ## Overview
 

docs/conf.py

@@ -24,7 +24,7 @@ copyright = '2018, MYNTAI'
 author = 'MYNTAI'
 
 # The short X.Y version
-version = '2.3.5'
+version = '2.3.6'
 # The full version, including alpha/beta/rc tags
 release = version
 

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
 

docs/src/sdk/changelog.rst

@@ -3,6 +3,13 @@
 Changelog
 =========
 
+2019-04-15(v2.3.6)
+-------------------
+
+1. Fix imu align bug of ros wrapper
+2. Fix 14.04 complie error of ros wrapper
+3. Support set iic address for s2100
+
 2019-04-01(v2.3.5)
 -------------------
 
@@ -21,11 +28,9 @@ Changelog
 7. Modify default orientation of point in ROS.
 
 
-
 2019-03-18(v2.3.4)
 -------------------
 
-
 1. Add API to get subsidiary chip&ISP's version(Depend on S2100/S210A 1.1 firmware & 1.0 subsidiary chip firmware).
 
 2. Fix point fragment issue in BM algorithm.
@@ -38,4 +43,4 @@ Changelog
 
 6. Fix depth image crash issue when use CUDA plugin.
 
-7. Documents update.
+7. Documents update.

docs/src/sdk/exe_install_windows.rst

@@ -26,7 +26,7 @@ Download and install SDK
 
 .. tip::
 
-  Download here: mynteye-s-2.3.4-win-x64-opencv-3.4.3.exe `Google Drive <https://drive.google.com/open?id=1PYC_5Mh2pzLFVXkYlkllEzPnr50EbKht>`_ `Baidu Pan <https://pan.baidu.com/s/1s4KIcuYkO5i_9E1pG5blQA>`_ .
+  Download here: mynteye-s-2.3.6-win-x64-opencv-3.4.3.exe `Google Drive <https://drive.google.com/open?id=1PYC_5Mh2pzLFVXkYlkllEzPnr50EbKht>`_ `Baidu Pan <https://pan.baidu.com/s/1s4KIcuYkO5i_9E1pG5blQA>`_ .
 
 After you install the win pack of SDK, there will be a shortcut to the SDK root directory on your desktop.
 

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
+  // MYNTEYE-S1030 don't support this option
-  if (model == Model::STANDARD2 || model == Model::STANDARD) {
+  if (model == Model::STANDARD) {
-    LOG(INFO) << "Sorry,MYNTEYE-S1030/S2100 don't support iic address setting";
+    LOG(INFO) << "Sorry,MYNTEYE-S1030 don't support iic address setting";
     return 0;
   }
 

src/mynteye/device/config.cc

@@ -45,7 +45,8 @@ const std::map<Model, OptionSupports> 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,

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<double>::max_digits10)
 
+static const std::size_t MAXSIZE = 4;
+
 MYNTEYE_BEGIN_NAMESPACE
 
 namespace enc = sensor_msgs::image_encodings;
@@ -927,121 +929,111 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   }
 
   void timestampAlign() {
-    static bool get_first_acc = false;
+    static std::vector<ImuData> acc_buf;
-    static bool get_first_gyro = false;
+    static std::vector<ImuData> gyro_buf;
-    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;
-    }
 
     if (imu_accel_ != nullptr) {
-      if (!has_gyro) {
+      acc_buf.push_back(*imu_accel_);
-        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<double>(imu_accel_->timestamp - acc.timestamp);
-        k = static_cast<double>(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;
-      }
     }
 
     if (imu_gyro_ != nullptr) {
-      has_gyro = true;
+      gyro_buf.push_back(*imu_gyro_);
-      gyro = *imu_gyro_;
+    }
-      imu_gyro_ = nullptr;
+
+    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<double>((it_acc+1)->timestamp - it_acc->timestamp);
+          k = static_cast<double>(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) {
+    for (int i = 0; i < imu_align_.size(); i++) {
-      return;
+      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(
@@ -1343,7 +1335,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);
             }
@@ -1590,6 +1582,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   bool is_started_;
   int frame_rate_;
   bool is_intrinsics_enable_;
+  std::vector<ImuData> imu_align_;
 };
 
 MYNTEYE_END_NAMESPACE