62 if (!LoadConfig(&converter_conf_)) {
67 reader_cfg_0.
channel_name = converter_conf_.apollo_channel_name_0();
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);
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);
79 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
81 auto apollo_blocker_1 =
84 auto apollo_blocker_2 =
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) {
94 auto func = [
this, apollo_blocker_1,
95 apollo_blocker_2](
const std::shared_ptr<InType0> in) {
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();
104 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
107 this->ConvertMsg(in_container, out_container);
108#ifdef RCLCPP__RCLCPP_HPP_
109 ros_publisher_0->publish(*out);
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));
153 if (!LoadConfig(&converter_conf_)) {
158 reader_cfg_0.
channel_name = converter_conf_.apollo_channel_name_0();
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);
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);
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();
173 auto apollo_blocker_1 =
176 auto apollo_blocker_2 =
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) {
190 auto func = [
this, apollo_blocker_1,
191 apollo_blocker_2](
const std::shared_ptr<InType0> in) {
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>();
201 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
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);
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));
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>,
232 bool Init()
override;
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;
253 if (!LoadConfig(&converter_conf_)) {
258 reader_cfg_0.
channel_name = converter_conf_.apollo_channel_name_0();
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);
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);
270 auto apollo_blocker_1 =
273 auto apollo_blocker_2 =
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();
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,
293 ros_publisher_2](
const std::shared_ptr<InType0> in) {
295 auto func = [
this, apollo_blocker_1,
296 apollo_blocker_2](
const std::shared_ptr<InType0> in) {
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>();
307 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
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);
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));
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>>>
342 bool Init()
override;
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>>&
360 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>>
::Init() {
365 if (!LoadConfig(&converter_conf_)) {
370 reader_cfg_0.
channel_name = converter_conf_.apollo_channel_name_0();
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);
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);
382 auto apollo_blocker_1 =
385 auto apollo_blocker_2 =
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();
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) {
411 auto func = [
this, apollo_blocker_1,
412 apollo_blocker_2](
const std::shared_ptr<InType0> in) {
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>();
424 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
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);
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));