From 415cf31a36804365ed9966deb9a154879c97048e Mon Sep 17 00:00:00 2001 From: Osenberg Date: Sat, 23 Mar 2019 14:23:23 +0800 Subject: [PATCH 1/2] feat(ros): optimized multiple device process for ros --- .../launch/mynteye_multiple.launch | 2 - .../src/wrapper_nodelet.cc | 75 +++++++------------ 2 files changed, 28 insertions(+), 49 deletions(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch index 0782cf5..cf6cbad 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch @@ -2,6 +2,4 @@ - 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 40fbd53..30b4ac4 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 @@ -1098,6 +1098,18 @@ class ROSWrapperNodelet : public nodelet::Nodelet { Context context; auto &&devices = context.devices(); + size_t n = devices.size(); + NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :("); + + NODELET_INFO_STREAM("MYNT EYE devices:"); + for (size_t i = 0; i < n; i++) { + auto &&device = devices[i]; + auto &&name = device->GetInfo(Info::DEVICE_NAME); + auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER); + NODELET_INFO_STREAM(" index: " << i << ", name: " << + name << ", serial number: " << serial_number); + } + bool is_multiple = false; private_nh_.getParam("is_multiple", is_multiple); if (is_multiple) { @@ -1106,62 +1118,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet { NODELET_FATAL_COND(sn.empty(), "Must set serial_number " "in mynteye_1.launch and mynteye_2.launch."); - size_t n = devices.size(); - NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :("); - - NODELET_INFO_STREAM("MYNT EYE devices:"); for (size_t i = 0; i < n; i++) { auto &&device = devices[i]; auto &&name = device->GetInfo(Info::DEVICE_NAME); auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER); - NODELET_INFO_STREAM(" index: " << i << ", name: " << - name << ", serial number: " << serial_number); - } - for (size_t i = 0; i < n; i++) { - auto &&device = devices[i]; - auto &&name = device->GetInfo(Info::DEVICE_NAME); - auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER); - NODELET_INFO_STREAM(" index: " << i << ", name: " << - name << ", serial number: " << serial_number); if (sn == serial_number) return device; -#if 0 - if (i == (n - 1)) { - /* - NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found," - " check the serial_number configuration. "); - */ - NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found," - " check the serial_number configuration. "); - return nullptr; - } -#endif + NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found," + " check the serial_number configuration. "); } - } - size_t n = devices.size(); - NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :("); - - NODELET_INFO_STREAM("MYNT EYE devices:"); - for (size_t i = 0; i < n; i++) { - auto &&device = devices[i]; - auto &&name = device->GetInfo(Info::DEVICE_NAME); - NODELET_INFO_STREAM(" index: " << i << ", name: " << name); - } - - if (n <= 1) { - NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0"); - return devices[0]; } else { - while (true) { - size_t i; - NODELET_INFO_STREAM( - "There are " << n << " MYNT EYE devices, select index: "); - std::cin >> i; - if (i >= n) { - NODELET_WARN_STREAM("Index out of range :("); - continue; + if (n <= 1) { + NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0"); + return devices[0]; + } else { + while (true) { + size_t i; + NODELET_INFO_STREAM( + "There are " << n << " MYNT EYE devices, select index: "); + std::cin >> i; + if (i >= n) { + NODELET_WARN_STREAM("Index out of range :("); + continue; + } + return devices[i]; } - return devices[i]; } } From f25685153b823de9a1f73c4dfbb0d6fd75893e71 Mon Sep 17 00:00:00 2001 From: kalman Date: Sat, 23 Mar 2019 16:58:54 +0800 Subject: [PATCH 2/2] feat(*): add iic address setting api for s210a --- include/mynteye/types.h | 8 ++++++++ src/mynteye/device/channel/channels.cc | 11 +++++++++-- src/mynteye/device/config.cc | 2 +- src/mynteye/types.cc | 1 + .../libmynteye/src/main/cpp/mynteye/cpp/option.hpp | 5 +++++ .../src/main/cpp/mynteye/impl/type_conversion.hpp | 4 ++++ .../src/main/java/com/slightech/mynteye/Option.java | 5 +++++ wrappers/android/mynteye/scripts/mynteye_types.djinni | 4 ++++ .../mynt_eye_ros_wrapper/config/device/standard2.yaml | 6 +++++- .../src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 3 ++- 10 files changed, 44 insertions(+), 5 deletions(-) diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 26ea5dd..5e7f540 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -259,6 +259,14 @@ enum class Option : std::uint8_t { */ GYROSCOPE_LOW_PASS_FILTER, + /** + * The setting of IIC address + *

+ * range: [0,65535], default: 0 + *

+ */ + IIC_ADDRESS_SETTING, + /** Zero drift calibration */ ZERO_DRIFT_CALIBRATION, /** Erase chip */ diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc index 8d1939e..1cadb96 100644 --- a/src/mynteye/device/channel/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -80,6 +80,9 @@ int XuCamCtrlId(Option option) { case Option::GYROSCOPE_LOW_PASS_FILTER: return 12; break; + case Option::IIC_ADDRESS_SETTING: + return 13; + break; default: LOG(FATAL) << "No cam ctrl id for " << option; } @@ -148,7 +151,8 @@ void Channels::UpdateControlInfos() { Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL, Option::HDR_MODE, Option::ACCELEROMETER_RANGE, Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER, - Option::GYROSCOPE_LOW_PASS_FILTER}) { + Option::GYROSCOPE_LOW_PASS_FILTER, + Option::IIC_ADDRESS_SETTING}) { if (supports.find(option) != supports.end()) control_infos_[option] = XuControlInfo(option); } @@ -196,6 +200,7 @@ std::int32_t Channels::GetControlValue(const Option &option) const { case Option::GYROSCOPE_RANGE: case Option::ACCELEROMETER_LOW_PASS_FILTER: case Option::GYROSCOPE_LOW_PASS_FILTER: + case Option::IIC_ADDRESS_SETTING: return XuCamCtrlGet(option); case Option::ZERO_DRIFT_CALIBRATION: case Option::ERASE_CHIP: @@ -277,7 +282,8 @@ void Channels::SetControlValue(const Option &option, std::int32_t value) { case Option::DESIRED_BRIGHTNESS: case Option::IR_CONTROL: case Option::HDR_MODE: - case Option::MIN_EXPOSURE_TIME: { + case Option::MIN_EXPOSURE_TIME: + case Option::IIC_ADDRESS_SETTING: { if (!in_range()) break; XuCamCtrlSet(option, value); @@ -313,6 +319,7 @@ bool Channels::RunControlAction(const Option &option) const { case Option::GYROSCOPE_RANGE: case Option::ACCELEROMETER_LOW_PASS_FILTER: case Option::GYROSCOPE_LOW_PASS_FILTER: + case Option::IIC_ADDRESS_SETTING: LOG(WARNING) << option << " run action useless"; return false; default: diff --git a/src/mynteye/device/config.cc b/src/mynteye/device/config.cc index 8e52978..c9c21db 100644 --- a/src/mynteye/device/config.cc +++ b/src/mynteye/device/config.cc @@ -53,7 +53,7 @@ const std::map option_supports_map = { 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::IIC_ADDRESS_SETTING, Option::ERASE_CHIP} } }; diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index 77d4eba..8b6ebcb 100644 --- a/src/mynteye/types.cc +++ b/src/mynteye/types.cc @@ -122,6 +122,7 @@ const char *to_string(const Option &value) { CASE(GYROSCOPE_RANGE) CASE(ACCELEROMETER_LOW_PASS_FILTER) CASE(GYROSCOPE_LOW_PASS_FILTER) + CASE(IIC_ADDRESS_SETTING) CASE(ZERO_DRIFT_CALIBRATION) CASE(ERASE_CHIP) default: diff --git a/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp b/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp index 6363de9..413ac8a 100644 --- a/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp +++ b/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/cpp/option.hpp @@ -95,6 +95,11 @@ enum class Option : int { * values: {23,64}, default: 64 */ GYROSCOPE_LOW_PASS_FILTER, + /** + * The setting of IIC address + * range: [0,65535], default: 0 + */ + IIC_ADDRESS_SETTING, /** Zero drift calibration */ ZERO_DRIFT_CALIBRATION, /** Erase chip */ diff --git a/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/impl/type_conversion.hpp b/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/impl/type_conversion.hpp index 16ff6a9..631546a 100644 --- a/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/impl/type_conversion.hpp +++ b/wrappers/android/mynteye/libmynteye/src/main/cpp/mynteye/impl/type_conversion.hpp @@ -277,6 +277,8 @@ RawOption from_jni(const JniOption& Option) { return RawOption::ACCELEROMETER_LOW_PASS_FILTER; case JniOption::GYROSCOPE_LOW_PASS_FILTER: return RawOption::GYROSCOPE_LOW_PASS_FILTER; + case JniOption::IIC_ADDRESS_SETTING: + return RawOption::IIC_ADDRESS_SETTING; case JniOption::ZERO_DRIFT_CALIBRATION: return RawOption::ZERO_DRIFT_CALIBRATION; case JniOption::ERASE_CHIP: @@ -321,6 +323,8 @@ JniOption to_jni(const RawOption& Option) { return JniOption::ACCELEROMETER_LOW_PASS_FILTER; case RawOption::GYROSCOPE_LOW_PASS_FILTER: return JniOption::GYROSCOPE_LOW_PASS_FILTER; + case JniOption::IIC_ADDRESS_SETTING: + return RawOption::IIC_ADDRESS_SETTING; case RawOption::ZERO_DRIFT_CALIBRATION: return JniOption::ZERO_DRIFT_CALIBRATION; case RawOption::ERASE_CHIP: diff --git a/wrappers/android/mynteye/libmynteye/src/main/java/com/slightech/mynteye/Option.java b/wrappers/android/mynteye/libmynteye/src/main/java/com/slightech/mynteye/Option.java index 121f421..b3333d8 100644 --- a/wrappers/android/mynteye/libmynteye/src/main/java/com/slightech/mynteye/Option.java +++ b/wrappers/android/mynteye/libmynteye/src/main/java/com/slightech/mynteye/Option.java @@ -95,6 +95,11 @@ public enum Option { * values: {23,64}, default: 64 */ GYROSCOPE_LOW_PASS_FILTER, + /** + * The setting of IIC address + * range: [0,65535], default: 0 + */ + IIC_ADDRESS_SETTING, /** Zero drift calibration */ ZERO_DRIFT_CALIBRATION, /** Erase chip */ diff --git a/wrappers/android/mynteye/scripts/mynteye_types.djinni b/wrappers/android/mynteye/scripts/mynteye_types.djinni index e6681eb..ed30395 100644 --- a/wrappers/android/mynteye/scripts/mynteye_types.djinni +++ b/wrappers/android/mynteye/scripts/mynteye_types.djinni @@ -178,6 +178,10 @@ option = enum { # values: {23,64}, default: 64 gyroscope_low_pass_filter; + iic_address_setting; + # The setting of IIC address + # range: [0,65535], default: 0 + # Zero drift calibration zero_drift_calibration; # Erase chip diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml index c633900..dc0f4e8 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml @@ -105,4 +105,8 @@ standard210a/accel_low_filter: -1 # standard210a/gyro_low_filter range: [23,64] standard210a/gyro_low_filter: -1 -# standard210a/gyro_low_filter: 64 \ No newline at end of file +# standard210a/gyro_low_filter: 64 + +# standard210a/iic_address_setting range: [0,65535] +standard210a/iic_address_setting: -1 +# standard210a/iic_address_setting: 0 \ No newline at end of file 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 30b4ac4..bf378c2 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 @@ -250,7 +250,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { {Option::GYROSCOPE_RANGE, "standard210a/gyro_range"}, {Option::ACCELEROMETER_LOW_PASS_FILTER, "standard210a/accel_low_filter"}, - {Option::GYROSCOPE_LOW_PASS_FILTER, "standard210a/gyro_low_filter"}}; + {Option::GYROSCOPE_LOW_PASS_FILTER, "standard210a/gyro_low_filter"} + {Option::IIC_ADDRESS_SETTING, "standard2/iic_address_setting"}}; } // device options of standard2