401 {
404 return false;
405 }
407 return false;
408 }
409
412
415 auto apollo_reader_1 =
416 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
417
420 auto apollo_reader_2 =
421 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
422
425 auto apollo_reader_3 =
426 cyber_node_->template CreateReader<InType3>(reader_cfg_3);
427
428 auto apollo_blocker_1 =
431 auto apollo_blocker_2 =
434 auto apollo_blocker_3 =
437
442
443#ifdef RCLCPP__RCLCPP_HPP_
444 auto ros_publisher_0 =
445 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
446 auto ros_publisher_1 =
447 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
448 auto ros_publisher_2 =
449 ros_node_->create_publisher<OutType2>(ros_topic_name_2, 10);
450 auto ros_publisher_3 =
451 ros_node_->create_publisher<OutType2>(ros_topic_name_3, 10);
452 ros_publishers_.push_back(std::move(ros_publisher_0));
453 ros_publishers_.push_back(std::move(ros_publisher_1));
454 ros_publishers_.push_back(std::move(ros_publisher_2));
455 ros_publishers_.push_back(std::move(ros_publisher_3));
456 auto func = [
this, apollo_blocker_1, apollo_blocker_2, apollo_blocker_3,
457 ros_publisher_0, ros_publisher_1, ros_publisher_2,
458 ros_publisher_3](const std::shared_ptr<InType0> in) {
459#else
460 auto func = [
this, apollo_blocker_1, apollo_blocker_2,
461 apollo_blocker_3](const std::shared_ptr<InType0> in) {
462#endif
463 if (!apollo_blocker_1->IsPublishedEmpty() &&
464 !apollo_blocker_2->IsPublishedEmpty() &&
465 !apollo_blocker_3->IsPublishedEmpty()) {
466 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
467 auto msg2 = apollo_blocker_2->GetLatestPublishedPtr();
468 auto msg3 = apollo_blocker_3->GetLatestPublishedPtr();
469 auto out_0 = std::make_shared<OutType0>();
470 auto out_1 = std::make_shared<OutType1>();
471 auto out_2 = std::make_shared<OutType2>();
472 auto out_3 = std::make_shared<OutType3>();
473 auto in_container =
474 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
475 std::shared_ptr<InType2>, std::shared_ptr<InType3>>{
476 std::make_tuple(in, msg1, msg2, msg3)};
477 auto out_container =
478 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
479 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>{
480 std::make_tuple(out_0, out_1, out_2, out_3)};
481 this->
ConvertMsg(in_container, out_container);
482#ifdef RCLCPP__RCLCPP_HPP_
483 ros_publisher_0->publish(*out_0);
484 ros_publisher_1->publish(*out_1);
485 ros_publisher_2->publish(*out_2);
486 ros_publisher_3->publish(*out_2);
487#endif
488 }
489 };
490 auto apollo_reader_0 =
491 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
496
497 return true;
498}
std::atomic< bool > init_
std::unique_ptr< apollo::cyber::Node > cyber_node_
std::vector< std::shared_ptr< apollo::cyber::ReaderBase > > apollo_readers_
ConverterConf converter_conf_
bool LoadConfig(ConverterConf *config)
static const std::shared_ptr< BlockerManager > & Instance()
optional string apollo_channel_name_2
optional string ros_topic_name_3
optional string ros_topic_name_2
optional string apollo_channel_name_0
optional string apollo_channel_name_3
optional string apollo_channel_name_1
optional string ros_topic_name_0
optional string ros_topic_name_1