Apollo 10.0
自动驾驶开放平台
convert_ros_triple.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2024 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17#ifndef CYBER_ROS_APOLLO_TRIPLE_H_
18#define CYBER_ROS_APOLLO_TRIPLE_H_
19
20#include <memory>
21#include <utility>
22
24
25#include "cyber/cyber.h"
27
28namespace apollo {
29namespace cyber {
30
31template <typename InType0, typename InType1, typename InType2,
32 typename OutType0>
34 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
35 std::shared_ptr<InType2>>,
36 OutputTypes<std::shared_ptr<OutType0>>> : public MessageConverter {
37 private:
38#ifdef RCLCPP__RCLCPP_HPP_
39 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1,
40 InType2>
41 approximate_sync_policy;
42 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
43 syncApproximate_;
44#endif
45 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
46
47 public:
50
51 bool Init() override {
53 if (!init_.load()) {
54 return false;
55 }
57 return false;
58 }
59
60 auto writer_attrs =
61 std::make_shared<apollo::cyber::proto::RoleAttributes>();
62 writer_attrs->set_channel_name(converter_conf_.apollo_channel_name_0());
63 apollo_attrs_.push_back(writer_attrs);
64 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
65 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
66 auto ros_topic_name_2 = converter_conf_.ros_topic_name_2();
67
68 apollo_writer_0_ =
69 cyber_node_->template CreateWriter<OutType0>(*writer_attrs);
70#ifdef RCLCPP__RCLCPP_HPP_
71 ros_msg_subs_.push_back(
72 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
73 ros_node_, ros_topic_name_0)));
74 ros_msg_subs_.push_back(
75 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
76 ros_node_, ros_topic_name_1)));
77 ros_msg_subs_.push_back(
78 std::move(std::make_shared<message_filters::Subscriber<InType2>>(
79 ros_node_, ros_topic_name_2)));
80 syncApproximate_ = std::make_shared<
81 message_filters::Synchronizer<approximate_sync_policy>>(
82 approximate_sync_policy(10),
83 *dynamic_cast<message_filters::Subscriber<InType0>*>(
84 ros_msg_subs_[0].get()),
85 *dynamic_cast<message_filters::Subscriber<InType1>*>(
86 ros_msg_subs_[1].get()),
87 *dynamic_cast<message_filters::Subscriber<InType2>*>(
88 ros_msg_subs_[2].get()));
89 syncApproximate_->registerCallback(
91 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
92 std::shared_ptr<InType2>>,
93 OutputTypes<std::shared_ptr<OutType0>>>::TopicCallback,
94 this);
95
96 ros_spin_thread_ =
97 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
98#endif
99 return true;
100 }
101
102 protected:
103 virtual bool ConvertMsg(
104 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
105 std::shared_ptr<InType2>>& input,
106 OutputTypes<std::shared_ptr<OutType0>>& output) = 0;
107
108#ifdef RCLCPP__RCLCPP_HPP_
109 private:
110 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
111 std::shared_ptr<InType1> ros_msg1,
112 std::shared_ptr<InType2> ros_msg2) {
113 auto out = std::make_shared<OutType0>();
114 typename InType0::SharedPtr internal_in_prt_0 =
115 std::make_shared<InType0>(*ros_msg0.get());
116 typename InType1::SharedPtr internal_in_prt_1 =
117 std::make_shared<InType1>(*ros_msg1.get());
118 typename InType2::SharedPtr internal_in_prt_2 =
119 std::make_shared<InType2>(*ros_msg2.get());
120 auto in_container =
121 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
122 std::shared_ptr<InType2>>{std::make_tuple(
123 internal_in_prt_0, internal_in_prt_1, internal_in_prt_2)};
124 auto out_container =
125 OutputTypes<std::shared_ptr<OutType0>>{std::make_tuple(out)};
126 this->ConvertMsg(in_container, out_container);
127 apollo_writer_0_->Write(out);
128 }
129#endif
130};
131
132template <typename InType0, typename InType1, typename InType2,
133 typename OutType0, typename OutType1>
135 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
136 std::shared_ptr<InType2>>,
137 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>>
138 : public MessageConverter {
139 private:
140#ifdef RCLCPP__RCLCPP_HPP_
141 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1,
142 InType2>
143 approximate_sync_policy;
144 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
145 syncApproximate_;
146#endif
147 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
148 std::shared_ptr<Writer<OutType1>> apollo_writer_1_ = nullptr;
149
150 public:
153
154 bool Init() override {
156 if (!init_.load()) {
157 return false;
158 }
160 return false;
161 }
162
163 apollo_attrs_.push_back(
164 std::make_shared<apollo::cyber::proto::RoleAttributes>());
166
167 apollo_attrs_.push_back(
168 std::make_shared<apollo::cyber::proto::RoleAttributes>());
170
171 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
172 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
173 auto ros_topic_name_2 = converter_conf_.ros_topic_name_2();
174
175 apollo_writer_0_ =
176 cyber_node_->template CreateWriter<OutType0>(*apollo_attrs_[0]);
177 apollo_writer_1_ =
178 cyber_node_->template CreateWriter<OutType1>(*apollo_attrs_[1]);
179#ifdef RCLCPP__RCLCPP_HPP_
180 ros_msg_subs_.push_back(
181 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
182 ros_node_, ros_topic_name_0)));
183 ros_msg_subs_.push_back(
184 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
185 ros_node_, ros_topic_name_1)));
186 ros_msg_subs_.push_back(
187 std::move(std::make_shared<message_filters::Subscriber<InType2>>(
188 ros_node_, ros_topic_name_2)));
189 syncApproximate_ = std::make_shared<
190 message_filters::Synchronizer<approximate_sync_policy>>(
191 approximate_sync_policy(10),
192 *dynamic_cast<message_filters::Subscriber<InType0>*>(
193 ros_msg_subs_[0].get()),
194 *dynamic_cast<message_filters::Subscriber<InType1>*>(
195 ros_msg_subs_[1].get()),
196 *dynamic_cast<message_filters::Subscriber<InType2>*>(
197 ros_msg_subs_[2].get()));
198 syncApproximate_->registerCallback(
200 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
201 std::shared_ptr<InType2>>,
202 OutputTypes<std::shared_ptr<OutType0>,
203 std::shared_ptr<OutType1>>>::TopicCallback,
204 this);
205
206 ros_spin_thread_ =
207 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
208#endif
209 return true;
210 }
211
212 protected:
213 virtual bool ConvertMsg(
214 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
215 std::shared_ptr<InType2>>& input,
216 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>&
217 output) = 0;
218
219#ifdef RCLCPP__RCLCPP_HPP_
220 private:
221 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
222 std::shared_ptr<InType1> ros_msg1,
223 std::shared_ptr<InType2> ros_msg2) {
224 auto out_0 = std::make_shared<OutType0>();
225 auto out_1 = std::make_shared<OutType1>();
226 typename InType0::SharedPtr internal_in_prt_0 =
227 std::make_shared<InType0>(*ros_msg0.get());
228 typename InType1::SharedPtr internal_in_prt_1 =
229 std::make_shared<InType1>(*ros_msg1.get());
230 typename InType2::SharedPtr internal_in_prt_2 =
231 std::make_shared<InType2>(*ros_msg2.get());
232 auto in_container =
233 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
234 std::shared_ptr<InType2>>{std::make_tuple(
235 internal_in_prt_0, internal_in_prt_1, internal_in_prt_2)};
236 auto out_container =
237 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>{
238 std::make_tuple(out_0, out_1)};
239 this->ConvertMsg(in_container, out_container);
240 apollo_writer_0_->Write(out_0);
241 apollo_writer_1_->Write(out_1);
242 }
243#endif
244};
245
246template <typename InType0, typename InType1, typename InType2,
247 typename OutType0, typename OutType1, typename OutType2>
249 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
250 std::shared_ptr<InType2>>,
251 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
252 std::shared_ptr<OutType2>>> : public MessageConverter {
253 private:
254#ifdef RCLCPP__RCLCPP_HPP_
255 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1,
256 InType2>
257 approximate_sync_policy;
258 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
259 syncApproximate_;
260#endif
261 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
262 std::shared_ptr<Writer<OutType1>> apollo_writer_1_ = nullptr;
263 std::shared_ptr<Writer<OutType2>> apollo_writer_2_ = nullptr;
264
265 public:
268
269 bool Init() override {
271 if (!init_.load()) {
272 return false;
273 }
275 return false;
276 }
277
278 apollo_attrs_.push_back(
279 std::make_shared<apollo::cyber::proto::RoleAttributes>());
281
282 apollo_attrs_.push_back(
283 std::make_shared<apollo::cyber::proto::RoleAttributes>());
285
286 apollo_attrs_.push_back(
287 std::make_shared<apollo::cyber::proto::RoleAttributes>());
289
290 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
291 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
292 auto ros_topic_name_2 = converter_conf_.ros_topic_name_2();
293
294 apollo_writer_0_ =
295 cyber_node_->template CreateWriter<OutType0>(*apollo_attrs_[0]);
296 apollo_writer_1_ =
297 cyber_node_->template CreateWriter<OutType1>(*apollo_attrs_[1]);
298 apollo_writer_2_ =
299 cyber_node_->template CreateWriter<OutType2>(*apollo_attrs_[2]);
300#ifdef RCLCPP__RCLCPP_HPP_
301 ros_msg_subs_.push_back(
302 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
303 ros_node_, ros_topic_name_0)));
304 ros_msg_subs_.push_back(
305 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
306 ros_node_, ros_topic_name_1)));
307 ros_msg_subs_.push_back(
308 std::move(std::make_shared<message_filters::Subscriber<InType2>>(
309 ros_node_, ros_topic_name_2)));
310 syncApproximate_ = std::make_shared<
311 message_filters::Synchronizer<approximate_sync_policy>>(
312 approximate_sync_policy(10),
313 *dynamic_cast<message_filters::Subscriber<InType0>*>(
314 ros_msg_subs_[0].get()),
315 *dynamic_cast<message_filters::Subscriber<InType1>*>(
316 ros_msg_subs_[1].get()),
317 *dynamic_cast<message_filters::Subscriber<InType2>*>(
318 ros_msg_subs_[2].get()));
319 syncApproximate_->registerCallback(
321 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
322 std::shared_ptr<InType2>>,
323 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
324 std::shared_ptr<OutType2>>>::TopicCallback,
325 this);
326
327 ros_spin_thread_ =
328 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
329#endif
330 return true;
331 }
332
333 protected:
334 virtual bool ConvertMsg(
335 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
336 std::shared_ptr<InType2>>& input,
337 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
338 std::shared_ptr<OutType2>>& output) = 0;
339
340#ifdef RCLCPP__RCLCPP_HPP_
341 private:
342 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
343 std::shared_ptr<InType1> ros_msg1,
344 std::shared_ptr<InType2> ros_msg2) {
345 auto out_0 = std::make_shared<OutType0>();
346 auto out_1 = std::make_shared<OutType1>();
347 auto out_2 = std::make_shared<OutType2>();
348 typename InType0::SharedPtr internal_in_prt_0 =
349 std::make_shared<InType0>(*ros_msg0.get());
350 typename InType1::SharedPtr internal_in_prt_1 =
351 std::make_shared<InType1>(*ros_msg1.get());
352 typename InType2::SharedPtr internal_in_prt_2 =
353 std::make_shared<InType2>(*ros_msg2.get());
354 auto in_container =
355 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
356 std::shared_ptr<InType2>>{std::make_tuple(
357 internal_in_prt_0, internal_in_prt_1, internal_in_prt_2)};
358 auto out_container =
359 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
360 std::shared_ptr<OutType2>>{
361 std::make_tuple(out_0, out_1, out_2)};
362 this->ConvertMsg(in_container, out_container);
363 apollo_writer_0_->Write(out_0);
364 apollo_writer_1_->Write(out_1);
365 apollo_writer_2_->Write(out_2);
366 }
367#endif
368};
369
370template <typename InType0, typename InType1, typename InType2,
371 typename OutType0, typename OutType1, typename OutType2,
372 typename OutType3>
374 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
375 std::shared_ptr<InType2>>,
376 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
377 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>>
378 : public MessageConverter {
379 private:
380#ifdef RCLCPP__RCLCPP_HPP_
381 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1>
382 approximate_sync_policy;
383 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
384 syncApproximate_;
385#endif
386 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
387 std::shared_ptr<Writer<OutType1>> apollo_writer_1_ = nullptr;
388 std::shared_ptr<Writer<OutType2>> apollo_writer_2_ = nullptr;
389 std::shared_ptr<Writer<OutType3>> apollo_writer_3_ = nullptr;
390
391 public:
394
395 bool Init() override {
397 if (!init_.load()) {
398 return false;
399 }
401 return false;
402 }
403
404 apollo_attrs_.push_back(
405 std::make_shared<apollo::cyber::proto::RoleAttributes>());
407
408 apollo_attrs_.push_back(
409 std::make_shared<apollo::cyber::proto::RoleAttributes>());
411
412 apollo_attrs_.push_back(
413 std::make_shared<apollo::cyber::proto::RoleAttributes>());
415
416 apollo_attrs_.push_back(
417 std::make_shared<apollo::cyber::proto::RoleAttributes>());
419
420 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
421 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
422 auto ros_topic_name_2 = converter_conf_.ros_topic_name_2();
423
424 apollo_writer_0_ =
425 cyber_node_->template CreateWriter<OutType0>(*apollo_attrs_[0]);
426 apollo_writer_1_ =
427 cyber_node_->template CreateWriter<OutType1>(*apollo_attrs_[1]);
428 apollo_writer_2_ =
429 cyber_node_->template CreateWriter<OutType2>(*apollo_attrs_[2]);
430 apollo_writer_3_ =
431 cyber_node_->template CreateWriter<OutType3>(*apollo_attrs_[3]);
432#ifdef RCLCPP__RCLCPP_HPP_
433 ros_msg_subs_.push_back(
434 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
435 ros_node_, ros_topic_name_0)));
436 ros_msg_subs_.push_back(
437 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
438 ros_node_, ros_topic_name_1)));
439 ros_msg_subs_.push_back(
440 std::move(std::make_shared<message_filters::Subscriber<InType2>>(
441 ros_node_, ros_topic_name_2)));
442 syncApproximate_ = std::make_shared<
443 message_filters::Synchronizer<approximate_sync_policy>>(
444 approximate_sync_policy(10),
445 *dynamic_cast<message_filters::Subscriber<InType0>*>(
446 ros_msg_subs_[0].get()),
447 *dynamic_cast<message_filters::Subscriber<InType1>*>(
448 ros_msg_subs_[1].get()),
449 *dynamic_cast<message_filters::Subscriber<InType2>*>(
450 ros_msg_subs_[2].get()));
451 syncApproximate_->registerCallback(
453 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
454 std::shared_ptr<InType2>>,
455 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
456 std::shared_ptr<OutType2>,
457 std::shared_ptr<OutType3>>>::TopicCallback,
458 this);
459
460 ros_spin_thread_ =
461 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
462#endif
463 return true;
464 }
465
466 protected:
467 virtual bool ConvertMsg(
468 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
469 std::shared_ptr<InType2>>& input,
470 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
471 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>&
472 output) = 0;
473
474#ifdef RCLCPP__RCLCPP_HPP_
475 private:
476 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
477 std::shared_ptr<InType1> ros_msg1,
478 std::shared_ptr<InType2> ros_msg2) {
479 auto out_0 = std::make_shared<OutType0>();
480 auto out_1 = std::make_shared<OutType1>();
481 auto out_2 = std::make_shared<OutType2>();
482 auto out_3 = std::make_shared<OutType3>();
483 typename InType0::SharedPtr internal_in_prt_0 =
484 std::make_shared<InType0>(*ros_msg0.get());
485 typename InType1::SharedPtr internal_in_prt_1 =
486 std::make_shared<InType1>(*ros_msg1.get());
487 typename InType2::SharedPtr internal_in_prt_2 =
488 std::make_shared<InType1>(*ros_msg2.get());
489 auto in_container =
490 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>,
491 std::shared_ptr<InType2>>{std::make_tuple(
492 internal_in_prt_0, internal_in_prt_1, internal_in_prt_2)};
493 auto out_container =
494 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
495 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>{
496 std::make_tuple(out_0, out_1, out_2, out_3)};
497 this->ConvertMsg(in_container, out_container);
498 apollo_writer_0_->Write(out_0);
499 apollo_writer_1_->Write(out_1);
500 apollo_writer_2_->Write(out_2);
501 apollo_writer_3_->Write(out_3);
502 }
503#endif
504};
505
506} // namespace cyber
507} // namespace apollo
508
509#endif // CYBER_APOLLO_ROS_MESSAGE_CONVERTER_H_
std::unique_ptr< apollo::cyber::Node > cyber_node_
std::vector< std::shared_ptr< apollo::cyber::proto::RoleAttributes > > apollo_attrs_
bool LoadConfig(ConverterConf *config)
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 >, std::shared_ptr< OutType1 >, std::shared_ptr< OutType2 > > &output)=0
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 > > &output)=0
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 >, std::shared_ptr< OutType1 >, std::shared_ptr< OutType2 >, std::shared_ptr< OutType3 > > &output)=0
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 >, std::shared_ptr< InType2 > > &input, OutputTypes< std::shared_ptr< OutType0 >, std::shared_ptr< OutType1 > > &output)=0
virtual bool ConvertMsg(const std::shared_ptr< R0 > &ros_msg0, const std::shared_ptr< R1 > &ros_msg1, const std::shared_ptr< R2 > &ros_msg2, const std::shared_ptr< R3 > &ros_msg3, std::shared_ptr< M0 > &apollo_msg0, std::shared_ptr< M1 > &apollo_msg1, std::shared_ptr< M2 > &apollo_msg2, std::shared_ptr< M3 > &apollo_msg3)=0
class register implement
Definition arena_queue.h:37
Definition future.h:29