248 {
251 return false;
252 }
254 return false;
255 }
256
259
262 auto apollo_reader_1 =
263 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
264
267 auto apollo_reader_2 =
268 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
269
270 auto apollo_blocker_1 =
273 auto apollo_blocker_2 =
276
280
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,
292 ros_publisher_1,
293 ros_publisher_2](const std::shared_ptr<InType0> in) {
294#else
295 auto func = [
this, apollo_blocker_1,
296 apollo_blocker_2](const std::shared_ptr<InType0> in) {
297#endif
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>();
305 auto in_container =
306 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
307 std::shared_ptr<InType2>>{std::make_tuple(in, msg1, msg2)};
308 auto out_container =
309 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
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);
317#endif
318 }
319 };
320 auto apollo_reader_0 =
321 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
325
326 return true;
327}
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_1
optional string ros_topic_name_0
optional string ros_topic_name_1