draft version for dreamview backend handling of pointcloud

modules/dreamview/backend/BUILD

@@ -16,6 +16,7 @@ cc_library(
         "//modules/common/adapters:adapter_manager",
         "//modules/dreamview/backend/handlers:websocket",
         "//modules/dreamview/backend/hmi",
+        "//modules/dreamview/backend/point_cloud:point_cloud_updater",
         "//modules/dreamview/backend/sim_control",
         "//modules/dreamview/backend/simulation_world:simulation_world_updater",
         "//modules/map/hdmap:hdmap_util",

modules/dreamview/backend/dreamview.cc

@@ -101,17 +101,21 @@ Status Dreamview::Init() {
 
   image_.reset(new ImageHandler());
   websocket_.reset(new WebSocketHandler());
+<<<<<<< HEAD
   map_ws_.reset(new WebSocketHandler());
+  point_cloud_ws_.reset(new WebSocketHandler());
   map_service_.reset(new MapService());
   sim_control_.reset(new SimControl(map_service_.get()));
 
   sim_world_updater_.reset(new SimulationWorldUpdater(
       websocket_.get(), map_ws_.get(), sim_control_.get(), map_service_.get(),
       FLAGS_routing_from_file));
+  point_cloud_updater_.reset(new PointCloudUpdater(point_cloud_ws_.get()));
   hmi_.reset(new HMI(websocket_.get(), map_service_.get()));
 
   server_->addWebSocketHandler("/websocket", *websocket_);
   server_->addWebSocketHandler("/map", *map_ws_);
+  server_->addWebSocketHandler("/pointcloud", *point_cloud_ws_);
   server_->addHandler("/image", *image_);
 
   ApolloApp::SetCallbackThreadNumber(FLAGS_dreamview_worker_num);
@@ -121,6 +125,7 @@ Status Dreamview::Init() {
 
 Status Dreamview::Start() {
   sim_world_updater_->Start();
+  point_cloud_updater_->Start();
   return Status::OK();
 }
 

modules/dreamview/backend/dreamview.h

@@ -28,6 +28,7 @@
 #include "modules/dreamview/backend/handlers/websocket.h"
 #include "modules/dreamview/backend/hmi/hmi.h"
 #include "modules/dreamview/backend/map/map_service.h"
+#include "modules/dreamview/backend/point_cloud/point_cloud_updater.h"
 #include "modules/dreamview/backend/sim_control/sim_control.h"
 #include "modules/dreamview/backend/simulation_world/simulation_world_updater.h"
 
@@ -51,10 +52,12 @@ class Dreamview : public apollo::common::ApolloApp {
   ros::Timer exit_timer_;
 
   std::unique_ptr<SimulationWorldUpdater> sim_world_updater_;
+  std::unique_ptr<PointCloudUpdater> point_cloud_updater_;
   std::unique_ptr<CivetServer> server_;
   std::unique_ptr<SimControl> sim_control_;
   std::unique_ptr<WebSocketHandler> websocket_;
   std::unique_ptr<WebSocketHandler> map_ws_;
+  std::unique_ptr<WebSocketHandler> point_cloud_ws_;
   std::unique_ptr<ImageHandler> image_;
   std::unique_ptr<MapService> map_service_;
   std::unique_ptr<HMI> hmi_;

modules/dreamview/backend/point_cloud/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "point_cloud_updater",
+    srcs = [
+        "point_cloud_updater.cc",
+    ],
+    hdrs = [
+        "point_cloud_updater.h",
+    ],
+    linkopts = [
+        "-lboost_thread",
+    ],
+    deps = [
+        "//modules/common:log",
+        "//modules/common/adapters:adapter_manager",
+        "//modules/dreamview/backend/common:dreamview_gflags",
+        "//modules/dreamview/backend/handlers:websocket",
+        "//third_party/json",
+        "@com_google_protobuf//:protobuf",
+        "@pcl//:pcl",
+    ],
+)
+
+cpplint()

modules/dreamview/backend/point_cloud/point_cloud_updater.cc

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/dreamview/backend/point_cloud/point_cloud_updater.h"
+
+#include "modules/common/adapters/adapter_manager.h"
+#include "modules/common/log.h"
+#include "modules/dreamview/backend/common/dreamview_gflags.h"
+#include "pcl_conversions/pcl_conversions.h"
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+#include "third_party/json/json.hpp"
+
+namespace apollo {
+namespace dreamview {
+
+using apollo::common::adapter::AdapterManager;
+using sensor_msgs::PointCloud2;
+using Json = nlohmann::json;
+
+PointCloudUpdater::PointCloudUpdater(WebSocketHandler *websocket)
+    : websocket_(websocket) {
+  RegisterMessageHandlers();
+}
+
+void PointCloudUpdater::RegisterMessageHandlers() {
+  websocket_->RegisterMessageHandler(
+      "RequestPointCloud",
+      [this](const Json &json, WebSocketHandler::Connection *conn) {
+        std::string to_send;
+        {
+          // Pay the price to copy the data instead of sending data over the
+          // wire while holding the lock.
+          boost::shared_lock<boost::shared_mutex> lock(mutex_);
+          to_send = point_cloud_str_;
+        }
+        websocket_->SendData(conn, to_send, true);
+      });
+}
+
+void PointCloudUpdater::Start() {
+  AdapterManager::AddPointCloudCallback(&PointCloudUpdater::UpdatePointCloud,
+                                        this);
+}
+
+void PointCloudUpdater::UpdatePointCloud(const PointCloud2 &point_cloud) {
+  // transform from ros to pcl
+  pcl::PointCloud < pcl::PointXYZ > pcl_data;
+  pcl::fromROSMsg(point_cloud, pcl_data);
+
+  if (pcl_data.size() == 0) {
+    point_cloud_str_ = "[]";
+    return;
+  }
+
+  std::stringstream stream;
+  std::string data("[");
+  for (size_t idx = 0; idx < pcl_data.size(); idx += 2) {
+    pcl::PointXYZ& pt = pcl_data.points[idx];
+    if (!isnan(pt.x) && !isnan(pt.y) && !isnan(pt.z)) {
+      data.append("[");
+      stream.str(std::string());
+      stream << std::fixed << std::setprecision(4) << pt.x;
+      data.append(stream.str());
+      data.append(",");
+      stream.str(std::string());
+      stream << std::fixed << std::setprecision(4) << pt.y;
+      data.append(stream.str());
+      data.append(",");
+      stream.str(std::string());
+      stream << std::fixed << std::setprecision(4) << pt.z;
+      data.append(stream.str());
+      data.append("]");
+      if ((idx + 1) == pcl_data.size()) {
+        data.append("]");
+      } else {
+        data.append(",");
+      }
+    }
+  }
+  boost::shared_lock<boost::shared_mutex> lock(mutex_);
+  point_cloud_str_ = data;
+}
+
+}  // namespace dreamview
+}  // namespace apollo

modules/dreamview/backend/point_cloud/point_cloud_updater.h

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#ifndef MODULES_DREAMVIEW_BACKEND_POINT_CLOUD_POINT_CLOUD_UPDATER_H_
+#define MODULES_DREAMVIEW_BACKEND_POINT_CLOUD_POINT_CLOUD_UPDATER_H_
+
+#include <string>
+
+#include "boost/thread/locks.hpp"
+#include "boost/thread/shared_mutex.hpp"
+
+#include "modules/common/log.h"
+#include "modules/common/util/string_util.h"
+#include "modules/dreamview/backend/handlers/websocket.h"
+#include "sensor_msgs/PointCloud2.h"
+
+/**
+ * @namespace apollo::dreamview
+ * @brief apollo::dreamview
+ */
+namespace apollo {
+namespace dreamview {
+
+/**
+ * @class PointCloudUpdater
+ * @brief A wrapper around WebSocketHandler to keep pushing PointCloud to frontend
+ * via websocket while handling the response from frontend.
+ */
+class PointCloudUpdater {
+ public:
+  /**
+   * @brief Constructor with the websocket handler.
+   * @param websocket Pointer of the websocket handler that has been attached to
+   * the server.
+   */
+  explicit PointCloudUpdater(WebSocketHandler *websocket);
+
+  /**
+   * @brief Starts to push PointCloud to frontend.
+   */
+  void Start();
+
+ private:
+  void RegisterMessageHandlers();
+
+  void UpdatePointCloud(const sensor_msgs::PointCloud2 &point_cloud);
+
+  WebSocketHandler *websocket_;
+
+  // The PointCloud to be pushed to frontend.
+  std::string point_cloud_str_;
+
+  // Mutex to protect concurrent access to point_cloud_str_.
+  // NOTE: Use boost until we have std version of rwlock support.
+  boost::shared_mutex mutex_;
+};
+
+}  // namespace dreamview
+}  // namespace apollo
+
+#endif  // MODULES_DREAMVIEW_BACKEND_POINT_CLOUD_POINT_CLOUD_UPDATER_H_

modules/dreamview/conf/adapter.conf

@@ -48,6 +48,11 @@ config {
   mode: RECEIVE_ONLY
   message_history_limit: 1
 }
+config: {
+  type: POINT_CLOUD
+  mode: RECEIVE_ONLY
+  message_history_limit: 1
+}
 config {
   type: ROUTING_RESPONSE
   mode: DUPLEX