360 {
363 return false;
364 }
366 return false;
367 }
368
371
374 auto apollo_reader_1 =
375 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
376
379 auto apollo_reader_2 =
380 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
381
382 auto apollo_blocker_1 =
385 auto apollo_blocker_2 =
388
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);
443
444 return true;
445}
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_1
optional string ros_topic_name_0
optional string ros_topic_name_1