51 using DvCallback = std::function<bool(
const std::string &
string)>;
58 void GetUpdate(std::string *camera_update);
65 static constexpr double kImageScale = 0.6;
66 std::vector<std::string> channels_;
68 void OnCompressedImage(
69 const std::shared_ptr<apollo::drivers::CompressedImage> &image);
70 void OnImage(
const std::shared_ptr<apollo::drivers::Image> &image);
72 const std::shared_ptr<apollo::localization::LocalizationEstimate> &loc);
74 const std::shared_ptr<apollo::perception::PerceptionObstacles>
81 void GetImageLocalization(std::vector<double> *localization);
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);
90 CameraUpdate camera_update_;
92 bool enabled_ =
false;
93 bool perception_obstacle_enable_ =
false;
94 double current_image_timestamp_;
95 std::string curr_channel_name =
"";
97 std::unique_ptr<cyber::Node> node_;
99 std::deque<std::shared_ptr<apollo::localization::LocalizationEstimate>>
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_;