Add ros wrapper

wrappers/README.md

@@ -12,4 +12,13 @@ make ros
 ### Run
 
 ```bash
+source wrappers/ros/devel/setup.bash
+roslaunch mynt_eye_ros_wrapper mynteye.launch
+```
+
+With RViz to preview,
+
+```bash
+source wrappers/ros/devel/setup.bash
+roslaunch mynt_eye_ros_wrapper display.launch
 ```

wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt

@@ -1,10 +1,15 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(mynt_eye_ros_wrapper)
 
+
+get_filename_component(SDK_DIR "${PROJECT_SOURCE_DIR}/../../../.." ABSOLUTE)
+
 if(NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE Release)
 endif()
 
+# packages
+
 function(checkPackage package customMessage)
   set(varName "${package}_FOUND")
   if (NOT "${${varName}}")
@@ -19,34 +24,71 @@ endfunction(checkPackage)
 
 find_package(catkin REQUIRED COMPONENTS
   cv_bridge
+  geometry_msgs
   image_transport
   nodelet
   roscpp
   sensor_msgs
   std_msgs
+  tf
 )
 
 checkPackage("cv_bridge" "")
+checkPackage("geometry_msgs" "")
 checkPackage("image_transport" "")
 checkPackage("nodelet" "")
 checkPackage("roscpp" "")
 checkPackage("sensor_msgs" "")
 checkPackage("std_msgs" "")
+checkPackage("tf" "")
 
 catkin_package(
-  CATKIN_DEPENDS cv_bridge image_transport nodelet roscpp sensor_msgs std_msgs
+  CATKIN_DEPENDS cv_bridge geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs tf
 )
 
-# Build
+get_filename_component(SDK_DIR "${PROJECT_SOURCE_DIR}/../../../.." ABSOLUTE)
+
+LIST(APPEND CMAKE_PREFIX_PATH ${SDK_DIR}/third_party/glog/_build)
+find_package(glog REQUIRED)
+message(STATUS "Found glog: ${glog_VERSION}")
+
+LIST(APPEND CMAKE_PREFIX_PATH ${SDK_DIR}/_install/lib/cmake)
+find_package(mynteye REQUIRED)
+message(STATUS "Found mynteye: ${mynteye_VERSION}")
+
+# targets
 
 add_compile_options(-std=c++11)
 
 include_directories(
-# include
   ${catkin_INCLUDE_DIRS}
+  ${SDK_DIR}/src  # for device layer interface
 )
 
