Dreamview: process image on demand & make voxel grid size configurable (#4147)

* Dreamview: process image on demand

* Adjust voxel grid size

modules/dreamview/backend/common/dreamview_gflags.cc

@@ -78,3 +78,8 @@ DEFINE_bool(sim_world_with_routing_path, false,
 DEFINE_string(
     request_timeout_ms, "2000",
     "Timeout for network read and network write operations, in milliseconds.");
+
+DEFINE_double(voxel_filter_size, 0.3, "VoxelGrid pointcloud filter leaf size");
+
+DEFINE_double(voxel_filter_height, 0.2,
+              "VoxelGrid pointcloud filter leaf height");

modules/dreamview/backend/common/dreamview_gflags.h

@@ -53,4 +53,8 @@ DECLARE_bool(sim_world_with_routing_path);
 
 DECLARE_string(request_timeout_ms);
 
+DECLARE_double(voxel_filter_size);
+
+DECLARE_double(voxel_filter_height);
+
 #endif  // MODULES_DREAMVIEW_BACKEND_COMMON_DREAMVIEW_GFLAGS_H_

modules/dreamview/backend/dreamview.cc

@@ -45,21 +45,7 @@ void Dreamview::TerminateProfilingMode(const ros::TimerEvent& event) {
   AWARN << "Profiling timer called shutdown!";
 }
 
-Status Dreamview::Init() {
-  AdapterManager::Init(FLAGS_dreamview_adapter_config_filename);
-  VehicleConfigHelper::Init();
-
-  if (FLAGS_dreamview_profiling_mode &&
-      FLAGS_dreamview_profiling_duration > 0.0) {
-    exit_timer_ = AdapterManager::CreateTimer(
-        ros::Duration(FLAGS_dreamview_profiling_duration),
-        &Dreamview::TerminateProfilingMode, this, true, true);
-    AWARN << "============================================================";
-    AWARN << "| Dreamview running in profiling mode, exit in "
-          << FLAGS_dreamview_profiling_duration << " seconds |";
-    AWARN << "============================================================";
-  }
-
+void Dreamview::CheckAdapters() {
   // Check the expected adapters are initialized.
   CHECK(AdapterManager::GetChassis()) << "ChassisAdapter is not initialized.";
   CHECK(AdapterManager::GetControlCommand())
@@ -90,6 +76,24 @@ Status Dreamview::Init() {
       << "PointCloudAdapter is not initialized.";
   CHECK(AdapterManager::GetRelativeMap())
       << "RelativeMapAdapter is not initialized.";
+}
+
+Status Dreamview::Init() {
+  AdapterManager::Init(FLAGS_dreamview_adapter_config_filename);
+  VehicleConfigHelper::Init();
+
+  if (FLAGS_dreamview_profiling_mode &&
+      FLAGS_dreamview_profiling_duration > 0.0) {
+    exit_timer_ = AdapterManager::CreateTimer(
+        ros::Duration(FLAGS_dreamview_profiling_duration),
+        &Dreamview::TerminateProfilingMode, this, true, true);
+    AWARN << "============================================================";
+    AWARN << "| Dreamview running in profiling mode, exit in "
+          << FLAGS_dreamview_profiling_duration << " seconds |";
+    AWARN << "============================================================";
+  }
+
+  CheckAdapters();
 
   // Initialize and run the web server which serves the dreamview htmls and
   // javascripts and handles websocket requests.

modules/dreamview/backend/dreamview.h

@@ -49,6 +49,8 @@ class Dreamview : public apollo::common::ApolloApp {
 
  private:
   void TerminateProfilingMode(const ros::TimerEvent& event);
+  void CheckAdapters();
+
   ros::Timer exit_timer_;
 
   std::unique_ptr<SimulationWorldUpdater> sim_world_updater_;

modules/dreamview/backend/handlers/image_handler.cc

@@ -34,6 +34,10 @@ constexpr double ImageHandler::kImageScale;
 
 template <>
 void ImageHandler::OnImage(const sensor_msgs::Image &image) {
+  if (requests_ == 0) {
+    return;
+  }
+
   if (image.encoding != "yuyv") {
     AERROR_EVERY(100) << "Image format not support: " << image.encoding;
     return;
@@ -43,6 +47,7 @@ void ImageHandler::OnImage(const sensor_msgs::Image &image) {
   auto mat = cv::Mat(image.height, image.width, CV_8UC3);
   apollo::perception::traffic_light::Yuyv2rgb(yuv, mat.data,
                                               image.height * image.width);
+
   cv::cvtColor(mat, mat, CV_RGB2BGR);
 
   cv::resize(mat, mat, cv::Size(image.width * ImageHandler::kImageScale,
@@ -56,6 +61,10 @@ void ImageHandler::OnImage(const sensor_msgs::Image &image) {
 
 template <>
 void ImageHandler::OnImage(const sensor_msgs::CompressedImage &image) {
+  if (requests_ == 0) {
+    return;
+  }
+
   try {
     std::unique_lock<std::mutex> lock(mutex_);
     auto current_image = cv_bridge::toCvCopy(image);
@@ -68,7 +77,7 @@ void ImageHandler::OnImage(const sensor_msgs::CompressedImage &image) {
   }
 }
 
-ImageHandler::ImageHandler() {
+ImageHandler::ImageHandler() : requests_(0) {
   if (FLAGS_use_navigation_mode) {
     AdapterManager::AddCompressedImageCallback(&ImageHandler::OnImage, this);
   } else {
@@ -77,9 +86,7 @@ ImageHandler::ImageHandler() {
 }
 
 bool ImageHandler::handleGet(CivetServer *server, struct mg_connection *conn) {
-  if (send_buffer_.empty()) {
-    return true;
-  }
+  requests_++;
 
   mg_printf(conn,
             "HTTP/1.1 200 OK\r\n"
@@ -92,27 +99,34 @@ bool ImageHandler::handleGet(CivetServer *server, struct mg_connection *conn) {
             "boundary=--BoundaryString\r\n"
             "\r\n");
 
+  std::vector<uchar> to_send;
   while (true) {
-    std::vector<uchar> to_send;
     {
       std::unique_lock<std::mutex> lock(mutex_);
       to_send = send_buffer_;
     }
-    // Sends the image data
-    mg_printf(conn,
-              "--BoundaryString\r\n"
-              "Content-type: image/jpeg\r\n"
-              "Content-Length: %zu\r\n"
-              "\r\n",
-              to_send.size());
-    if (mg_write(conn, &to_send[0], to_send.size()) <= 0) {
-      return false;
+
+    if (!to_send.empty()) {
+      // Sends the image data
+      mg_printf(conn,
+                "--BoundaryString\r\n"
+                "Content-type: image/jpeg\r\n"
+                "Content-Length: %zu\r\n"
+                "\r\n",
+                to_send.size());
+
+      if (mg_write(conn, &to_send[0], to_send.size()) <= 0) {
+        requests_--;
+        return false;
+      }
+      mg_printf(conn, "\r\n\r\n");
     }
-    mg_printf(conn, "\r\n\r\n");
 
     std::unique_lock<std::mutex> lock(mutex_);
     cvar_.wait(lock);
   }
+
+  requests_--;
   return true;
 }
 

modules/dreamview/backend/handlers/image_handler.h

@@ -21,11 +21,11 @@
 #ifndef MODULES_DREAMVIEW_BACKEND_HANDLERS_IMAGE_HANDLER_H_
 #define MODULES_DREAMVIEW_BACKEND_HANDLERS_IMAGE_HANDLER_H_
 
+#include <atomic>
 #include <condition_variable>
 #include <mutex>
 #include <string>
 #include <vector>
-
 #include "cv_bridge/cv_bridge.h"
 
 #include "CivetServer.h"
@@ -57,6 +57,7 @@ class ImageHandler : public CivetHandler {
   void OnImage(const SensorMsgsImage &image);
 
   std::vector<uchar> send_buffer_;
+  std::atomic<int> requests_;
 
   // mutex lock and condition variable to protect the received image
   std::mutex mutex_;

modules/dreamview/backend/point_cloud/point_cloud_updater.cc

@@ -37,15 +37,11 @@ using sensor_msgs::PointCloud2;
 using Json = nlohmann::json;
 
 PointCloudUpdater::PointCloudUpdater(WebSocketHandler *websocket)
-    : websocket_(websocket) {
+    : websocket_(websocket), point_cloud_str_(""), future_ready_(true) {
   RegisterMessageHandlers();
-  point_cloud_str_ = "";
-  future_ready_ = true;
 }
 
-PointCloudUpdater::~PointCloudUpdater() {
-  Stop();
-}
+PointCloudUpdater::~PointCloudUpdater() { Stop(); }
 
 void PointCloudUpdater::RegisterMessageHandlers() {
   // Send current point_cloud status to the new client.
@@ -119,18 +115,19 @@ void PointCloudUpdater::UpdatePointCloud(const PointCloud2 &point_cloud) {
     pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr(
         new pcl::PointCloud<pcl::PointXYZ>);
     pcl::fromROSMsg(point_cloud, *pcl_ptr);
-    std::future<void> f = std::async(
-        std::launch::async, &PointCloudUpdater::FilterPointCloud, this,
-        pcl_ptr);
+    std::future<void> f =
+        std::async(std::launch::async, &PointCloudUpdater::FilterPointCloud,
+                   this, pcl_ptr);
     async_future_ = std::move(f);
   }
 }
 
 void PointCloudUpdater::FilterPointCloud(
     const pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr) {
-  pcl::VoxelGrid < pcl::PointXYZ > voxel_grid;
+  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
   voxel_grid.setInputCloud(pcl_ptr);
-  voxel_grid.setLeafSize(1.0f, 1.0f, 0.2f);
+  voxel_grid.setLeafSize(FLAGS_voxel_filter_size, FLAGS_voxel_filter_size,
+                         FLAGS_voxel_filter_height);
   voxel_grid.filter(*pcl_ptr);
   AINFO << "filtered point cloud data size: " << pcl_ptr->size();
 

modules/dreamview/frontend/proto_bundle/sim_world_proto_bundle.json

@@ -2930,124 +2930,6 @@
         },
         "hdmap": {
           "nested": {
-            "Projection": {
-              "fields": {
-                "proj": {
-                  "type": "string",
-                  "id": 1
-                }
-              }
-            },
-            "Header": {
-              "fields": {
-                "version": {
-                  "type": "bytes",
-                  "id": 1
-                },
-                "date": {
-                  "type": "bytes",
-                  "id": 2
-                },
-                "projection": {
-                  "type": "Projection",
-                  "id": 3
-                },
-                "district": {
-                  "type": "bytes",
-                  "id": 4
-                },
-                "generation": {
-                  "type": "bytes",
-                  "id": 5
-                },
-                "revMajor": {
-                  "type": "bytes",
-                  "id": 6
-                },
-                "revMinor": {
-                  "type": "bytes",
-                  "id": 7
-                },
-                "left": {
-                  "type": "double",
-                  "id": 8
-                },
-                "top": {
-                  "type": "double",
-                  "id": 9
-                },
-                "right": {
-                  "type": "double",
-                  "id": 10
-                },
-                "bottom": {
-                  "type": "double",
-                  "id": 11
-                },
-                "vendor": {
-                  "type": "bytes",
-                  "id": 12
-                }
-              }
-            },
-            "Map": {
-              "fields": {
-                "header": {
-                  "type": "Header",
-                  "id": 1
-                },
-                "crosswalk": {
-                  "rule": "repeated",
-                  "type": "Crosswalk",
-                  "id": 2
-                },
-                "junction": {
-                  "rule": "repeated",
-                  "type": "Junction",
-                  "id": 3
-                },
-                "lane": {
-                  "rule": "repeated",
-                  "type": "Lane",
-                  "id": 4
-                },
-                "stopSign": {
-                  "rule": "repeated",
-                  "type": "StopSign",
-                  "id": 5
-                },
-                "signal": {
-                  "rule": "repeated",
-                  "type": "Signal",
-                  "id": 6
-                },
-                "yield": {
-                  "rule": "repeated",
-                  "type": "YieldSign",
-                  "id": 7
-                },
-                "overlap": {
-                  "rule": "repeated",
-                  "type": "Overlap",
-                  "id": 8
-                },
-                "clearArea": {
-                  "rule": "repeated",
-                  "type": "ClearArea",
-                  "id": 9
-                },
-                "speedBump": {
-                  "rule": "repeated",
-                  "type": "SpeedBump",
-                  "id": 10
-                },
-                "road": {
-                  "rule": "repeated",
-                  "type": "Road",
-                  "id": 11
-                }
-              }
-            },
             "ClearArea": {
               "fields": {
                 "id": {
@@ -3498,6 +3380,124 @@
                 }
               }
             },
+            "Projection": {
+              "fields": {
+                "proj": {
+                  "type": "string",
+                  "id": 1
+                }
+              }
+            },
+            "Header": {
+              "fields": {
+                "version": {
+                  "type": "bytes",
+                  "id": 1
+                },
+                "date": {
+                  "type": "bytes",
+                  "id": 2
+                },
+                "projection": {
+                  "type": "Projection",
+                  "id": 3
+                },
+                "district": {
+                  "type": "bytes",
+                  "id": 4
+                },
+                "generation": {
+                  "type": "bytes",
+                  "id": 5
+                },
+                "revMajor": {
+                  "type": "bytes",
+                  "id": 6
+                },
+                "revMinor": {
+                  "type": "bytes",
+                  "id": 7
+                },
+                "left": {
+                  "type": "double",
+                  "id": 8
+                },
+                "top": {
+                  "type": "double",
+                  "id": 9
+                },
+                "right": {
+                  "type": "double",
+                  "id": 10
+                },
+                "bottom": {
+                  "type": "double",
+                  "id": 11
+                },
+                "vendor": {
+                  "type": "bytes",
+                  "id": 12
+                }
+              }
+            },
+            "Map": {
+              "fields": {
+                "header": {
+                  "type": "Header",
+                  "id": 1
+                },
+                "crosswalk": {
+                  "rule": "repeated",
+                  "type": "Crosswalk",
+                  "id": 2
+                },
+                "junction": {
+                  "rule": "repeated",
+                  "type": "Junction",
+                  "id": 3
+                },
+                "lane": {
+                  "rule": "repeated",
+                  "type": "Lane",
+                  "id": 4
+                },
+                "stopSign": {
+                  "rule": "repeated",
+                  "type": "StopSign",
+                  "id": 5
+                },
+                "signal": {
+                  "rule": "repeated",
+                  "type": "Signal",
+                  "id": 6
+                },
+                "yield": {
+                  "rule": "repeated",
+                  "type": "YieldSign",
+                  "id": 7
+                },
+                "overlap": {
+                  "rule": "repeated",
+                  "type": "Overlap",
+                  "id": 8
+                },
+                "clearArea": {
+                  "rule": "repeated",
+                  "type": "ClearArea",
+                  "id": 9
+                },
+                "speedBump": {
+                  "rule": "repeated",
+                  "type": "SpeedBump",
+                  "id": 10
+                },
+                "road": {
+                  "rule": "repeated",
+                  "type": "Road",
+                  "id": 11
+                }
+              }
+            },
             "BoundaryEdge": {
               "fields": {
                 "curve": {

modules/dreamview/frontend/src/renderer/point_cloud.js

 import * as THREE from "three";
 
-const MAX_POINTS = 10000;
+const MAX_POINTS = 100000;
 const HEIGHT_COLOR_MAPPING = {
     0.5: 0xFF0000,
     1.0: 0xFF7F00,