Support enable stream and publish them in ros

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

@@ -6,17 +6,40 @@
 
   <arg name="left_topic" default="left" />
   <arg name="right_topic" default="right" />
+  <arg name="left_rect_topic" default="left_rect" />
+  <arg name="right_rect_topic" default="right_rect" />
+  <arg name="disparity_topic" default="disparity" />
+  <arg name="disparity_norm_topic" default="disparity_norm" />
+  <arg name="points_topic" default="points" />
+  <arg name="depth_topic" default="depth" />
+
   <arg name="imu_topic" default="imu" />
   <arg name="temp_topic" default="temp" />
 
   <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="left_rect_frame_id" default="$(arg mynteye)_left_rect_frame" />
+  <arg name="right_rect_frame_id" default="$(arg mynteye)_right_rect_frame" />
+  <arg name="disparity_frame_id" default="$(arg mynteye)_disparity_frame" />
+  <arg name="disparity_norm_frame_id" default="$(arg mynteye)_disparity_norm_frame" />
+  <arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
+  <arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
+
   <arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
   <arg name="temp_frame_id" default="$(arg mynteye)_temp_frame" />
 
   <arg name="gravity" default="9.8" />
 
+  <!-- stream toggles -->
+
+  <arg name="enable_left_rect" default="false" />
+  <arg name="enable_right_rect" default="false" />
+  <arg name="enable_disparity" default="false" />
+  <arg name="enable_disparity_norm" default="false" />
+  <arg name="enable_points" default="false" />
+  <arg name="enable_depth" default="true" />
+
   <!-- device options, -1 will not set the value -->
 
   <!-- gain range: [0,48] -->
@@ -56,7 +79,7 @@
   <!-- <arg name="desired_brightness" default="192" /> -->
 
   <!-- ir_control range: [0,160] -->
-  <arg name="ir_control" default="-1" />
+  <arg name="ir_control" default="80" />
   <!-- <arg name="ir_control" default="0" /> -->
 
   <!-- hdr_mode, 0: 10-bit, 1: 12-bit -->
@@ -73,17 +96,40 @@
 
       <param name="left_topic" value="$(arg left_topic)" />
       <param name="right_topic" value="$(arg right_topic)" />
+      <param name="left_rect_topic" value="$(arg left_rect_topic)" />
+      <param name="right_rect_topic" value="$(arg right_rect_topic)" />
+      <param name="disparity_topic" value="$(arg disparity_topic)" />
+      <param name="disparity_norm_topic" value="$(arg disparity_norm_topic)" />
+      <param name="points_topic" value="$(arg points_topic)" />
+      <param name="depth_topic" value="$(arg depth_topic)" />
+
       <param name="imu_topic" value="$(arg imu_topic)" />
       <param name="temp_topic" value="$(arg temp_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="left_rect_frame_id" value="$(arg left_rect_frame_id)" />
+      <param name="right_rect_frame_id" value="$(arg right_rect_frame_id)" />
+      <param name="disparity_frame_id" value="$(arg disparity_frame_id)" />
+      <param name="disparity_norm_frame_id" value="$(arg disparity_norm_frame_id)" />
+      <param name="points_frame_id" value="$(arg points_frame_id)" />
+      <param name="depth_frame_id" value="$(arg depth_frame_id)" />
+
       <param name="imu_frame_id" value="$(arg imu_frame_id)" />
       <param name="temp_frame_id" value="$(arg temp_frame_id)" />
 
       <param name="gravity" value="$(arg gravity)" />
 
+      <!-- stream toggles -->
+
+      <param name="enable_left_rect" value="$(arg enable_left_rect)" />
+      <param name="enable_right_rect" value="$(arg enable_right_rect)" />
+      <param name="enable_disparity" value="$(arg enable_disparity)" />
+      <param name="enable_disparity_norm" value="$(arg enable_disparity_norm)" />
+      <param name="enable_points" value="$(arg enable_points)" />
+      <param name="enable_depth" value="$(arg enable_depth)" />
+
       <!-- device options -->
 
       <param name="gain" value="$(arg gain)" />

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

@@ -6,6 +6,9 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /Images1
+        - /Images1/Rectified1
+        - /Disparity1
       Splitter Ratio: 0.5
     Tree Height: 853
   - Class: rviz/Selection