-# Install
+set(LINK_LIBS
+  ${catkin_LIBRARIES}
+  mynteye
+)
+
+add_library(mynteye_wrapper src/wrapper_nodelet.cc)
+target_link_libraries(mynteye_wrapper ${LINK_LIBS})
+
+add_executable(mynteye_wrapper_node src/wrapper_node.cc)
+target_link_libraries(mynteye_wrapper_node mynteye_wrapper ${LINK_LIBS})
+
+if(MSVC OR MSYS OR MINGW)
+  target_compile_definitions(mynteye_wrapper_node
+    PUBLIC GLOG_NO_ABBREVIATED_SEVERITIES
+  )
+endif()
+
+# install
+
+install(TARGETS mynteye_wrapper mynteye_wrapper_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 install(DIRECTORY launch
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
@@ -55,3 +97,7 @@ install(DIRECTORY launch
 install(DIRECTORY rviz
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
+
+install(FILES nodelet_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

wrappers/ros/src/mynt_eye_ros_wrapper/launch/display.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+
+  <include file="$(find mynt_eye_ros_wrapper)/launch/mynteye.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mynt_eye_ros_wrapper)/rviz/mynteye.rviz" output="screen" />
+
+</launch>

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -0,0 +1,96 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mynteye" default="mynteye" />
+
+  <!-- node params -->
+
+  <arg name="left_topic" default="left" />
+  <arg name="right_topic" default="right" />
+  <arg name="imu_topic" default="imu" />
+
+  <arg name="base_frame_id" default="$(arg mynteye)_link" />
+  <arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
+  <arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
+  <arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
+
+  <!-- device options, -1 will not set the value -->
+
+  <!-- gain range: [0,48]-->
+  <arg name="gain" default="-1" />
+  <!-- <arg name="gain" default="24" /> -->
+
+  <!-- brightness range: [0,240]-->
+  <arg name="brightness" default="-1" />
+  <!-- <arg name="brightness" default="120" /> -->
+
+  <!-- contrast range: [0,255]-->
+  <arg name="contrast" default="-1" />
+  <!-- <arg name="contrast" default="127" /> -->
+
+  <!-- frame_rate range: [10,60]-->
+  <arg name="frame_rate" default="-1" />
+  <!-- <arg name="frame_rate" default="25" /> -->
+
+  <!-- imu_frequency range: [200,500]-->
+  <arg name="imu_frequency" default="-1" />
+  <!-- <arg name="imu_frequency" default="200" /> -->
+
+  <!-- exposure_mode, 0: auto-exposure, 1: manual-exposure -->
+  <arg name="exposure_mode" default="-1" />
+  <!-- <arg name="exposure_mode" default="0" /> -->
+
+  <!-- max_gain range: [0,48]-->
+  <arg name="max_gain" default="-1" />
+  <!-- <arg name="max_gain" default="48" /> -->
+
+  <!-- max_exposure_time range: [0,240]-->
+  <arg name="max_exposure_time" default="-1" />
+  <!-- <arg name="max_exposure_time" default="240" /> -->
+
+  <!-- desired_brightness range: [0,255]-->
+  <arg name="desired_brightness" default="-1" />
+  <!-- <arg name="desired_brightness" default="192" /> -->
+
+  <!-- ir_control range: [0,160]-->
+  <arg name="ir_control" default="-1" />
+  <!-- <arg name="ir_control" default="0" /> -->
+
+  <!-- hdr_mode, 0: 10-bit, 1: 12-bit -->
+  <arg name="hdr_mode" default="-1" />
+  <!-- <arg name="hdr_mode" default="0" /> -->
+
+  <!-- Push down all topics/nodelets into "mynteye" namespace -->
+  <group ns="$(arg mynteye)">
+
+    <!-- mynteye_wrapper_node -->
+    <node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen">
+
+      <!-- node params -->
+
+      <param name="left_topic" value="$(arg left_topic)" />
+      <param name="right_topic" value="$(arg right_topic)" />
+      <param name="imu_topic" value="$(arg imu_topic)" />
+
+      <param name="base_frame_id" value="$(arg base_frame_id)" />
+      <param name="left_frame_id" value="$(arg left_frame_id)" />
+      <param name="right_frame_id" value="$(arg right_frame_id)" />
+      <param name="imu_frame_id" value="$(arg imu_frame_id)" />
+
+      <!-- device options -->
+
+      <param name="gain" value="$(arg gain)" />
+      <param name="brightness" value="$(arg brightness)" />
+      <param name="contrast" value="$(arg contrast)" />
+      <param name="frame_rate" value="$(arg frame_rate)" />
+      <param name="imu_frequency" value="$(arg imu_frequency)" />
+      <param name="exposure_mode" value="$(arg exposure_mode)" />
+      <param name="max_gain" value="$(arg max_gain)" />
+      <param name="max_exposure_time" value="$(arg max_exposure_time)" />
+      <param name="desired_brightness" value="$(arg desired_brightness)" />
+      <param name="ir_control" value="$(arg ir_control)" />
+      <param name="hdr_mode" value="$(arg hdr_mode)" />
+
+    </node>
+
+  </group> <!-- mynteye -->
+</launch>

wrappers/ros/src/mynt_eye_ros_wrapper/nodelet_plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libmynteye_wrapper">
+  <class name="mynteye/ROSWrapperNodelet"
+         type="mynteye::ROSWrapperNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>This is the nodelet of ROS interface for MYNT EYE camera.</description>
+  </class>
+</library>

wrappers/ros/src/mynt_eye_ros_wrapper/package.xml

@@ -14,28 +14,35 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>cv_bridge</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>tf</build_depend>
 
   <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>image_transport</build_export_depend>
   <build_export_depend>nodelet</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
 
   <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>image_transport</exec_depend>
   <exec_depend>nodelet</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
   </export>
 </package>

wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz

@@ -0,0 +1,150 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 853
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Left
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_plugin_tutorials/Imu
+      Color: 204; 51; 204
+      Enabled: false
+      History Length: 1
+      Name: Imu
+      Topic: /mynteye/imu
+      Unreliable: false
+      Value: false
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /mynteye/left
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Left
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /mynteye/right
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Right
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: mynteye_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 14.7952175
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.785398006
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Left:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bb0000011e0000024f00000191fb0000000a00520069006700680074030000040c0000011e0000025000000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Right:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_node.cc

@@ -0,0 +1,22 @@
+#include <nodelet/loader.h>
+#include <ros/ros.h>
+
+#include "mynteye/glog_init.h"
+
+int main(int argc, char *argv[]) {
+  glog_init _(argc, argv);
+
+  ros::init(argc, argv, "mynteye_wrapper_node");
+  ros::console::set_logger_level(
+      ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+
+  nodelet::Loader nodelet;
+  nodelet::M_string remap(ros::names::getRemappings());
+  nodelet::V_string nargv;
+  nodelet.load(
+      ros::this_node::getName(), "mynteye/ROSWrapperNodelet", remap, nargv);
+
+  ros::spin();
+
+  return 0;
+}

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -0,0 +1,354 @@
+#include <nodelet/nodelet.h>  // NOLINT
+#include <ros/ros.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/image_encodings.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
+#include <glog/logging.h>
+
+#include <map>
+#include <string>
+
+#include "device/context.h"
+#include "device/device.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace enc = sensor_msgs::image_encodings;
+
+class ROSWrapperNodelet : public nodelet::Nodelet {
+ public:
+  ROSWrapperNodelet() {}
+
+  ~ROSWrapperNodelet() {
+    // std::cout << __func__ << std::endl;
+    if (device_) {
+      device_->Stop(Source::ALL);
+    }
+    if (time_beg_ != -1) {
+      double time_end = ros::Time::now().toSec();
+      double time_elapsed = time_end - time_beg_;
+
+      LOG(INFO) << "Time elapsed: " << time_elapsed << "s";
+      LOG(INFO) << "Left count: " << left_count_
+                << ", fps: " << (left_count_ / time_elapsed);
+      LOG(INFO) << "Right count: " << right_count_
+                << ", fps: " << (right_count_ / time_elapsed);
+      LOG(INFO) << "Imu count: " << imu_count_
+                << ", hz: " << (imu_count_ / time_elapsed);
+
+      // ROS messages could not be reliably printed here, using glog instead :(
+      // ros::Duration(1).sleep();  // 1s
+      // https://answers.ros.org/question/35163/how-to-perform-an-action-at-nodelet-unload-shutdown/
+    }
+  }
+
+  void onInit() override {
+    initDevice();
+    NODELET_FATAL_COND(device_ == nullptr, "No MYNT EYE device selected :(");
+
+    nh_ = getMTNodeHandle();
+    private_nh_ = getMTPrivateNodeHandle();
+
+    // node params
+
+    std::string left_topic = "left";
+    std::string right_topic = "right";
+    std::string imu_topic = "imu";
+    private_nh_.getParam("left_topic", left_topic);
+    private_nh_.getParam("right_topic", right_topic);
+    private_nh_.getParam("imu_topic", imu_topic);
+
+    base_frame_id_ = "camera_link";
+    left_frame_id_ = "camera_left_frame";
+    right_frame_id_ = "camera_right_frame";
+    imu_frame_id_ = "camera_imu_frame";
+    private_nh_.getParam("base_frame_id", base_frame_id_);
+    private_nh_.getParam("left_frame_id", left_frame_id_);
+    private_nh_.getParam("right_frame_id", right_frame_id_);
+    private_nh_.getParam("imu_frame_id", imu_frame_id_);
+
+    // device options
+
+    std::map<std::string, Option> option_map = {
+        {"gain", Option::GAIN},
+        {"brightness", Option::BRIGHTNESS},
+        {"contrast", Option::CONTRAST},
+        {"frame_rate", Option::FRAME_RATE},
+        {"imu_frequency", Option::IMU_FREQUENCY},
+        {"exposure_mode", Option::EXPOSURE_MODE},
+        {"max_gain", Option::MAX_GAIN},
+        {"max_exposure_time", Option::MAX_EXPOSURE_TIME},
+        {"desired_brightness", Option::DESIRED_BRIGHTNESS},
+        {"ir_control", Option::IR_CONTROL},
+        {"hdr_mode", Option::HDR_MODE},
+    };
+    int value;
+    for (auto &&it = option_map.begin(); it != option_map.end(); it++) {
+      value = -1;
+      private_nh_.getParam(it->first, value);
+      if (value != -1) {
+        NODELET_INFO_STREAM("Set " << it->first << " to " << value);
+        device_->SetOptionValue(it->second, value);
+      }
+      NODELET_INFO_STREAM(
+          it->second << ": " << device_->GetOptionValue(it->second));
+    }
+
+    // image publishers
+
+    image_transport::ImageTransport it_mynteye(nh_);
+
+    pub_left_ = it_mynteye.advertise(left_topic, 1);
+    NODELET_INFO_STREAM("Advertized on topic " << left_topic);
+    pub_right_ = it_mynteye.advertise(right_topic, 1);
+    NODELET_INFO_STREAM("Advertized on topic " << right_topic);
+
+    // imu publisher
+
+    pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 1);
+    NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
+
+    publishStaticTransforms();
+    publishTopics();
+  }
+
+  void publishStaticTransforms() {
+    ros::Time tf_stamp = ros::Time::now();
+
+    // The left frame is used as the base frame.
+    geometry_msgs::TransformStamped b2l_msg;
+    b2l_msg.header.stamp = tf_stamp;
+    b2l_msg.header.frame_id = base_frame_id_;
+    b2l_msg.child_frame_id = left_frame_id_;
+    b2l_msg.transform.translation.x = 0;
+    b2l_msg.transform.translation.y = 0;
+    b2l_msg.transform.translation.z = 0;
+    b2l_msg.transform.rotation.x = 0;
+    b2l_msg.transform.rotation.y = 0;
+    b2l_msg.transform.rotation.z = 0;
+    b2l_msg.transform.rotation.w = 1;
+    static_tf_broadcaster_.sendTransform(b2l_msg);
+
+    // Transform base frame to right frame
+    geometry_msgs::TransformStamped b2r_msg;
+    b2r_msg.header.stamp = tf_stamp;
+    b2r_msg.header.frame_id = base_frame_id_;
+    b2r_msg.child_frame_id = right_frame_id_;
+    b2r_msg.transform.translation.x = 0;
+    b2r_msg.transform.translation.y = 0;
+    b2r_msg.transform.translation.z = 0;
+    b2r_msg.transform.rotation.x = 0;
+    b2r_msg.transform.rotation.y = 0;
+    b2r_msg.transform.rotation.z = 0;
+    b2r_msg.transform.rotation.w = 1;
+    static_tf_broadcaster_.sendTransform(b2r_msg);
+
+    // Transform base frame to imu frame
+    geometry_msgs::TransformStamped b2i_msg;
+    b2i_msg.header.stamp = tf_stamp;
+    b2i_msg.header.frame_id = base_frame_id_;
+    b2i_msg.child_frame_id = imu_frame_id_;
+    b2i_msg.transform.translation.x = 0;
+    b2i_msg.transform.translation.y = 0;
+    b2i_msg.transform.translation.z = 0;
+    b2i_msg.transform.rotation.x = 0;
+    b2i_msg.transform.rotation.y = 0;
+    b2i_msg.transform.rotation.z = 0;
+    b2i_msg.transform.rotation.w = 1;
+    static_tf_broadcaster_.sendTransform(b2i_msg);
+  }
+
+  void publishTopics() {
+    device_->SetStreamCallback(
+        Stream::LEFT, [this](const device::StreamData &data) {
+          ++left_count_;
+          publishLeft(data, left_count_, ros::Time::now());
+          NODELET_DEBUG_STREAM(
+              Stream::LEFT << ", count: " << left_count_
+                           << ", frame_id: " << data.img->frame_id
+                           << ", timestamp: " << data.img->timestamp
+                           << ", exposure_time: " << data.img->exposure_time);
+        });
+
+    device_->SetStreamCallback(
+        Stream::RIGHT, [this](const device::StreamData &data) {
+          ++right_count_;
+          publishRight(data, right_count_, ros::Time::now());
+          NODELET_DEBUG_STREAM(
+              Stream::RIGHT << ", count: " << right_count_
+                            << ", frame_id: " << data.img->frame_id
+                            << ", timestamp: " << data.img->timestamp
+                            << ", exposure_time: " << data.img->exposure_time);
+        });
+
+    device_->SetMotionCallback([this](const device::MotionData &data) {
+      static std::uint32_t imu_time_beg = -1;
+      static double imu_ros_time_beg;
+      if (imu_time_beg == -1) {
+        imu_time_beg = data.imu->timestamp;
+        imu_ros_time_beg = ros::Time::now().toSec();
+      }
+      // 0.01 ms > 1 s
+      ros::Time stamp(
+          imu_ros_time_beg + (data.imu->timestamp - imu_time_beg) * 0.00001f);
+
+      ++imu_count_;
+      publishIMU(data, imu_count_, stamp);
+      NODELET_DEBUG_STREAM(
+          "Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id
+                        << ", timestamp: " << data.imu->timestamp
+                        << ", accel_x: " << data.imu->accel[0]
+                        << ", accel_y: " << data.imu->accel[1]
+                        << ", accel_z: " << data.imu->accel[2]
+                        << ", gyro_x: " << data.imu->gyro[0]
+                        << ", gyro_y: " << data.imu->gyro[1]
+                        << ", gyro_z: " << data.imu->gyro[2]
+                        << ", temperature: " << data.imu->temperature);
+    });
+
+    time_beg_ = ros::Time::now().toSec();
+    device_->Start(Source::ALL);
+  }
+
+  void publishLeft(
+      const device::StreamData &data, std::uint32_t seq, ros::Time stamp) {
+    std_msgs::Header header;
+    header.seq = seq;
+    header.stamp = stamp;
+    header.frame_id = left_frame_id_;
+    cv::Mat img(
+        data.frame->height(), data.frame->width(), CV_8UC1, data.frame->data());
+    pub_left_.publish(cv_bridge::CvImage(header, enc::MONO8, img).toImageMsg());
+  }
+
+  void publishRight(
+      const device::StreamData &data, std::uint32_t seq, ros::Time stamp) {
+    std_msgs::Header header;
+    header.seq = seq;
+    header.stamp = stamp;
+    header.frame_id = right_frame_id_;
+    cv::Mat img(
+        data.frame->height(), data.frame->width(), CV_8UC1, data.frame->data());
+    pub_right_.publish(
+        cv_bridge::CvImage(header, enc::MONO8, img).toImageMsg());
+  }
+
+  void publishIMU(
+      const device::MotionData &data, std::uint32_t seq, ros::Time stamp) {
+    sensor_msgs::Imu msg;
+
+    msg.header.seq = seq;
+    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 = data.imu->accel[0] * 9.8;
+    msg.linear_acceleration.y = data.imu->accel[1] * 9.8;
+    msg.linear_acceleration.z = data.imu->accel[2] * 9.8;
+
+    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 = data.imu->gyro[0] / 57.2956;
+    msg.angular_velocity.y = data.imu->gyro[1] / 57.2956;
+    msg.angular_velocity.z = data.imu->gyro[2] / 57.2956;
+
+    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);
+  }
+
+ 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> 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_ = device;
+  }
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  image_transport::Publisher pub_left_;
+  image_transport::Publisher pub_right_;
+
+  ros::Publisher pub_imu_;
+
+  tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
+
+  // node params
+
+  std::string base_frame_id_;
+  std::string left_frame_id_;
+  std::string right_frame_id_;
+  std::string imu_frame_id_;
+
+  // device
+
+  std::shared_ptr<Device> device_;
+
+  double time_beg_ = -1;
+  std::size_t left_count_ = 0;
+  std::size_t right_count_ = 0;
+  std::size_t imu_count_ = 0;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#include <pluginlib/class_list_macros.h>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(mynteye::ROSWrapperNodelet, nodelet::Nodelet);