162 {
165 return false;
166 }
168 return false;
169 }
170
173
176 auto apollo_reader_1 =
177 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
178
181 auto apollo_reader_2 =
182 cyber_node_->template CreateReader<InType2>(reader_cfg_2);
183
186 auto apollo_reader_3 =
187 cyber_node_->template CreateReader<InType3>(reader_cfg_3);
188
191
192 auto apollo_blocker_1 =
195 auto apollo_blocker_2 =
198 auto apollo_blocker_3 =
201
202#ifdef RCLCPP__RCLCPP_HPP_
203 auto ros_publisher_0 =
204 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
205 auto ros_publisher_1 =
206 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
207 ros_publishers_.push_back(std::move(ros_publisher_0));
208 ros_publishers_.push_back(std::move(ros_publisher_1));
209 auto func = [
this, apollo_blocker_1, apollo_blocker_2, apollo_blocker_3,
210 ros_publisher_0,
211 ros_publisher_1](const std::shared_ptr<InType0> in) {
212#else
213 auto func = [
this, apollo_blocker_1, apollo_blocker_2,
214 apollo_blocker_3](const std::shared_ptr<InType0> in) {
215#endif
216 if (!apollo_blocker_1->IsPublishedEmpty() &&
217 !apollo_blocker_2->IsPublishedEmpty() &&
218 !apollo_blocker_3->IsPublishedEmpty()) {
219 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
220 auto msg2 = apollo_blocker_2->GetLatestPublishedPtr();
221 auto msg3 = apollo_blocker_3->GetLatestPublishedPtr();
222 auto out_0 = std::make_shared<OutType0>();
223 auto out_1 = std::make_shared<OutType1>();
224 auto in_container =
225 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
226 std::shared_ptr<InType2>, std::shared_ptr<InType3>>{
227 std::make_tuple(in, msg1, msg2, msg3)};
228 auto out_container =
229 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>{
230 std::make_tuple(out_0, out_1)};
231 this->
ConvertMsg(in_container, out_container);
232#ifdef RCLCPP__RCLCPP_HPP_
233 ros_publisher_0->publish(*out_0);
234 ros_publisher_1->publish(*out_1);
235#endif
236 }
237 };
238 auto apollo_reader_0 =
239 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
244
245 return true;
246}
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 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