Apollo 10.0
自动驾驶开放平台
convert_apollo_triple.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2024 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#ifndef CYBER_APOLLO_ROS_SINGLE_H_
18#define CYBER_APOLLO_ROS_SINGLE_H_
19
20#include <memory>
21#include <string>
22#include <utility>
23
25
26#include "cyber/cyber.h"
29
30namespace apollo {
31namespace cyber {
32
33template <typename InType0, typename InType1, typename InType2,
34 typename OutType0>
36 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
37 std::shared_ptr<InType2>>,
38 OutputTypes<std::shared_ptr<OutType0>>> : public MessageConverter {
39 public:
42
43 bool Init() override;
44
45 protected:
46 virtual bool ConvertMsg(
47 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
48 std::shared_ptr<InType2>>& input,
49 OutputTypes<std::shared_ptr<OutType0>>& output) = 0;
50};
51
52template <typename InType0, typename InType1, typename InType2,
53 typename OutType0>
55 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
56 std::shared_ptr<InType2>>,
59 if (!init_.load()) {
60 return false;
61 }
62 if (!LoadConfig(&converter_conf_)) {
63 return false;
64 }
65
66 apollo::cyber::ReaderConfig reader_cfg_0;
67 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
68
69 apollo::cyber::ReaderConfig reader_cfg_1;
70 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
71 auto apollo_reader_1 =
72 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
73
74 apollo::cyber::ReaderConfig reader_cfg_2;
75 reader_cfg_2.channel_name = converter_conf_.apollo_channel_name_2();
76 auto apollo_reader_2 =
77 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
78
79 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
80
81 auto apollo_blocker_1 =
82 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
83 reader_cfg_1.channel_name);
84 auto apollo_blocker_2 =
85 blocker::BlockerManager::Instance()->GetBlocker<InType2>(
86 reader_cfg_2.channel_name);
87#ifdef RCLCPP__RCLCPP_HPP_
88 auto ros_publisher_0 =
89 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
90 ros_publishers_.push_back(std::move(ros_publisher_0));
91 auto func = [this, ros_publisher_0, apollo_blocker_1,
92 apollo_blocker_2](const std::shared_ptr<InType0> in) {
93#else
94 auto func = [this, apollo_blocker_1,
95 apollo_blocker_2](const std::shared_ptr<InType0> in) {
96#endif
97 auto out = std::make_shared<OutType0>();
98 if (!apollo_blocker_1->IsPublishedEmpty() &&
99 !apollo_blocker_2->IsPublishedEmpty()) {
100 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
101 auto msg2 = apollo_blocker_2->GetLatestPublishedPtr();
102 auto in_container =
103 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
104 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
105 auto out_container =
106 OutputTypes<std::shared_ptr<OutType0>>{std::make_tuple(out)};
107 this->ConvertMsg(in_container, out_container);
108#ifdef RCLCPP__RCLCPP_HPP_
109 ros_publisher_0->publish(*out);
110#endif
111 }
112 };
113 auto apollo_reader_0 =
114 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
115 apollo_readers_.push_back(std::move(apollo_reader_0));
116 apollo_readers_.push_back(std::move(apollo_reader_1));
117 apollo_readers_.push_back(std::move(apollo_reader_2));
118
119 return true;
120}
121
122template <typename InType0, typename InType1, typename InType2,
123 typename OutType0, typename OutType1>
125 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
126 std::shared_ptr<InType2>>,
127 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>>
128 : public MessageConverter {
129 public:
132
133 bool Init() override;
134
135 protected:
136 virtual bool ConvertMsg(
137 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
138 std::shared_ptr<InType2>>& input,
139 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>&
140 output) = 0;
141};
142
143template <typename InType0, typename InType1, typename InType2,
144 typename OutType0, typename OutType1>
146 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
147 std::shared_ptr<InType2>>,
148 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>>::Init() {
150 if (!init_.load()) {
151 return false;
152 }
153 if (!LoadConfig(&converter_conf_)) {
154 return false;
155 }
156
157 apollo::cyber::ReaderConfig reader_cfg_0;
158 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
159
160 apollo::cyber::ReaderConfig reader_cfg_1;
161 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
162 auto apollo_reader_1 =
163 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
164
165 apollo::cyber::ReaderConfig reader_cfg_2;
166 reader_cfg_2.channel_name = converter_conf_.apollo_channel_name_2();
167 auto apollo_reader_2 =
168 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
169
170 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
171 std::string ros_topic_name_1 = converter_conf_.ros_topic_name_1();
172
173 auto apollo_blocker_1 =
174 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
175 reader_cfg_1.channel_name);
176 auto apollo_blocker_2 =
177 blocker::BlockerManager::Instance()->GetBlocker<InType2>(
178 reader_cfg_2.channel_name);
179
180#ifdef RCLCPP__RCLCPP_HPP_
181 auto ros_publisher_0 =
182 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
183 auto ros_publisher_1 =
184 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
185 ros_publishers_.push_back(std::move(ros_publisher_0));
186 ros_publishers_.push_back(std::move(ros_publisher_1));
187 auto func = [this, apollo_blocker_1, apollo_blocker_2, ros_publisher_0,
188 ros_publisher_1](const std::shared_ptr<InType0> in) {
189#else
190 auto func = [this, apollo_blocker_1,
191 apollo_blocker_2](const std::shared_ptr<InType0> in) {
192#endif
193 if (!apollo_blocker_1->IsPublishedEmpty() &&
194 !apollo_blocker_2->IsPublishedEmpty()) {
195 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
196 auto msg2 = apollo_blocker_2->GetLatestPublishedPtr();
197 auto out_0 = std::make_shared<OutType0>();
198 auto out_1 = std::make_shared<OutType1>();
199 auto in_container =
200 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
201 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
202 auto out_container =
203 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>{
204 std::make_tuple(out_0, out_1)};
205 this->ConvertMsg(in_container, out_container);
206#ifdef RCLCPP__RCLCPP_HPP_
207 ros_publisher_0->publish(*out_0);
208 ros_publisher_1->publish(*out_1);
209#endif
210 }
211 };
212 auto apollo_reader_0 =
213 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
214 apollo_readers_.push_back(std::move(apollo_reader_0));
215 apollo_readers_.push_back(std::move(apollo_reader_1));
216 apollo_readers_.push_back(std::move(apollo_reader_2));
217
218 return true;
219}
220
221template <typename InType0, typename InType1, typename InType2,
222 typename OutType0, typename OutType1, typename OutType2>
224 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
225 std::shared_ptr<InType2>>,
226 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
227 std::shared_ptr<OutType2>>> : public MessageConverter {
228 public:
231
232 bool Init() override;
233
234 protected:
235 virtual bool ConvertMsg(
236 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
237 std::shared_ptr<InType2>>& input,
238 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
239 std::shared_ptr<OutType2>>& output) = 0;
240};
241
242template <typename InType0, typename InType1, typename InType2,
243 typename OutType0, typename OutType1, typename OutType2>
245 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
246 std::shared_ptr<InType2>>,
247 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
248 std::shared_ptr<OutType2>>>::Init() {
250 if (!init_.load()) {
251 return false;
252 }
253 if (!LoadConfig(&converter_conf_)) {
254 return false;
255 }
256
257 apollo::cyber::ReaderConfig reader_cfg_0;
258 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
259
260 apollo::cyber::ReaderConfig reader_cfg_1;
261 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
262 auto apollo_reader_1 =
263 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
264
265 apollo::cyber::ReaderConfig reader_cfg_2;
266 reader_cfg_2.channel_name = converter_conf_.apollo_channel_name_2();
267 auto apollo_reader_2 =
268 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
269
270 auto apollo_blocker_1 =
271 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
272 reader_cfg_1.channel_name);
273 auto apollo_blocker_2 =
274 blocker::BlockerManager::Instance()->GetBlocker<InType2>(
275 reader_cfg_2.channel_name);
276
277 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
278 std::string ros_topic_name_1 = converter_conf_.ros_topic_name_1();
279 std::string ros_topic_name_2 = converter_conf_.ros_topic_name_2();
280
281#ifdef RCLCPP__RCLCPP_HPP_
282 auto ros_publisher_0 =
283 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
284 auto ros_publisher_1 =
285 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
286 auto ros_publisher_2 =
287 ros_node_->create_publisher<OutType2>(ros_topic_name_2, 10);
288 ros_publishers_.push_back(std::move(ros_publisher_0));
289 ros_publishers_.push_back(std::move(ros_publisher_1));
290 ros_publishers_.push_back(std::move(ros_publisher_2));
291 auto func = [this, apollo_blocker_1, apollo_blocker_2, ros_publisher_0,
292 ros_publisher_1,
293 ros_publisher_2](const std::shared_ptr<InType0> in) {
294#else
295 auto func = [this, apollo_blocker_1,
296 apollo_blocker_2](const std::shared_ptr<InType0> in) {
297#endif
298 if (!apollo_blocker_1->IsPublishedEmpty() &&
299 !apollo_blocker_1->IsPublishedEmpty()) {
300 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
301 auto msg2 = apollo_blocker_2->GetLatestPublishedPtr();
302 auto out_0 = std::make_shared<OutType0>();
303 auto out_1 = std::make_shared<OutType1>();
304 auto out_2 = std::make_shared<OutType2>();
305 auto in_container =
306 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
307 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
308 auto out_container =
309 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
310 std::shared_ptr<OutType2>>{
311 std::make_tuple(out_0, out_1, out_2)};
312 this->ConvertMsg(in_container, out_container);
313#ifdef RCLCPP__RCLCPP_HPP_
314 ros_publisher_0->publish(*out_0);
315 ros_publisher_1->publish(*out_1);
316 ros_publisher_2->publish(*out_2);
317#endif
318 }
319 };
320 auto apollo_reader_0 =
321 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
322 apollo_readers_.push_back(std::move(apollo_reader_0));
323 apollo_readers_.push_back(std::move(apollo_reader_1));
324 apollo_readers_.push_back(std::move(apollo_reader_2));
325
326 return true;
327}
328
329template <typename InType0, typename InType1, typename InType2,
330 typename OutType0, typename OutType1, typename OutType2,
331 typename OutType3>
333 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
334 std::shared_ptr<InType2>>,
335 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
336 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>>
337 : public MessageConverter {
338 public:
341
342 bool Init() override;
343
344 protected:
345 virtual bool ConvertMsg(
346 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
347 std::shared_ptr<InType2>>& input,
348 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
349 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>&
350 output) = 0;
351};
352
353template <typename InType0, typename InType1, typename InType2,
354 typename OutType0, typename OutType1, typename OutType2,
355 typename OutType3>
357 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
358 std::shared_ptr<InType2>>,
359 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
360 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>>::Init() {
362 if (!init_.load()) {
363 return false;
364 }
365 if (!LoadConfig(&converter_conf_)) {
366 return false;
367 }
368
369 apollo::cyber::ReaderConfig reader_cfg_0;
370 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
371
372 apollo::cyber::ReaderConfig reader_cfg_1;
373 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
374 auto apollo_reader_1 =
375 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
376
377 apollo::cyber::ReaderConfig reader_cfg_2;
378 reader_cfg_2.channel_name = converter_conf_.apollo_channel_name_2();
379 auto apollo_reader_2 =
380 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
381
382 auto apollo_blocker_1 =
383 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
384 reader_cfg_1.channel_name);
385 auto apollo_blocker_2 =
386 blocker::BlockerManager::Instance()->GetBlocker<InType2>(
387 reader_cfg_2.channel_name);
388
389 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
390 std::string ros_topic_name_1 = converter_conf_.ros_topic_name_1();
391 std::string ros_topic_name_2 = converter_conf_.ros_topic_name_2();
392 std::string ros_topic_name_3 = converter_conf_.ros_topic_name_3();
393
394#ifdef RCLCPP__RCLCPP_HPP_
395 auto ros_publisher_0 =
396 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
397 auto ros_publisher_1 =
398 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
399 auto ros_publisher_2 =
400 ros_node_->create_publisher<OutType2>(ros_topic_name_2, 10);
401 auto ros_publisher_3 =
402 ros_node_->create_publisher<OutType2>(ros_topic_name_3, 10);
403 ros_publishers_.push_back(std::move(ros_publisher_0));
404 ros_publishers_.push_back(std::move(ros_publisher_1));
405 ros_publishers_.push_back(std::move(ros_publisher_2));
406 ros_publishers_.push_back(std::move(ros_publisher_3));
407 auto func = [this, apollo_blocker_1, apollo_blocker_2, ros_publisher_0,
408 ros_publisher_1, ros_publisher_2,
409 ros_publisher_3](const std::shared_ptr<InType0> in) {
410#else
411 auto func = [this, apollo_blocker_1,
412 apollo_blocker_2](const std::shared_ptr<InType0> in) {
413#endif
414 if (!apollo_blocker_1->IsPublishedEmpty() &&
415 !apollo_blocker_2->IsPublishedEmpty()) {
416 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
417 auto msg2 = apollo_blocker_2->GetLatestPublishedPtr();
418 auto out_0 = std::make_shared<OutType0>();
419 auto out_1 = std::make_shared<OutType1>();
420 auto out_2 = std::make_shared<OutType2>();
421 auto out_3 = std::make_shared<OutType3>();
422 auto in_container =
423 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
424 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
425 auto out_container =
426 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
427 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>{
428 std::make_tuple(out_0, out_1, out_2, out_3)};
429 this->ConvertMsg(in_container, out_container);
430#ifdef RCLCPP__RCLCPP_HPP_
431 ros_publisher_0->publish(*out_0);
432 ros_publisher_1->publish(*out_1);
433 ros_publisher_2->publish(*out_2);
434 ros_publisher_3->publish(*out_2);
435#endif
436 }
437 };
438 auto apollo_reader_0 =
439 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
440 apollo_readers_.push_back(std::move(apollo_reader_0));
441 apollo_readers_.push_back(std::move(apollo_reader_1));
442 apollo_readers_.push_back(std::move(apollo_reader_2));
443
444 return true;
445}
446
447} // namespace cyber
448} // namespace apollo
449
450#endif // CYBER_APOLLO_ROS_MESSAGE_CONVERTER_H_
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 >, std::shared_ptr< OutType1 >, std::shared_ptr< OutType2 > > &output)=0
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 >, std::shared_ptr< OutType1 >, std::shared_ptr< OutType2 >, std::shared_ptr< OutType3 > > &output)=0
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 >, std::shared_ptr< OutType1 > > &output)=0
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 > > &output)=0
static const std::shared_ptr< BlockerManager > & Instance()
bool Init(const char *binary_name, const std::string &dag_info)
Definition init.cc:98
class register implement
Definition arena_queue.h:37
Definition future.h:29