306 {
309 return false;
310 }
312 return false;
313 }
314
317
320 auto apollo_reader_1 =
321 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
322
323 auto apollo_blocker_1 =
326
331
332#ifdef RCLCPP__RCLCPP_HPP_
333 auto ros_publisher_0 =
334 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
335 auto ros_publisher_1 =
336 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
337 auto ros_publisher_2 =
338 ros_node_->create_publisher<OutType2>(ros_topic_name_2, 10);
339 auto ros_publisher_3 =
340 ros_node_->create_publisher<OutType2>(ros_topic_name_3, 10);
341 ros_publishers_.push_back(std::move(ros_publisher_0));
342 ros_publishers_.push_back(std::move(ros_publisher_1));
343 ros_publishers_.push_back(std::move(ros_publisher_2));
344 ros_publishers_.push_back(std::move(ros_publisher_3));
345 auto func = [
this, apollo_blocker_1, ros_publisher_0, ros_publisher_1,
346 ros_publisher_2,
347 ros_publisher_3](const std::shared_ptr<InType0> in) {
348#else
349 auto func = [
this, apollo_blocker_1](
const std::shared_ptr<InType0> in) {
350#endif
351 if (!apollo_blocker_1->IsPublishedEmpty()) {
352 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
353 auto out_0 = std::make_shared<OutType0>();
354 auto out_1 = std::make_shared<OutType1>();
355 auto out_2 = std::make_shared<OutType2>();
356 auto out_3 = std::make_shared<OutType3>();
357 auto in_container =
358 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
359 std::make_tuple(in, msg1)};
360 auto out_container =
361 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
362 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>{
363 std::make_tuple(out_0, out_1, out_2, out_3)};
364 this->
ConvertMsg(in_container, out_container);
365#ifdef RCLCPP__RCLCPP_HPP_
366 ros_publisher_0->publish(*out_0);
367 ros_publisher_1->publish(*out_1);
368 ros_publisher_2->publish(*out_2);
369 ros_publisher_3->publish(*out_2);
370#endif
371 }
372 };
373 auto apollo_reader_0 =
374 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
377
378 return true;
379}
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 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