@@ -48,6 +51,132 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
+    - Class: rviz/Group
+      Displays:
+        - 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
+        - Class: rviz/Group
+          Displays:
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /mynteye/left_rect
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: LeftRect
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: raw
+              Unreliable: false
+              Value: true
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /mynteye/right_rect
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: RightRect
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: raw
+              Unreliable: false
+              Value: true
+          Enabled: false
+          Name: Rectified
+      Enabled: true
+      Name: Images
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Image
+          Enabled: true
+          Image Topic: /mynteye/disparity
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Disparity
+          Normalize Range: true
+          Queue Size: 2
+          Transport Hint: raw
+          Unreliable: false
+          Value: true
+        - Class: rviz/Image
+          Enabled: true
+          Image Topic: /mynteye/disparity_norm
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: DisparityNorm
+          Normalize Range: true
+          Queue Size: 2
+          Transport Hint: raw
+          Unreliable: false
+          Value: true
+      Enabled: false
+      Name: Disparity
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /mynteye/depth
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Depth
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Points
+      Position Transformer: ""
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /mynteye/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
     - Alpha: 1
       Class: rviz_plugin_tutorials/Imu
       Color: 204; 51; 204
@@ -57,30 +186,6 @@ Visualization Manager:
       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
@@ -127,6 +232,12 @@ Visualization Manager:
       Yaw: 0.785398006
     Saved: ~
 Window Geometry:
+  Depth:
+    collapsed: false
+  Disparity:
+    collapsed: false
+  DisparityNorm:
+    collapsed: false
   Displays:
     collapsed: false
   Height: 1056
@@ -134,9 +245,13 @@ Window Geometry:
   Hide Right Dock: false
   Left:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bb0000011e0000024f00000191fb0000000a00520069006700680074030000040c0000011e0000025000000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  LeftRect:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bd000000860000024e00000190fb0000000a00520069006700680074030000040c000000860000025000000190fb00000010004c006500660074005200650063007402000001bd000002170000024e00000190fb00000012005200690067006800740052006500630074020000040c000002170000024f00000190fb0000000a0044006500700074006803000001bc000002170000024f00000190fb0000001200440069007300700061007200690074007902000001bd000002170000024e0000018ffb0000001a004400690073007000610072006900740079004e006f0072006d020000040c000002170000025100000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Right:
     collapsed: false
+  RightRect:
+    collapsed: false
   Selection:
     collapsed: false
   Time:

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

@@ -4,7 +4,9 @@
 #include <cv_bridge/cv_bridge.h>
 #include <image_transport/image_transport.h>
 #include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/image_encodings.h>
+#include <tf/tf.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 
 #include <mynt_eye_ros_wrapper/Temp.h>
@@ -63,10 +65,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     // node params
 
-    std::string left_topic = "left";
-    std::string right_topic = "right";
-    private_nh_.getParam("left_topic", left_topic);
-    private_nh_.getParam("right_topic", right_topic);
+    std::map<Stream, std::string> stream_names{
+        {Stream::LEFT, "left"},
+        {Stream::RIGHT, "right"},
+        {Stream::LEFT_RECTIFIED, "left_rect"},
+        {Stream::RIGHT_RECTIFIED, "right_rect"},
+        {Stream::DISPARITY, "disparity"},
+        {Stream::DISPARITY_NORMALIZED, "disparity_norm"},
+        {Stream::DEPTH, "depth"},
+        {Stream::POINTS, "points"}};
+
+    std::map<Stream, std::string> stream_topics{};
+    for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
+      stream_topics[it->first] = it->second;
+      private_nh_.getParam(it->second + "_topic", stream_topics[it->first]);
+    }
 
     std::string imu_topic = "imu";
     std::string temp_topic = "temp";
@@ -76,10 +89,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     base_frame_id_ = "camera_link";
     private_nh_.getParam("base_frame_id", base_frame_id_);
 
-    camera_frame_ids_[Stream::LEFT] = "camera_left_frame";
-    camera_frame_ids_[Stream::RIGHT] = "camera_right_frame";
-    private_nh_.getParam("left_frame_id", camera_frame_ids_[Stream::LEFT]);
-    private_nh_.getParam("right_frame_id", camera_frame_ids_[Stream::RIGHT]);
+    for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
+      frame_ids_[it->first] = "camera_" + it->second + "_frame";
+      private_nh_.getParam(it->second + "_frame_id", frame_ids_[it->first]);
+    }
 
     imu_frame_id_ = "camera_imu_frame";
     temp_frame_id_ = "camera_temp_frame";
@@ -91,55 +104,79 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     // 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},
+    std::map<Option, std::string> option_names = {
+        {Option::GAIN, "gain"},
+        {Option::BRIGHTNESS, "brightness"},
+        {Option::CONTRAST, "contrast"},
+        {Option::FRAME_RATE, "frame_rate"},
+        {Option::IMU_FREQUENCY, "imu_frequency"},
+        {Option::EXPOSURE_MODE, "exposure_mode"},
+        {Option::MAX_GAIN, "max_gain"},
+        {Option::MAX_EXPOSURE_TIME, "max_exposure_time"},
+        {Option::DESIRED_BRIGHTNESS, "desired_brightness"},
+        {Option::IR_CONTROL, "ir_control"},
+        {Option::HDR_MODE, "hdr_mode"},
     };
