Apollo 10.0
自动驾驶开放平台
pri_sec_fusion_component.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 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 <memory>
20#include <thread>
21
22namespace apollo {
23namespace drivers {
24namespace fusion {
25
27
29 if (!GetProtoConfig(&conf_)) {
30 AWARN << "Load config failed, config file" << ConfigFilePath();
31 return false;
32 }
33 buffer_ptr_ = apollo::transform::Buffer::Instance();
34
35 fusion_writer_ = node_->CreateWriter<PointCloud>(conf_.fusion_channel());
36
37 for (const auto& channel : conf_.input_channel()) {
38 auto reader = node_->CreateReader<PointCloud>(channel);
39 readers_.emplace_back(reader);
40 }
41 return true;
42}
43
45 const std::shared_ptr<PointCloud>& point_cloud) {
46 auto target = std::make_shared<PointCloud>(*point_cloud);
47 auto fusion_readers = readers_;
48 auto start_time = Time::Now().ToSecond();
49 while ((Time::Now().ToSecond() - start_time) < conf_.wait_time_s()
50 && fusion_readers.size() > 0) {
51 for (auto itr = fusion_readers.begin(); itr != fusion_readers.end();) {
52 (*itr)->Observe();
53 if (!(*itr)->Empty()) {
54 auto source = (*itr)->GetLatestObserved();
55 if (conf_.drop_expired_data() && IsExpired(target, source)) {
56 ++itr;
57 } else {
58 Fusion(target, source);
59 itr = fusion_readers.erase(itr);
60 }
61 } else {
62 ++itr;
63 }
64 }
65 std::this_thread::sleep_for(std::chrono::milliseconds(5));
66 }
67 auto diff = Time::Now().ToNanosecond() - target->header().lidar_timestamp();
68 AINFO << "Pointcloud fusion diff: " << diff / 1000000 << "ms";
69 fusion_writer_->Write(target);
70
71 return true;
72}
73
74bool PriSecFusionComponent::IsExpired(
75 const std::shared_ptr<PointCloud>& target,
76 const std::shared_ptr<PointCloud>& source) {
77 auto diff = target->measurement_time() - source->measurement_time();
78 return diff * 1000 > conf_.max_interval_ms();
79}
80
81bool PriSecFusionComponent::QueryPoseAffine(
82 const std::string& target_frame_id,
83 const std::string& source_frame_id,
84 Eigen::Affine3d* pose) {
85 std::string err_string;
86 if (!buffer_ptr_->canTransform(
87 target_frame_id,
88 source_frame_id,
89 cyber::Time(0),
90 0.02f,
91 &err_string)) {
92 AERROR << "Can not find transform. "
93 << "target_id:" << target_frame_id
94 << " frame_id:" << source_frame_id
95 << " Error info: " << err_string;
96 return false;
97 }
99 try {
100 stamped_transform = buffer_ptr_->lookupTransform(
101 target_frame_id, source_frame_id, cyber::Time(0));
102 } catch (tf2::TransformException& ex) {
103 AERROR << ex.what();
104 return false;
105 }
106 *pose = Eigen::Translation3d(
107 stamped_transform.transform().translation().x(),
108 stamped_transform.transform().translation().y(),
109 stamped_transform.transform().translation().z())
110 * Eigen::Quaterniond(
111 stamped_transform.transform().rotation().qw(),
112 stamped_transform.transform().rotation().qx(),
113 stamped_transform.transform().rotation().qy(),
114 stamped_transform.transform().rotation().qz());
115 return true;
116}
117
118void PriSecFusionComponent::AppendPointCloud(
119 std::shared_ptr<PointCloud> point_cloud,
120 std::shared_ptr<PointCloud> point_cloud_add,
121 const Eigen::Affine3d& pose) {
122 if (std::isnan(pose(0, 0))) {
123 for (auto& point : point_cloud_add->point()) {
124 PointXYZIT* point_new = point_cloud->add_point();
125 point_new->set_intensity(point.intensity());
126 point_new->set_timestamp(point.timestamp());
127 point_new->set_x(point.x());
128 point_new->set_y(point.y());
129 point_new->set_z(point.z());
130 }
131 } else {
132 for (auto& point : point_cloud_add->point()) {
133 if (std::isnan(point.x())) {
134 PointXYZIT* point_new = point_cloud->add_point();
135 point_new->set_intensity(point.intensity());
136 point_new->set_timestamp(point.timestamp());
137 point_new->set_x(point.x());
138 point_new->set_y(point.y());
139 point_new->set_z(point.z());
140 } else {
141 PointXYZIT* point_new = point_cloud->add_point();
142 point_new->set_intensity(point.intensity());
143 point_new->set_timestamp(point.timestamp());
144 Eigen::Matrix<float, 3, 1> pt(point.x(), point.y(), point.z());
145 point_new->set_x(static_cast<float>(
146 pose(0, 0) * pt.coeffRef(0)
147 + pose(0, 1) * pt.coeffRef(1)
148 + pose(0, 2) * pt.coeffRef(2) + pose(0, 3)));
149 point_new->set_y(static_cast<float>(
150 pose(1, 0) * pt.coeffRef(0)
151 + pose(1, 1) * pt.coeffRef(1)
152 + pose(1, 2) * pt.coeffRef(2) + pose(1, 3)));
153 point_new->set_z(static_cast<float>(
154 pose(2, 0) * pt.coeffRef(0)
155 + pose(2, 1) * pt.coeffRef(1)
156 + pose(2, 2) * pt.coeffRef(2) + pose(2, 3)));
157 }
158 }
159 }
160
161 int new_width = point_cloud->point_size() / point_cloud->height();
162 point_cloud->set_width(new_width);
163}
164
165bool PriSecFusionComponent::Fusion(
166 std::shared_ptr<PointCloud> target,
167 std::shared_ptr<PointCloud> source) {
168 Eigen::Affine3d pose;
169 if (QueryPoseAffine(
170 target->header().frame_id(),
171 source->header().frame_id(),
172 &pose)) {
173 AppendPointCloud(target, source, pose);
174 return true;
175 }
176 return false;
177}
178
179} // namespace fusion
180} // namespace drivers
181} // namespace apollo
bool GetProtoConfig(T *config) const
const std::string & ConfigFilePath() const
std::shared_ptr< Node > node_
Cyber has builtin time type Time.
Definition time.h:31
uint64_t ToNanosecond() const
convert time to nanosecond.
Definition time.cc:83
static Time Now()
get the current time.
Definition time.cc:57
double ToSecond() const
convert time to second.
Definition time.cc:77
bool Proc(const std::shared_ptr< PointCloud > &point_cloud) override
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const cyber::Time &target_time, const float timeout_second=0.01f, std::string *errstr=nullptr) const
Test if a transform is possible
Definition buffer.cc:195
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 AINFO
Definition log.h:42
#define AWARN
Definition log.h:43
class register implement
Definition arena_queue.h:37
optional apollo::common::Quaternion rotation
optional apollo::common::Point3D translation