Merge pull request uzh-rpg#69 from uzh-rpg/negre/depth

change depth format from RGB to RFloat
Peter010103 · Dec 1, 2020 · 20e30d1 · 20e30d1
2 parents c008954 + 16fd627
commit 20e30d1
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 25 deletions.
diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp
@@ -216,28 +216,56 @@ bool UnityBridge::handleOutput() {
       for (size_t layer_idx = 0; layer_idx <= cam.enabled_layers.size();
            layer_idx++) {
         if (!layer_idx == 0 && !cam.enabled_layers[layer_idx - 1]) continue;
-        uint32_t image_len = cam.width * cam.height * cam.channels;
-        // Get raw image bytes from ZMQ message.
-        // WARNING: This is a zero-copy operation that also casts the input to
-        // an array of unit8_t. when the message is deleted, this pointer is
-        // also dereferenced.
-        const uint8_t* image_data;
-        msg.get(image_data, image_i);
-        image_i = image_i + 1;
-        // Pack image into cv::Mat
-        cv::Mat new_image =
-          cv::Mat(cam.height, cam.width, CV_MAKETYPE(CV_8U, cam.channels));
-        memcpy(new_image.data, image_data, image_len);
-        // Flip image since OpenCV origin is upper left, but Unity's is lower
-        // left.
-        cv::flip(new_image, new_image, 0);
-
-        // Tell OpenCv that the input is RGB.
-        if (cam.channels == 3) {
-          cv::cvtColor(new_image, new_image, CV_RGB2BGR);
+
+        if (layer_idx == 1) {
+          // depth
+          uint32_t image_len = cam.width * cam.height * 4;
+          // Get raw image bytes from ZMQ message.
+          // WARNING: This is a zero-copy operation that also casts the input to
+          // an array of unit8_t. when the message is deleted, this pointer is
+          // also dereferenced.
+          const uint8_t* image_data;
+          msg.get(image_data, image_i);
+          image_i = image_i + 1;
+          // Pack image into cv::Mat
+          cv::Mat new_image = cv::Mat(cam.height, cam.width, CV_32FC1);
+          memcpy(new_image.data, image_data, image_len);
+          // Flip image since OpenCV origin is upper left, but Unity's is lower
+          // left.
+          new_image = new_image * (100.f);
+          cv::flip(new_image, new_image, 0);
+
+
+          unity_quadrotors_[idx]
+            ->getCameras()[cam.output_index]
+            ->feedImageQueue(layer_idx, new_image);
+
+
+        } else {
+          uint32_t image_len = cam.width * cam.height * cam.channels;
+          // Get raw image bytes from ZMQ message.
+          // WARNING: This is a zero-copy operation that also casts the input to
+          // an array of unit8_t. when the message is deleted, this pointer is
+          // also dereferenced.
+          const uint8_t* image_data;
+          msg.get(image_data, image_i);
+          image_i = image_i + 1;
+          // Pack image into cv::Mat
+          cv::Mat new_image =
+            cv::Mat(cam.height, cam.width, CV_MAKETYPE(CV_8U, cam.channels));
+          memcpy(new_image.data, image_data, image_len);
+          // Flip image since OpenCV origin is upper left, but Unity's is lower
+          // left.
+          cv::flip(new_image, new_image, 0);
+
+          // Tell OpenCv that the input is RGB.
+          if (cam.channels == 3) {
+            cv::cvtColor(new_image, new_image, CV_RGB2BGR);
+          }
+          unity_quadrotors_[idx]
+            ->getCameras()[cam.output_index]
+            ->feedImageQueue(layer_idx, new_image);
         }
-        unity_quadrotors_[idx]->getCameras()[cam.output_index]->feedImageQueue(
-          layer_idx, new_image);
       }
     }
   }

diff --git a/flightros/launch/camera/camera.launch b/flightros/launch/camera/camera.launch
@@ -1,8 +1,9 @@
 <launch>
     <arg name="debug" default="0" />
-
+     <arg name="use_unity_editor" default="false" />
+
     <!-- RPG Flightmare Unity Render. -->
-    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" >
+    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">
     </node>
 
     <node name="camera" pkg="flightros" type="camera" output="screen" launch-prefix="gdb -ex run --args" if="$(arg debug)" >

diff --git a/flightros/src/camera/camera.cpp b/flightros/src/camera/camera.cpp
@@ -90,7 +90,7 @@ int main(int argc, char *argv[]) {
 
     rgb_camera->getDepthMap(img);
     sensor_msgs::ImagePtr depth_msg =
-      cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
+      cv_bridge::CvImage(std_msgs::Header(), "32FC1", img).toImageMsg();
     depth_msg->header.stamp = timestamp;
     depth_pub.publish(depth_msg);
 
@@ -112,4 +112,4 @@ int main(int argc, char *argv[]) {
   }
 
   return 0;
-}
+}