-    int value;
-    for (auto &&it = option_map.begin(); it != option_map.end(); it++) {
-      value = -1;
-      private_nh_.getParam(it->first, value);
+    for (auto &&it = option_names.begin(); it != option_names.end(); ++it) {
+      if (!api_->Supports(it->first))
+        continue;
+      int value = -1;
+      private_nh_.getParam(it->second, value);
       if (value != -1) {
-        NODELET_INFO_STREAM("Set " << it->first << " to " << value);
-        api_->SetOptionValue(it->second, value);
+        NODELET_INFO_STREAM("Set " << it->second << " to " << value);
+        api_->SetOptionValue(it->first, value);
       }
-      NODELET_INFO_STREAM(
-          it->second << ": " << api_->GetOptionValue(it->second));
+      NODELET_INFO_STREAM(it->first << ": " << api_->GetOptionValue(it->first));
     }
 
-    // image publishers
+    // publishers
 
     image_transport::ImageTransport it_mynteye(nh_);
 
-    camera_encodings_[Stream::LEFT] = enc::MONO8;
-    camera_publishers_[Stream::LEFT] =
-        it_mynteye.advertiseCamera(left_topic, 1);
-    NODELET_INFO_STREAM("Advertized on topic " << left_topic);
+    for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
+      auto &&topic = stream_topics[it->first];
+      if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {  // camera
+        camera_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
+      } else if (it->first == Stream::POINTS) {  // pointcloud
+        points_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(topic, 1);
+      } else {  // image
+        image_publishers_[it->first] = it_mynteye.advertise(topic, 1);
+      }
+      NODELET_INFO_STREAM("Advertized on topic " << topic);
+    }
 
-    camera_encodings_[Stream::RIGHT] = enc::MONO8;
-    camera_publishers_[Stream::RIGHT] =
-        it_mynteye.advertiseCamera(right_topic, 1);
-    NODELET_INFO_STREAM("Advertized on topic " << right_topic);
+    camera_encodings_ = {{Stream::LEFT, enc::MONO8},
+                         {Stream::RIGHT, enc::MONO8}};
 
-    // imu publisher
+    image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::MONO8},
+                        {Stream::RIGHT_RECTIFIED, enc::MONO8},
+                        {Stream::DISPARITY, enc::MONO8},  // float
+                        {Stream::DISPARITY_NORMALIZED, enc::MONO8},
+                        {Stream::DEPTH, enc::MONO16}};
 
     pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 1);
     NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
 
-    // temp publisher
-
     pub_temp_ = nh_.advertise<mynt_eye_ros_wrapper::Temp>(temp_topic, 1);
     NODELET_INFO_STREAM("Advertized on topic " << temp_topic);
 
+    // stream toggles
+
+    for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
+      if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {  // camera
+        continue;
+      } else {  // image, pointcloud
+        if (!api_->Supports(it->first))
+          continue;
+        bool enabled = false;
+        private_nh_.getParam("enable_" + it->second, enabled);
+        if (enabled) {
+          api_->EnableStreamData(it->first);
+          NODELET_INFO_STREAM("Enable stream data of " << it->first);
+        }
+      }
+    }
+
     publishStaticTransforms();
     publishTopics();
   }
@@ -151,7 +188,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     geometry_msgs::TransformStamped b2l_msg;
     b2l_msg.header.stamp = tf_stamp;
     b2l_msg.header.frame_id = base_frame_id_;
-    b2l_msg.child_frame_id = camera_frame_ids_[Stream::LEFT];
+    b2l_msg.child_frame_id = frame_ids_[Stream::LEFT];
     b2l_msg.transform.translation.x = 0;
     b2l_msg.transform.translation.y = 0;
     b2l_msg.transform.translation.z = 0;
@@ -162,17 +199,24 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     static_tf_broadcaster_.sendTransform(b2l_msg);
 
     // Transform base frame to right frame
+    auto &&b2r_ex = api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT);
+    tf::Quaternion b2r_q;
+    tf::Matrix3x3 b2r_r(
+        b2r_ex.rotation[0][0], b2r_ex.rotation[0][1], b2r_ex.rotation[0][2],
+        b2r_ex.rotation[1][0], b2r_ex.rotation[1][1], b2r_ex.rotation[1][2],
+        b2r_ex.rotation[2][0], b2r_ex.rotation[2][1], b2r_ex.rotation[2][2]);
+    b2r_r.getRotation(b2r_q);
     geometry_msgs::TransformStamped b2r_msg;
     b2r_msg.header.stamp = tf_stamp;
     b2r_msg.header.frame_id = base_frame_id_;
