From 48a9cd046147345b09b6259b923924fd2d9bf2b3 Mon Sep 17 00:00:00 2001
From: Kalman <kalmanxiong@slightech.com>
Date: Thu, 25 Oct 2018 20:25:14 +0800
Subject: [PATCH] Publish gray topic

---
 .../launch/mynteye.launch                     |  6 +++
 .../mynt_eye_ros_wrapper/rviz/mynteye.rviz    | 29 ++++++++++++++
 .../src/wrapper_nodelet.cc                    | 39 +++++++++++++++++++
 3 files changed, 74 insertions(+)

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 6b8f792..c40222e 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -13,6 +13,9 @@
   <arg name="depth_topic" default="depth/image_raw" />
   <arg name="points_topic" default="points/data_raw" />
 
+  <arg name="left_gray_topic" default="left/image_gray" />
+  <arg name="right_gray_topic" default="right/image_gray" />
+
   <arg name="imu_topic" default="imu/data_raw" />
   <arg name="temp_topic" default="temp/data_raw" />
 
@@ -102,6 +105,9 @@
       <param name="points_topic" value="$(arg points_topic)" />
       <param name="depth_topic" value="$(arg depth_topic)" />
 
+      <param name="left_gray_topic" value="$(arg left_gray_topic)" />
+      <param name="right_gray_topic" value="$(arg right_gray_topic)" />
+
       <param name="imu_topic" value="$(arg imu_topic)" />
       <param name="temp_topic" value="$(arg temp_topic)" />
 
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz
index 0fd02ab..d378a1a 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz
@@ -7,6 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
         - /Images1
+        - /Images1/Gray1
         - /Images1/Rectified1
         - /Disparity1
       Splitter Ratio: 0.5
@@ -77,6 +78,34 @@ Visualization Manager:
           Transport Hint: raw
           Unreliable: false
           Value: true
+        - Class: rviz/Group
+          Displays:
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /mynteye/left/image_gray
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: LeftGray
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: raw
+              Unreliable: false
+              Value: true
+            - Class: rviz/Image
+              Enabled: true
+              Image Topic: /mynteye/right/image_gray
+              Max Value: 1
+              Median window: 5
+              Min Value: 0
+              Name: RightGray
+              Normalize Range: true
+              Queue Size: 2
+              Transport Hint: raw
+              Unreliable: false
+              Value: true
+          Enabled: false
+          Name: Gray
         - Class: rviz/Group
           Displays:
             - Class: rviz/Image
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 80b941f..3f6d0a4 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
@@ -115,6 +115,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       private_nh_.getParam(it->second + "_topic", stream_topics[it->first]);
     }
 
+    std::map<Stream, std::string> gray_names{{Stream::LEFT, "left_gray"},
+                                             {Stream::RIGHT, "right_gray"}};
+
+    std::map<Stream, std::string> gray_topics{};
+    for (auto &&it = gray_names.begin(); it != gray_names.end(); ++it) {
+      gray_topics[it->first] = it->second;
+      private_nh_.getParam(it->second + "_topic", gray_topics[it->first]);
+    }
+
     std::string imu_topic = "imu";
     std::string temp_topic = "temp";
     private_nh_.getParam("imu_topic", imu_topic);
@@ -179,6 +188,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       NODELET_INFO_STREAM("Advertized on topic " << topic);
     }
 
+    for (auto &&it = gray_topics.begin(); it != gray_topics.end(); ++it) {
+      auto &&topic = gray_topics[it->first];
+      if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {  // camera
+        gray_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
+      }
+      NODELET_INFO_STREAM("Advertized on topic " << topic);
+    }
+
     camera_encodings_ = {{Stream::LEFT, enc::BGR8}, {Stream::RIGHT, enc::BGR8}};
 
     image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::BGR8},
@@ -274,6 +291,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
           ++left_count_;
           publishCamera(Stream::LEFT, data, left_count_, stamp);
+          publishGray(Stream::LEFT, data, left_count_, stamp);
           NODELET_DEBUG_STREAM(
               Stream::LEFT << ", count: " << left_count_
                            << ", frame_id: " << data.img->frame_id
@@ -287,6 +305,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
           ++right_count_;
           publishCamera(Stream::RIGHT, data, right_count_, stamp);
+          publishGray(Stream::RIGHT, data, right_count_, stamp);
           NODELET_DEBUG_STREAM(
               Stream::RIGHT << ", count: " << right_count_
                             << ", frame_id: " << data.img->frame_id
@@ -400,6 +419,23 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     image_publishers_[stream].publish(msg);
   }
 
+  void publishGray(
+      const Stream &stream, const api::StreamData &data, std::uint32_t seq,
+      ros::Time stamp) {
+    if (gray_publishers_[stream].getNumSubscribers() == 0)
+      return;
+    std_msgs::Header header;
+    header.seq = seq;
+    header.stamp = stamp;
+    header.frame_id = frame_ids_[stream];
+    cv::Mat gray;
+    cv::cvtColor(data.frame, gray, CV_RGB2GRAY);
+    auto &&msg = cv_bridge::CvImage(header, enc::MONO8, gray).toImageMsg();
+    auto &&info = getCameraInfo(stream);
+    info->header.stamp = msg->header.stamp;
+    gray_publishers_[stream].publish(msg, info);
+  }
+
   void publishPoints(
       const api::StreamData &data, std::uint32_t seq, ros::Time stamp) {
     if (points_publisher_.getNumSubscribers() == 0)
@@ -842,6 +878,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   std::map<Stream, image_transport::Publisher> image_publishers_;
   std::map<Stream, std::string> image_encodings_;
 
+  // gray: LEFT, RIGHT
+  std::map<Stream, image_transport::CameraPublisher> gray_publishers_;
+
   // pointcloud: POINTS
   ros::Publisher points_publisher_;