Apollo 10.0
自动驾驶开放平台
point_cloud_updater.cc
浏览该文件的文档.
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
18
19#include <utility>
20#include <vector>
21
22#include "nlohmann/json.hpp"
23#include "pcl/filters/voxel_grid.h"
24#include "yaml-cpp/yaml.h"
25
26#include "modules/dreamview/proto/point_cloud.pb.h"
27
28#include "cyber/common/file.h"
29#include "cyber/common/log.h"
30#include "cyber/time/clock.h"
33namespace apollo {
34namespace dreamview {
35
39using Json = nlohmann::json;
40
41boost::shared_mutex PointCloudUpdater::mutex_;
42
44 : UpdaterWithChannelsBase({"PointCloud", "PerceptionEdgeInfo"},
45 {"sensor", "edge"}),
46 node_(cyber::CreateNode("point_cloud")),
47 websocket_(websocket) {
48 localization_reader_ = node_->CreateReader<LocalizationEstimate>(
49 FLAGS_localization_topic,
50 [this](const std::shared_ptr<LocalizationEstimate> &msg) {
51 UpdateLocalizationTime(msg);
52 });
53}
54
56
57PointCloudChannelUpdater* PointCloudUpdater::GetPointCloudChannelUpdater(
58 const std::string &channel_name) {
59 std::lock_guard<std::mutex> lck(channel_updater_map_mutex_);
60 if (channel_updaters_.find(channel_name) == channel_updaters_.end()) {
61 channel_updaters_[channel_name] =
62 new PointCloudChannelUpdater(channel_name);
63 if (channel_name == FLAGS_perception_edge_info_topic) {
64 channel_updaters_[channel_name]->perception_edge_reader_ =
65 node_->CreateReader<PerceptionEdgeInfo>(
66 channel_name,
67 [channel_name,
68 this](const std::shared_ptr<PerceptionEdgeInfo> &msg) {
69 UpdatePerceptionEdge(msg, channel_name);
70 });
71 } else {
72 channel_updaters_[channel_name]->point_cloud_reader_ =
73 node_->CreateReader<drivers::PointCloud>(
74 channel_name,
75 [channel_name,
76 this](const std::shared_ptr<drivers::PointCloud> &msg) {
77 UpdatePointCloud(msg, channel_name);
78 });
79 }
80 }
81 return channel_updaters_[channel_name];
82}
83
84void PointCloudUpdater::StartStream(const double &time_interval_ms,
85 const std::string &channel_name,
86 nlohmann::json *subscribe_param) {
87 if (channel_name.empty()) {
88 AERROR << "Failed to subscribe channel for channel is empty!";
89 return;
90 }
91 if (std::find(channels_.begin(), channels_.end(), channel_name) ==
92 channels_.end()) {
93 AERROR << "Failed to subscribe point cloud updater, for channel: "
94 << channel_name << " is invalid.";
95 return;
96 }
97 if (time_interval_ms > 0) {
99 GetPointCloudChannelUpdater(channel_name);
100 updater->timer_.reset(new cyber::Timer(
101 time_interval_ms,
102 [channel_name, this]() { this->OnTimer(channel_name); }, false));
103 updater->timer_->Start();
104 } else {
105 this->OnTimer(channel_name);
106 }
107}
108void PointCloudUpdater::Stop() { enabled_ = false; }
109void PointCloudUpdater::OnTimer(const std::string &channel_name) {
110 PublishMessage(channel_name);
111}
112void PointCloudUpdater::StopStream(const std::string& channel_name) {
113 if (channel_name.empty()) {
114 AERROR << "Failed to unsubscribe channel for channel is empty!";
115 return;
116 }
117 PointCloudChannelUpdater *updater = GetPointCloudChannelUpdater(channel_name);
118 if (updater->timer_) {
119 updater->timer_->Stop();
120 }
121 // 回到初始值
122 updater->last_point_cloud_time_ = 0.0;
123 updater->point_cloud_str_ = "";
124 updater->future_ready_ = true;
125}
126void PointCloudUpdater::PublishMessage(const std::string &channel_name) {
127 PointCloudChannelUpdater *updater = GetPointCloudChannelUpdater(channel_name);
128 std::string to_send;
129 // the channel has no data input, clear the sending object.
130 if ((updater->point_cloud_reader_ &&
131 !updater->point_cloud_reader_->HasWriter()) ||
132 (updater->perception_edge_reader_ &&
133 !updater->perception_edge_reader_->HasWriter())) {
134 updater->last_point_cloud_time_ = 0.0;
135 updater->point_cloud_str_ = "";
136 to_send = "";
137 } else {
138 if (updater->point_cloud_str_ != "" &&
139 std::fabs(last_localization_time_ - updater->last_point_cloud_time_) >
140 2.0) {
141 boost::unique_lock<boost::shared_mutex> writer_lock(mutex_);
142 updater->point_cloud_str_ = "";
143 }
144 {
145 boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
146 to_send = updater->point_cloud_str_;
147 }
148 }
149 StreamData stream_data;
150 std::string stream_data_string;
151 stream_data.set_action("stream");
152 stream_data.set_data_name("pointcloud");
153 stream_data.set_channel_name(channel_name);
154 std::vector<uint8_t> byte_data(to_send.begin(), to_send.end());
155 stream_data.set_data(&(byte_data[0]), byte_data.size());
156 stream_data.set_type("pointcloud");
157 stream_data.SerializeToString(&stream_data_string);
158 websocket_->BroadcastBinaryData(stream_data_string);
159}
160
161void PointCloudUpdater::GetChannelMsg(std::vector<std::string> *channels) {
162 enabled_ = true;
163 GetChannelMsgWithFilter(channels);
164}
165
166pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudUpdater::ConvertPCLPointCloud(
167 const std::shared_ptr<drivers::PointCloud> &point_cloud,
168 const std::string &channel_name) {
169 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr(
170 new pcl::PointCloud<pcl::PointXYZ>);
171 pcl_ptr->width = point_cloud->width();
172 pcl_ptr->height = point_cloud->height();
173 pcl_ptr->is_dense = false;
174
175 if (point_cloud->width() * point_cloud->height() !=
176 static_cast<unsigned int>(point_cloud->point_size())) {
177 pcl_ptr->width = 1;
178 pcl_ptr->height = point_cloud->point_size();
179 }
180 pcl_ptr->points.resize(point_cloud->point_size());
181
182 for (size_t i = 0; i < pcl_ptr->points.size(); ++i) {
183 const auto &point = point_cloud->point(static_cast<int>(i));
184 pcl_ptr->points[i].x = point.x();
185 pcl_ptr->points[i].y = point.y();
186 pcl_ptr->points[i].z = point.z();
187 }
188
189 TransformPointCloud(pcl_ptr, point_cloud->header().frame_id());
190
191 return pcl_ptr;
192}
193
194void PointCloudUpdater::UpdatePerceptionEdge(
195 const std::shared_ptr<PerceptionEdgeInfo> &perception_edge,
196 const std::string &channel_name) {
197 PointCloudChannelUpdater *updater = GetPointCloudChannelUpdater(channel_name);
198 if (perception_edge->header().has_timestamp_sec()) {
199 updater->last_point_cloud_time_ = perception_edge->header().timestamp_sec();
200 }
201 if (std::fabs(last_localization_time_ - updater->last_point_cloud_time_) >
202 0.1) {
203 AWARN << "skipping outdated perception edge data";
204 return;
205 }
206 apollo::dreamview::PointCloud point_cloud_pb;
207 point_cloud_pb.set_is_edge(true);
208 for (size_t idx = 0; idx < perception_edge->edge_point_size(); ++idx) {
209 auto pt = perception_edge->edge_point(idx);
210 if (!std::isnan(pt.x()) && !std::isnan(pt.y()) && !std::isnan(pt.z())) {
211 point_cloud_pb.add_num(pt.x());
212 point_cloud_pb.add_num(pt.y());
213 point_cloud_pb.add_num(pt.z());
214 }
215 }
216 {
217 boost::unique_lock<boost::shared_mutex> writer_lock(mutex_);
218 point_cloud_pb.SerializeToString(&updater->point_cloud_str_);
219 }
220}
221
222void PointCloudUpdater::UpdatePointCloud(
223 const std::shared_ptr<drivers::PointCloud> &point_cloud,
224 const std::string& channel_name) {
225 PointCloudChannelUpdater *updater = GetPointCloudChannelUpdater(channel_name);
226 if (point_cloud->header().has_timestamp_sec()) {
227 updater->last_point_cloud_time_ = point_cloud->header().timestamp_sec();
228 } else {
229 updater->last_point_cloud_time_ = point_cloud->measurement_time();
230 }
231 if (std::fabs(last_localization_time_ - updater->last_point_cloud_time_) >
232 0.1) {
233 AWARN << "skipping outdated point cloud data";
234 return;
235 }
236 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr;
237 // Check if last filter process has finished before processing new data.
238 if (enable_voxel_filter_) {
239 if (updater->future_ready_) {
240 updater->future_ready_ = false;
241 // transform from drivers::PointCloud to pcl::PointCloud
242 pcl_ptr = ConvertPCLPointCloud(point_cloud, channel_name);
243 std::future<void> f = cyber::Async(&PointCloudUpdater::FilterPointCloud,
244 this, pcl_ptr, channel_name);
245 updater->async_future_ = std::move(f);
246 }
247 } else {
248 pcl_ptr = ConvertPCLPointCloud(point_cloud, channel_name);
249 this->FilterPointCloud(pcl_ptr, channel_name);
250 }
251}
252
253void PointCloudUpdater::FilterPointCloud(
254 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr,
255 const std::string &channel_name) {
256 PointCloudChannelUpdater *updater = GetPointCloudChannelUpdater(channel_name);
257 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_filtered_ptr(
258 new pcl::PointCloud<pcl::PointXYZ>);
259
260 /*
261 By default, disable voxel filter since it's taking more than 500ms
262 ideally the most efficient sampling method is to
263 use per beam random sample for organized cloud(TODO)
264 */
265 if (enable_voxel_filter_) {
266 pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
267 voxel_grid.setInputCloud(pcl_ptr);
268 voxel_grid.setLeafSize(static_cast<float>(FLAGS_voxel_filter_size),
269 static_cast<float>(FLAGS_voxel_filter_size),
270 static_cast<float>(FLAGS_voxel_filter_height));
271 voxel_grid.filter(*pcl_filtered_ptr);
272 } else {
273 pcl_filtered_ptr = pcl_ptr;
274 }
275
276 apollo::dreamview::PointCloud point_cloud_pb;
277 for (size_t idx = 0; idx < pcl_filtered_ptr->size(); ++idx) {
278 pcl::PointXYZ &pt = pcl_filtered_ptr->points[idx];
279 if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z)) {
280 point_cloud_pb.add_num(pt.x);
281 point_cloud_pb.add_num(pt.y);
282 point_cloud_pb.add_num(pt.z);
283 }
284 }
285 {
286 boost::unique_lock<boost::shared_mutex> writer_lock(mutex_);
287 point_cloud_pb.SerializeToString(&updater->point_cloud_str_);
288 updater->future_ready_ = true;
289 }
290}
291
292void PointCloudUpdater::UpdateLocalizationTime(
293 const std::shared_ptr<LocalizationEstimate> &localization) {
294 last_localization_time_ = localization->header().timestamp_sec();
295}
296
297void PointCloudUpdater::TransformPointCloud(
298 pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud,
299 const std::string &frame_id) {
300 if (frame_id.empty()) {
301 AERROR << "Failed to get frame id";
302 return;
303 }
304 apollo::transform::Buffer *tf2_buffer = apollo::transform::Buffer::Instance();
305 apollo::transform::TransformStamped stamped_transform;
306 try {
307 stamped_transform = tf2_buffer->lookupTransform("localization", frame_id,
309 } catch (tf2::TransformException &ex) {
310 AERROR << ex.what();
311 AERROR << "Failed to get matrix of frame id: " << frame_id;
312 return;
313 }
314
315 auto &matrix = stamped_transform.transform();
316
317 // Define a translation vector
318 Eigen::Vector3f translation(
319 matrix.translation().x(), matrix.translation().y(),
320 matrix.translation().z()); // Replace with your actual values
321
322 // Define a quaternion for rotation
323 Eigen::Quaternionf rotation(
324 matrix.rotation().qw(), matrix.rotation().qx(), matrix.rotation().qy(),
325 matrix.rotation().qz()); // Replace with your actual values (w, x, y, z)
326 rotation.normalize(); // Ensure the quaternion is a valid unit quaternion
327
328 // Combine into a transformation matrix
329 Eigen::Affine3f transform = Eigen::Affine3f::Identity();
330 transform.translation() << translation;
331 transform.rotate(rotation);
332
333 // Transform the point cloud
334 pcl::transformPointCloud(*point_cloud, *point_cloud, transform);
335 // Define the rotation matrix for 90 degree counter-clockwise rotation in
336 // the xy-plane
337 Eigen::Matrix4f counter_transform = Eigen::Matrix4f::Identity();
338 counter_transform(0, 0) = 0;
339 counter_transform(0, 1) = 1;
340 counter_transform(1, 0) = -1;
341 counter_transform(1, 1) = 0;
342
343 // Perform the transformation
344 pcl::transformPointCloud(*point_cloud, *point_cloud, counter_transform);
345}
346
347} // namespace dreamview
348} // namespace apollo
double f
Cyber has builtin time type Time.
Definition time.h:31
Used to perform oneshot or periodic timing tasks
Definition timer.h:71
void StopStream(const std::string &channel_name="") override
Stop data flow.
void StartStream(const double &time_interval_ms, const std::string &channel_name="", nlohmann::json *subscribe_param=nullptr) override
Start data flow.
PointCloudUpdater(WebSocketHandler *websocket, SimulationWorldUpdater *sim_world_updater)
Constructor with the websocket handler.
void OnTimer(const std::string &channel_name="")
void PublishMessage(const std::string &channel_name="") override
Publish Message to dreamview frontend.
Base Class for data updater with multiply channels.
void GetChannelMsgWithFilter(std::vector< std::string > *channels)
The WebSocketHandler, built on top of CivetWebSocketHandler, is a websocket handler that handles diff...
bool BroadcastBinaryData(const std::string &data, bool skippable=false)
Sends the provided binary data to all the connected clients.
virtual TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const cyber::Time &time, const float timeout_second=0.01f) const
Get the transform between two frames by frame ID.
Definition buffer.cc:168
#define AERROR
Definition log.h:44
#define AWARN
Definition log.h:43
nlohmann::json Json
std::unique_ptr< Node > CreateNode(const std::string &node_name, const std::string &name_space)
Definition cyber.cc:33
std::string frame_id
Point cloud frame id
class register implement
Definition arena_queue.h:37
std::shared_ptr< cyber::Reader< apollo::perception::PerceptionEdgeInfo > > perception_edge_reader_
std::shared_ptr< cyber::Reader< drivers::PointCloud > > point_cloud_reader_