Apollo 10.0
自动驾驶开放平台
point_cloud_updater.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2018 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
21#pragma once
22
23#include <memory>
24#include <string>
25#include <vector>
26
27#include <boost/thread/locks.hpp>
28#include <boost/thread/shared_mutex.hpp>
29
30#include "pcl/point_cloud.h"
31#include "pcl/point_types.h"
32
33#include "modules/common_msgs/localization_msgs/localization.pb.h"
34#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h"
35
36#include "cyber/common/log.h"
37#include "cyber/cyber.h"
45namespace apollo {
46namespace dreamview {
47
54 public:
62 SimulationWorldUpdater *sim_world_updater);
63
65 // dreamview callback function
66 using DvCallback = std::function<bool(const std::string &string)>;
67 static void LoadLidarHeight(const std::string &file_path);
68
72 void Start(DvCallback callback_api);
73 void Stop();
74 // The height of lidar w.r.t the ground.
75 static float lidar_height_;
76
77 // Mutex to protect concurrent access to point_cloud_str_ and lidar_height_.
78 // NOTE: Use boost until we have std version of rwlock support.
79 static boost::shared_mutex mutex_;
80
81 private:
82 void RegisterMessageHandlers();
83
84 void UpdatePointCloud(
85 const std::shared_ptr<drivers::PointCloud> &point_cloud);
86
87 void FilterPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr);
88
89 void UpdateLocalizationTime(
90 const std::shared_ptr<apollo::localization::LocalizationEstimate>
91 &localization);
92 pcl::PointCloud<pcl::PointXYZ>::Ptr ConvertPCLPointCloud(
93 const std::shared_ptr<drivers::PointCloud> &point_cloud);
94
95 void GetChannelMsg(std::vector<std::string> *channels);
96 bool ChangeChannel(const std::string &channel);
97 constexpr static float kDefaultLidarHeight = 1.91f;
98
99 std::unique_ptr<cyber::Node> node_;
100
101 WebSocketHandler *websocket_;
102
103 std::vector<std::string> channels_;
104
105 bool enabled_ = false;
106
107 // The PointCloud to be pushed to frontend.
108 std::string point_cloud_str_;
109
110 std::future<void> async_future_;
111 std::atomic<bool> future_ready_;
112
113 // Cyber messsage readers.
114 std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
115 localization_reader_;
116 std::shared_ptr<cyber::Reader<drivers::PointCloud>> point_cloud_reader_;
117 double last_point_cloud_time_ = 0.0;
118 double last_localization_time_ = 0.0;
119 SimulationWorldUpdater *simworld_updater_;
120 bool enable_voxel_filter_ = false;
121 std::string curr_channel_name = "";
122 DvCallback callback_api_;
123};
124} // namespace dreamview
125} // namespace apollo
A wrapper around WebSocketHandler to keep pushing PointCloud to frontend via websocket while handling...
void Start(DvCallback callback_api)
Starts to push PointCloud to frontend.
std::function< bool(const std::string &string)> DvCallback
static void LoadLidarHeight(const std::string &file_path)
A wrapper around SimulationWorldService and WebSocketHandler to keep pushing SimulationWorld to front...
The WebSocketHandler, built on top of CivetWebSocketHandler, is a websocket handler that handles diff...
class register implement
Definition arena_queue.h:37
Some string util functions.