Merge pull request uzh-rpg#63 from negre/master

change depth format from RGB to RFloat
ayushgaud · Dec 1, 2020 · b3219b3 · b3219b3
2 parents c008954 + 27df4ef
commit b3219b3
Show file tree

Hide file tree

Showing 4 changed files with 42 additions and 5 deletions.
diff --git a/flightlib/CMakeLists.txt b/flightlib/CMakeLists.txt
@@ -50,6 +50,13 @@ include(cmake/yaml.cmake)
 find_package(OpenCV REQUIRED)
 find_package(OpenMP REQUIRED)
 
+if(OPENMP_FOUND)
+     message("OPENMP FOUND")
+     set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+     set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
 if(ENABLE_BLAS)
   set(BLA_VENDOR "Generic")
   find_package(BLAS REQUIRED)
@@ -202,6 +209,7 @@ else()
   add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES})
   target_link_libraries(${PROJECT_NAME} PRIVATE
     ${OpenCV_LIBRARIES}
+    openmp
     yaml-cpp
     zmq
     zmqpp

diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp
@@ -216,7 +216,34 @@ 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;
+
+	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
@@ -238,6 +265,7 @@ bool UnityBridge::handleOutput() {
         }
         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;
-}
+}