-    b2r_msg.child_frame_id = camera_frame_ids_[Stream::RIGHT];
-    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;
+    b2r_msg.child_frame_id = frame_ids_[Stream::RIGHT];
+    b2r_msg.transform.translation.x = b2r_ex.translation[0];
+    b2r_msg.transform.translation.y = b2r_ex.translation[1];
+    b2r_msg.transform.translation.z = b2r_ex.translation[2];
+    b2r_msg.transform.rotation.x = b2r_q.getX();
+    b2r_msg.transform.rotation.y = b2r_q.getY();
+    b2r_msg.transform.rotation.z = b2r_q.getZ();
+    b2r_msg.transform.rotation.w = b2r_q.getW();
     static_tf_broadcaster_.sendTransform(b2r_msg);
 
     // Transform base frame to imu frame
@@ -233,6 +277,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
                             << ", exposure_time: " << data.img->exposure_time);
         });
 
+    std::vector<Stream> image_streams{
+        Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY,
+        Stream::DISPARITY_NORMALIZED, Stream::DEPTH};
+
+    for (auto &&stream : image_streams) {
+      api_->SetStreamCallback(
+          stream, [this, stream](const api::StreamData &data) {
+            static std::size_t count = 0;
+            ++count;
+            publishImage(stream, data, count, ros::Time::now());
+          });
+    }
+
     api_->SetMotionCallback([this](const api::MotionData &data) {
       static double ros_time_beg = ros::Time::now().toSec();
       static double imu_time_beg = data.imu->timestamp;
@@ -277,15 +334,39 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     std_msgs::Header header;
     header.seq = seq;
     header.stamp = stamp;
-    header.frame_id = camera_frame_ids_[stream];
+    header.frame_id = frame_ids_[stream];
+    cv::Mat img = data.frame;
+    if (stream == Stream::DISPARITY) {  // 32FC1 > 8UC1 = MONO8
+      img.convertTo(img, CV_8UC1);
+    }
     auto &&msg =
-        cv_bridge::CvImage(header, camera_encodings_[stream], data.frame)
-            .toImageMsg();
+        cv_bridge::CvImage(header, camera_encodings_[stream], img).toImageMsg();
     auto &&info = getCameraInfo(stream);
     info->header.stamp = msg->header.stamp;
     camera_publishers_[stream].publish(msg, info);
   }
 
+  void publishImage(
+      const Stream &stream, const api::StreamData &data, std::uint32_t seq,
+      ros::Time stamp) {
+    if (image_publishers_[stream].getNumSubscribers() == 0)
+      return;
+    std_msgs::Header header;
+    header.seq = seq;
+    header.stamp = stamp;
+    header.frame_id = frame_ids_[stream];
+    cv::Mat img = data.frame;
+    if (stream == Stream::DISPARITY) {  // 32FC1 > 8UC1 = MONO8
+      img.convertTo(img, CV_8UC1);
+    }
+    auto &&msg =
+        cv_bridge::CvImage(header, image_encodings_[stream], img).toImageMsg();
+    image_publishers_[stream].publish(msg);
+  }
+
+  // void publishPoints(
+  // )
+
   void publishImu(
       const api::MotionData &data, std::uint32_t seq, ros::Time stamp) {
     if (pub_imu_.getNumSubscribers() == 0)
@@ -394,7 +475,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     auto &&in = api_->GetIntrinsics(stream);
 
-    camera_info->header.frame_id = camera_frame_ids_[stream];
+    camera_info->header.frame_id = frame_ids_[stream];
     camera_info->width = in.width;
     camera_info->height = in.height;
 
@@ -456,10 +537,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
 
+  // camera: LEFT, RIGHT
   std::map<Stream, image_transport::CameraPublisher> camera_publishers_;
   std::map<Stream, sensor_msgs::CameraInfoPtr> camera_info_ptrs_;
   std::map<Stream, std::string> camera_encodings_;
 
+  // image: LEFT_RECTIFIED, RIGHT_RECTIFIED, DISPARITY, DISPARITY_NORMALIZED,
+  // DEPTH
+  std::map<Stream, image_transport::Publisher> image_publishers_;
+  std::map<Stream, std::string> image_encodings_;
+
+  // pointcloud: POINTS
+  ros::Publisher points_publisher_;
+
   ros::Publisher pub_imu_;
   ros::Publisher pub_temp_;
 
@@ -470,7 +560,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   std::string base_frame_id_;
   std::string imu_frame_id_;
   std::string temp_frame_id_;
-  std::map<Stream, std::string> camera_frame_ids_;
+  std::map<Stream, std::string> frame_ids_;
 
   double gravity_;