feat: add ROS camera info api.

samples/tutorials/data/get_img_params.cc

@@ -21,10 +21,7 @@ int main(int argc, char *argv[]) {
   auto &&api = API::Create(argc, argv);
   if (!api) return 1;
 
-  auto info = api->GetCameraROSMsgInfoPair();
 
-  if (!info->isEmpty())
-    std::cout << "ROSMsgInfoPair:"<< std::endl << *info << std::endl;
 
   bool ok;
   auto &&request = api->SelectStreamRequest(&ok);
@@ -49,5 +46,10 @@ int main(int argc, char *argv[]) {
   LOG(INFO) << "Extrinsics right to left: {"
             << api->GetExtrinsics(Stream::RIGHT, Stream::LEFT) << "}";
 
+  auto info = api->GetCameraROSMsgInfoPair();
+
+  if (!info->isEmpty())
+    std::cout << "ROSMsgInfoPair:"<< std::endl << *info << std::endl;
+
   return 0;
 }

src/mynteye/device/channel/file_channel.cc

@@ -429,23 +429,31 @@ std::size_t ImuParamsParser::GetFromData(
     const std::uint8_t *data, const std::uint16_t &data_size,
     imu_params_t *imu_params) const {
   // s1030 old params
-  if (spec_version_ == Version(1, 0) && data_size == 384) {
+  if (spec_version_ == Version(1, 0)) {
-    return GetFromData_old(data, data_size, imu_params);
+    if (data_size == 384) {
+      return GetFromData_old(data, data_size, imu_params);
+    } else if (data_size == 386) {
+      return GetFromData_new(data, data_size, imu_params, false);
+    }
   }
   // s210a old params
-  if (spec_version_ >= Version(1, 1) && data_size == 384) {
+  if (spec_version_ >= Version(1, 1)) {
-    return GetFromData_old(data, data_size, imu_params);
+    if (data_size == 384) {
+      return GetFromData_old(data, data_size, imu_params);
+    } else if (data_size == 386) {
+      return GetFromData_new(data, data_size, imu_params, false);
+    }
   }
   // get imu params with new version format
-  return GetFromData_new(data, data_size, imu_params);
+  return GetFromData_new(data, data_size, imu_params, true);
 }
 
 std::size_t ImuParamsParser::SetToData(
     const imu_params_t *imu_params, std::uint8_t *data) const {
   if (spec_version_ >= Version(1, 2)) {
-    return SetToData_bool_new(imu_params, data, true);
+    return SetToData_new(imu_params, data, true);
   } else {
-    return SetToData_bool_new(imu_params, data, false);
+    return SetToData_new(imu_params, data, false);
   }
 }
 
@@ -477,7 +485,7 @@ std::size_t ImuParamsParser::SetToData_old(
 
 std::size_t ImuParamsParser::GetFromData_new(
     const std::uint8_t *data, const std::uint16_t &data_size,
-    imu_params_t *imu_params) const {
+    imu_params_t *imu_params, const bool is_get) const {
   std::size_t i = 0;
   // version, 2
   Version version(data[i], data[i + 1]);
@@ -485,9 +493,15 @@ std::size_t ImuParamsParser::GetFromData_new(
   i += 2;
   // get imu params according to version
   if (version == Version(1, 2)) {  // v1.2
-    i += bytes::from_data(&imu_params->in_accel, data + i, true);
+    if (is_get) {
-    i += bytes::from_data(&imu_params->in_gyro, data + i, true);
+      i += bytes::from_data(&imu_params->in_accel, data + i, true);
-    i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
+      i += bytes::from_data(&imu_params->in_gyro, data + i, true);
+      i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
+    } else {
+      i += bytes::from_data(&imu_params->in_accel, data + i, false);
+      i += bytes::from_data(&imu_params->in_gyro, data + i, false);
+      i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
+    }
   } else {
     LOG(FATAL) << "Could not get imu params of version "
         << version.to_string() << ", please use latest SDK.";
@@ -497,35 +511,8 @@ std::size_t ImuParamsParser::GetFromData_new(
 }
 
 std::size_t ImuParamsParser::SetToData_new(
-    const imu_params_t *imu_params, std::uint8_t *data) const {
+    const imu_params_t *imu_params,
-  std::size_t i = 3;  // skip id, size
+    std::uint8_t *data, const bool is_set) const {
-
-  Version version_new(1, 2);  // new version
-  Version version_raw(imu_params->version);
-
-  // version, 2
-  data[i] = version_new.major();
-  data[i + 1] = version_new.minor();
-  i += 2;
-  // set imu params with new version format
-  if (version_raw <= version_new) {
-    i += bytes::to_data(&imu_params->in_accel, data + i, true);
-    i += bytes::to_data(&imu_params->in_gyro, data + i, true);
-    i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
-  } else {
-    LOG(FATAL) << "Could not set imu params of version "
-        << version_raw.to_string() << ", please use latest SDK.";
-  }
-  // others
-  std::size_t size = i - 3;
-  data[0] = FID_IMU_PARAMS;
-  data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
-  data[2] = static_cast<std::uint8_t>(size & 0xFF);
-  return size + 3;
-}
-
-std::size_t ImuParamsParser::SetToData_bool_new(
-    const imu_params_t *imu_params, std::uint8_t *data, bool is_set) const {
   std::size_t i = 3;  // skip id, size
 
   Version version_new(1, 2);  // new version

src/mynteye/device/channel/file_channel.h

@@ -140,12 +140,10 @@ class ImuParamsParser {
 
   std::size_t GetFromData_new(
       const std::uint8_t *data, const std::uint16_t &data_size,
-      imu_params_t *imu_params) const;
+      imu_params_t *imu_params, const bool is_get) const;
-  std::size_t SetToData_new(
-      const imu_params_t *imu_params, std::uint8_t *data) const;
 
-  std::size_t SetToData_bool_new(
+  std::size_t SetToData_new(
-      const imu_params_t *imu_params, std::uint8_t *data, bool is_set) const;
+      const imu_params_t *imu_params, std::uint8_t *data, const bool is_set) const;
 
  private:
   Version spec_version_;