Apollo 10.0
自动驾驶开放平台
pri_sec_fusion_component.h
浏览该文件的文档.
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
17#pragma once
18
19#include <memory>
20#include <string>
21#include <vector>
22
23#include "Eigen/Eigen"
24
25// Eigen 3.3.7: #define ALIVE (0)
26// fastrtps: enum ChangeKind_t { ALIVE, ... };
27#if defined(ALIVE)
28#undef ALIVE
29#endif
30
31#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h"
32#include "modules/drivers/lidar/fusion/proto/fusion_config.pb.h"
33
34#include "cyber/cyber.h"
36
37namespace apollo {
38namespace drivers {
39namespace fusion {
40
45
46class PriSecFusionComponent : public Component<PointCloud> {
47 public:
48 bool Init() override;
49 bool Proc(const std::shared_ptr<PointCloud>& point_cloud) override;
50
51 private:
52 bool Fusion(
53 std::shared_ptr<PointCloud> target,
54 std::shared_ptr<PointCloud> source);
55 bool IsExpired(
56 const std::shared_ptr<PointCloud>& target,
57 const std::shared_ptr<PointCloud>& source);
58 bool QueryPoseAffine(
59 const std::string& target_frame_id,
60 const std::string& source_frame_id,
61 Eigen::Affine3d* pose);
62 void AppendPointCloud(
63 std::shared_ptr<PointCloud> point_cloud,
64 std::shared_ptr<PointCloud> point_cloud_add,
65 const Eigen::Affine3d& pose);
66
67 FusionConfig conf_;
68 apollo::transform::Buffer* buffer_ptr_ = nullptr;
69 std::shared_ptr<Writer<PointCloud>> fusion_writer_;
70 std::vector<std::shared_ptr<Reader<PointCloud>>> readers_;
71};
72
74} // namespace fusion
75} // namespace drivers
76} // namespace apollo
Reader subscribes a channel, it has two main functions:
Definition reader.h:69
bool Proc(const std::shared_ptr< PointCloud > &point_cloud) override
#define CYBER_REGISTER_COMPONENT(name)
Definition component.h:656
class register implement
Definition arena_queue.h:37