/wrappers/ros/
+ ├─src/
+ │ └─mynt_eye_ros_wrapper/
+ │ ├─launch/
+ │ │ ├─display.launch
+ │ │ └─mynteye.launch
+ │ ├─msg/
+ │ ├─rviz/
+ │ ├─src/
+ │ │ ├─wrapper_node.cc
+ │ │ └─wrapper_nodelet.cc
+ │ ├─CMakeLists.txt
+ │ ├─nodelet_plugins.xml
+ │ └─package.xml
+ └─README.md
+
+In ``mynteye.launch``, you can configure the topics and frame_ids, decide which data to enable, and set the control options. Please set ``gravity`` to the local gravity acceleration.
+
+.. code-block:: xml
+
+ # s2100/s210a modify frame/resolution
+
+
+ # s1030 modify frame/imu hz
+
+
+
+
+
+
+
+ ...
+
+ # s2100 modify brightness
+
+
+
+ ...
+
+ # s210a modify brightness
+
+
+
+ ...
+
+.. code-block:: xml
+
+
+
+For printing debug info, replace ``Info`` in ``wrapper_node.cc`` to ``Debug`` :
+
+.. code-block:: c++
+
+ ros::console::set_logger_level(
+ ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
diff --git a/include/mynteye/types.h b/include/mynteye/types.h
index 348d37d..26ea5dd 100644
--- a/include/mynteye/types.h
+++ b/include/mynteye/types.h
@@ -137,109 +137,125 @@ enum class Info : std::uint8_t {
enum class Option : std::uint8_t {
/**
* Image gain, valid if manual-exposure
- *
+ *
* range: [0,48], default: 24
+ *
*/
GAIN,
/**
* Image brightness, valid if manual-exposure
- *
+ *
* range: [0,240], default: 120
+ *
*/
BRIGHTNESS,
/**
* Image contrast, valid if manual-exposure
- *
+ *
* range: [0,255], default: 127
+ *
*/
CONTRAST,
/**
* Image frame rate, must set IMU_FREQUENCY together
- *
+ *
* values: {10,15,20,25,30,35,40,45,50,55,60}, default: 25
+ *
*/
FRAME_RATE,
/**
* IMU frequency, must set FRAME_RATE together
- *
+ *
* values: {100,200,250,333,500}, default: 200
+ *
*/
IMU_FREQUENCY,
/**
* Exposure mode
- *
- * 0: enable auto-exposure
+ *
+ * 0: enable auto-exposure
* 1: disable auto-exposure (manual-exposure)
+ *
*/
EXPOSURE_MODE,
/**
* Max gain, valid if auto-exposure
- *
- * range of standard 1: [0,48], default: 48
+ *
+ * range of standard 1: [0,48], default: 48
* range of standard 2: [0,255], default: 8
+ *
*/
MAX_GAIN,
/**
* Max exposure time, valid if auto-exposure
- *
- * range of standard 1: [0,240], default: 240
+ *
+ * range of standard 1: [0,240], default: 240
* range of standard 2: [0,1000], default: 333
+ *
*/
MAX_EXPOSURE_TIME,
/**
* min exposure time, valid if auto-exposure
- *
- * range: [0,1000], default: 0
+ *
+ * range: [0,1000], default: 0
+ *
*/
MIN_EXPOSURE_TIME,
/**
* Desired brightness, valid if auto-exposure
- *
- * range of standard 1: [0,255], default: 192
+ *
+ * range of standard 1: [0,255], default: 192
* range of standard 2: [1,255], default: 122
+ *
*/
DESIRED_BRIGHTNESS,
/**
* IR control
- *
- * range: [0,160], default: 0
+ *
+ * range: [0,160], default: 0
+ *
*/
IR_CONTROL,
/**
* HDR mode
- *
- * 0: 10-bit
+ *
+ * 0: 10-bit
* 1: 12-bit
+ *
*/
HDR_MODE,
/**
* The range of accelerometer
- *
- * value of standard 1: {4,8,16,32}, default: 8
+ *
+ * value of standard 1: {4,8,16,32}, default: 8
* value of standard 2: {6,12,24,48}, default: 12
+ *
*/
ACCELEROMETER_RANGE,
/**
* The range of gyroscope
- *
- * value of standard 1: {500,1000,2000,4000}, default: 1000
+ *
+ * value of standard 1: {500,1000,2000,4000}, default: 1000
* value of standard 2: {250,500,1000,2000,4000}, default: 1000
+ *
*/
GYROSCOPE_RANGE,
/**
* The parameter of accelerometer low pass filter
- *
+ *
* values: {0,1,2}, default: 2
+ *
*/
ACCELEROMETER_LOW_PASS_FILTER,
/**
* The parameter of gyroscope low pass filter
- *
+ *
* values: {23,64}, default: 64
+ *
*/
GYROSCOPE_LOW_PASS_FILTER,
@@ -664,10 +680,11 @@ struct MYNTEYE_API ImuData {
std::uint32_t frame_id;
/**
* IMU accel or gyro flag
- *
- * 0: accel and gyro are both valid
- * 1: accel is valid
+ *
+ * 0: accel and gyro are both valid
+ * 1: accel is valid
* 2: gyro is valid
+ *
*/
std::uint8_t flag;
/** IMU timestamp in 1us */
diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h
index a903854..ec8b2b9 100644
--- a/src/mynteye/device/channel/def.h
+++ b/src/mynteye/device/channel/def.h
@@ -72,6 +72,8 @@ struct ImuSegment {
std::uint32_t frame_id;
std::uint64_t timestamp;
std::uint8_t flag;
+ // Is external time source
+ bool is_ets;
std::int16_t temperature;
std::int16_t accel[3];
std::int16_t gyro[3];
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc
index 13efb89..0c0157b 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.cc
+++ b/src/mynteye/device/standard2/channels_adapter_s2.cc
@@ -55,7 +55,8 @@ struct ImuData {
void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
seg->frame_id = imu.frame_id;
seg->timestamp = imu.timestamp;
- seg->flag = imu.flag;
+ seg->flag = imu.flag & 0b0011;
+ seg->is_ets = (imu.flag & 0b0100 == 0b0100);
seg->temperature = imu.temperature;
seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0;
seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0;
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 d4eaafb..c633900 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
@@ -50,7 +50,7 @@ standard2/ir_control: 80
# standard2/ir_control: 0
# standard2/accel_range range: [6,48]
-standard2/accel_range: 24
+standard2/accel_range: -1
# standard2/accel_range: 6
# standard2/gyro_range range: [250,4000]
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 5ecc718..79615a5 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -2,6 +2,13 @@
+
+
+
+
+
+
+
@@ -42,6 +49,9 @@
+
+
+
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
new file mode 100644
index 0000000..0782cf5
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch
@@ -0,0 +1,7 @@
+
+
+
+
+
+
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch
new file mode 100644
index 0000000..3885271
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch
@@ -0,0 +1,377 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch
new file mode 100644
index 0000000..66215a0
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch
@@ -0,0 +1,377 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
+ - 'image_transport/compressedDepth'
+
+
+
+
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 4e4c668..40fbd53 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
@@ -1034,39 +1034,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
private:
void initDevice() {
- NODELET_INFO_STREAM("Detecting MYNT EYE devices");
-
- 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);
- NODELET_INFO_STREAM(" index: " << i << ", name: " << name);
- }
-
std::shared_ptr device = nullptr;
- if (n <= 1) {
- device = devices[0];
- NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 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;
- }
- device = devices[i];
- break;
- }
- }
+
+ device = selectDevice();
api_ = API::Create(device);
auto &&requests = device->GetStreamRequests();
@@ -1122,6 +1092,82 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
computeRectTransforms();
}
+ std::shared_ptr selectDevice() {
+ NODELET_INFO_STREAM("Detecting MYNT EYE devices");
+
+ Context context;
+ auto &&devices = context.devices();
+
+ bool is_multiple = false;
+ private_nh_.getParam("is_multiple", is_multiple);
+ if (is_multiple) {
+ std::string sn;
+ private_nh_.getParam("serial_number", sn);
+ 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
+ }
+ }
+ 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;
+ }
+ return devices[i];
+ }
+ }
+
+ return nullptr;
+ }
+
std::shared_ptr getDefaultIntrinsics() {
auto res = std::make_shared();
res->width = 640;