From b432bb799022ff76106b021ad0ccef6026067142 Mon Sep 17 00:00:00 2001 From: unacao Date: Wed, 7 Feb 2018 17:32:19 -0800 Subject: [PATCH] draft version for dreamview backend handling of pointcloud --- modules/dreamview/backend/BUILD | 1 + modules/dreamview/backend/dreamview.cc | 5 + modules/dreamview/backend/dreamview.h | 3 + modules/dreamview/backend/point_cloud/BUILD | 27 +++++ .../point_cloud/point_cloud_updater.cc | 99 +++++++++++++++++++ .../backend/point_cloud/point_cloud_updater.h | 78 +++++++++++++++ modules/dreamview/conf/adapter.conf | 5 + 7 files changed, 218 insertions(+) create mode 100644 modules/dreamview/backend/point_cloud/BUILD create mode 100644 modules/dreamview/backend/point_cloud/point_cloud_updater.cc create mode 100644 modules/dreamview/backend/point_cloud/point_cloud_updater.h diff --git a/modules/dreamview/backend/BUILD b/modules/dreamview/backend/BUILD index 29de4ac6fe..5192886753 100644 --- a/modules/dreamview/backend/BUILD +++ b/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", diff --git a/modules/dreamview/backend/dreamview.cc b/modules/dreamview/backend/dreamview.cc index 25d17c0fe0..041c34c786 100644 --- a/modules/dreamview/backend/dreamview.cc +++ b/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(); } diff --git a/modules/dreamview/backend/dreamview.h b/modules/dreamview/backend/dreamview.h index cf525ed0a1..d3f31133c8 100644 --- a/modules/dreamview/backend/dreamview.h +++ b/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 sim_world_updater_; + std::unique_ptr point_cloud_updater_; std::unique_ptr server_; std::unique_ptr sim_control_; std::unique_ptr websocket_; std::unique_ptr map_ws_; + std::unique_ptr point_cloud_ws_; std::unique_ptr image_; std::unique_ptr map_service_; std::unique_ptr hmi_; diff --git a/modules/dreamview/backend/point_cloud/BUILD b/modules/dreamview/backend/point_cloud/BUILD new file mode 100644 index 0000000000..6bf5b78e47 --- /dev/null +++ b/modules/dreamview/backend/point_cloud/BUILD @@ -0,0 +1,27 @@ +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() diff --git a/modules/dreamview/backend/point_cloud/point_cloud_updater.cc b/modules/dreamview/backend/point_cloud/point_cloud_updater.cc new file mode 100644 index 0000000000..f264a826bd --- /dev/null +++ b/modules/dreamview/backend/point_cloud/point_cloud_updater.cc @@ -0,0 +1,99 @@ +/****************************************************************************** + * 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 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 lock(mutex_); + point_cloud_str_ = data; +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/point_cloud/point_cloud_updater.h b/modules/dreamview/backend/point_cloud/point_cloud_updater.h new file mode 100644 index 0000000000..3b0ffd0e86 --- /dev/null +++ b/modules/dreamview/backend/point_cloud/point_cloud_updater.h @@ -0,0 +1,78 @@ +/****************************************************************************** + * 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 + +#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_ diff --git a/modules/dreamview/conf/adapter.conf b/modules/dreamview/conf/adapter.conf index a264225f42..721666fc41 100644 --- a/modules/dreamview/conf/adapter.conf +++ b/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 -- GitLab