277 {
280 return false;
281 }
283 return false;
284 }
285
288
291 auto apollo_reader_1 =
292 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
293
296 auto apollo_reader_2 =
297 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
298
301 auto apollo_reader_3 =
302 cyber_node_->template CreateReader<InType3>(reader_cfg_3);
303
304 auto apollo_blocker_1 =
307 auto apollo_blocker_2 =
310 auto apollo_blocker_3 =
313
317
318#ifdef RCLCPP__RCLCPP_HPP_
319 auto ros_publisher_0 =
320 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
321 auto ros_publisher_1 =
322 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
323 auto ros_publisher_2 =
324 ros_node_->create_publisher<OutType2>(ros_topic_name_2, 10);
325 ros_publishers_.push_back(std::move(ros_publisher_0));
326 ros_publishers_.push_back(std::move(ros_publisher_1));
327 ros_publishers_.push_back(std::move(ros_publisher_2));
328 auto func = [
this, apollo_blocker_1, apollo_blocker_2, apollo_blocker_3,
329 ros_publisher_0, ros_publisher_1,
330 ros_publisher_2](const std::shared_ptr<InType0> in) {
331#else
332 auto func = [
this, apollo_blocker_1, apollo_blocker_2,
333 apollo_blocker_3](const std::shared_ptr<InType0> in) {
334#endif
335 if (!apollo_blocker_1->IsPublishedEmpty() &&
336 !apollo_blocker_1->IsPublishedEmpty() &&
337 !apollo_blocker_1->IsPublishedEmpty()) {
338 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
339 auto msg2 = apollo_blocker_2->GetLatestPublishedPtr();
340 auto msg3 = apollo_blocker_3->GetLatestPublishedPtr();
341 auto out_0 = std::make_shared<OutType0>();
342 auto out_1 = std::make_shared<OutType1>();
343 auto out_2 = std::make_shared<OutType2>();
344 auto in_container =
345 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
346 std::shared_ptr<InType2>, std::shared_ptr<InType3>>{
347 std::make_tuple(in, msg1, msg2, msg3)};
348 auto out_container =
349 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
350 std::shared_ptr<OutType2>>{
351 std::make_tuple(out_0, out_1, out_2)};
352 this->
ConvertMsg(in_container, out_container);
353#ifdef RCLCPP__RCLCPP_HPP_
354 ros_publisher_0->publish(*out_0);
355 ros_publisher_1->publish(*out_1);
356 ros_publisher_2->publish(*out_2);
357#endif
358 }
359 };
360 auto apollo_reader_0 =
361 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
366
367 return true;
368}
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_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