Apollo 10.0
自动驾驶开放平台
perception_camera_updater.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2019 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 <deque>
20#include <memory>
21#include <string>
22#include <vector>
23
24#include "Eigen/Dense"
25
26#include "modules/common_msgs/localization_msgs/localization.pb.h"
27#include "modules/common_msgs/localization_msgs/pose.pb.h"
28#include "modules/common_msgs/perception_msgs/perception_obstacle.pb.h"
29#include "modules/common_msgs/sensor_msgs/sensor_image.pb.h"
30#include "modules/common_msgs/transform_msgs/transform.pb.h"
31#include "modules/dreamview/proto/camera_update.pb.h"
32
33#include "cyber/cyber.h"
37
38namespace apollo {
39namespace dreamview {
40
42 public:
50 explicit PerceptionCameraUpdater(WebSocketHandler *websocket);
51 using DvCallback = std::function<bool(const std::string &string)>;
52
53 void Start(DvCallback callback_api);
54 void Stop();
55
56 bool IsEnabled() const { return enabled_; }
57
58 void GetUpdate(std::string *camera_update);
59
60 void GetChannelMsg(std::vector<std::string> *channels);
61
62 bool ChangeChannel(std::string channel);
63
64 private:
65 static constexpr double kImageScale = 0.6;
66 std::vector<std::string> channels_;
67 void InitReaders();
68 void OnCompressedImage(
69 const std::shared_ptr<apollo::drivers::CompressedImage> &image);
70 void OnImage(const std::shared_ptr<apollo::drivers::Image> &image);
71 void OnLocalization(
72 const std::shared_ptr<apollo::localization::LocalizationEstimate> &loc);
73 void OnObstacles(
74 const std::shared_ptr<apollo::perception::PerceptionObstacles>
75 &obstacles);
81 void GetImageLocalization(std::vector<double> *localization);
82
83 apollo::transform::Buffer *tf_buffer_ = apollo::transform::Buffer::Instance();
84 bool QueryStaticTF(const std::string &frame_id,
85 const std::string &child_frame_id,
86 Eigen::Matrix4d *matrix);
87 void GetLocalization2CameraTF(std::vector<double> *localization2camera_tf);
88
89 WebSocketHandler *websocket_;
90 CameraUpdate camera_update_;
91
92 bool enabled_ = false;
93 bool perception_obstacle_enable_ = false;
94 double current_image_timestamp_;
95 std::string curr_channel_name = "";
96
97 std::unique_ptr<cyber::Node> node_;
98
99 std::deque<std::shared_ptr<apollo::localization::LocalizationEstimate>>
100 localization_queue_;
101 std::shared_ptr<cyber::Reader<drivers::Image>> perception_camera_reader_;
102 std::vector<uint8_t> image_buffer_;
103 std::vector<double> tf_static_;
104 std::vector<apollo::perception::BBox2D> bbox2ds;
105 std::vector<int32_t> obstacle_id;
106 std::vector<int32_t> obstacle_sub_type;
107 std::mutex image_mutex_;
108 std::mutex localization_mutex_;
109 std::mutex obstacle_mutex_;
110 DvCallback callback_api_;
111};
112
113} // namespace dreamview
114} // namespace apollo
std::function< bool(const std::string &string)> DvCallback
void GetChannelMsg(std::vector< std::string > *channels)
GetChannelMsg
The WebSocketHandler, built on top of CivetWebSocketHandler, is a websocket handler that handles diff...
class register implement
Definition arena_queue.h:37