Apollo 10.0
自动驾驶开放平台
convert_ros_double.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_DOUBLE_H_
18#define CYBER_ROS_APOLLO_DOUBLE_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 OutType0>
33 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
34 OutputTypes<std::shared_ptr<OutType0>>> : public MessageConverter {
35 private:
36#ifdef RCLCPP__RCLCPP_HPP_
37 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1>
38 approximate_sync_policy;
39 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
40 syncApproximate_;
41#endif
42 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
43
44 public:
47
48 bool Init() override {
50 if (!init_.load()) {
51 return false;
52 }
54 return false;
55 }
56
57 auto writer_attrs =
58 std::make_shared<apollo::cyber::proto::RoleAttributes>();
59 writer_attrs->set_channel_name(converter_conf_.apollo_channel_name_0());
60 apollo_attrs_.push_back(writer_attrs);
61 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
62 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
63
64 apollo_writer_0_ =
65 cyber_node_->template CreateWriter<OutType0>(*writer_attrs);
66#ifdef RCLCPP__RCLCPP_HPP_
67 ros_msg_subs_.push_back(
68 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
69 ros_node_, ros_topic_name_0)));
70 ros_msg_subs_.push_back(
71 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
72 ros_node_, ros_topic_name_1)));
73 syncApproximate_ = std::make_shared<
74 message_filters::Synchronizer<approximate_sync_policy>>(
75 approximate_sync_policy(10),
76 *dynamic_cast<message_filters::Subscriber<InType0>*>(
77 ros_msg_subs_[0].get()),
78 *dynamic_cast<message_filters::Subscriber<InType1>*>(
79 ros_msg_subs_[1].get()));
80 syncApproximate_->registerCallback(
82 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
83 OutputTypes<std::shared_ptr<OutType0>>>::TopicCallback,
84 this);
85
86 ros_spin_thread_ =
87 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
88#endif
89 return true;
90 }
91
92 protected:
93 virtual bool ConvertMsg(
94 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
95 OutputTypes<std::shared_ptr<OutType0>>& output) = 0;
96
97#ifdef RCLCPP__RCLCPP_HPP_
98 private:
99 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
100 std::shared_ptr<InType1> ros_msg1) {
101 auto out = std::make_shared<OutType0>();
102 typename InType0::SharedPtr internal_in_prt_0 =
103 std::make_shared<InType0>(*ros_msg0.get());
104 typename InType1::SharedPtr internal_in_prt_1 =
105 std::make_shared<InType1>(*ros_msg1.get());
106 auto in_container =
107 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
108 std::make_tuple(internal_in_prt_0, internal_in_prt_1)};
109 auto out_container =
110 OutputTypes<std::shared_ptr<OutType0>>{std::make_tuple(out)};
111 this->ConvertMsg(in_container, out_container);
112 apollo_writer_0_->Write(out);
113 }
114#endif
115};
116
117template <typename InType0, typename InType1, typename OutType0,
118 typename OutType1>
120 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
121 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>>
122 : public MessageConverter {
123 private:
124#ifdef RCLCPP__RCLCPP_HPP_
125 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1>
126 approximate_sync_policy;
127 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
128 syncApproximate_;
129#endif
130 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
131 std::shared_ptr<Writer<OutType1>> apollo_writer_1_ = nullptr;
132
133 public:
136
137 bool Init() override {
139 if (!init_.load()) {
140 return false;
141 }
143 return false;
144 }
145
146 apollo_attrs_.push_back(
147 std::make_shared<apollo::cyber::proto::RoleAttributes>());
149
150 apollo_attrs_.push_back(
151 std::make_shared<apollo::cyber::proto::RoleAttributes>());
153
154 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
155 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
156
157 apollo_writer_0_ =
158 cyber_node_->template CreateWriter<OutType0>(*apollo_attrs_[0]);
159 apollo_writer_1_ =
160 cyber_node_->template CreateWriter<OutType1>(*apollo_attrs_[1]);
161#ifdef RCLCPP__RCLCPP_HPP_
162 ros_msg_subs_.push_back(
163 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
164 ros_node_, ros_topic_name_0)));
165 ros_msg_subs_.push_back(
166 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
167 ros_node_, ros_topic_name_1)));
168 syncApproximate_ = std::make_shared<
169 message_filters::Synchronizer<approximate_sync_policy>>(
170 approximate_sync_policy(10),
171 *dynamic_cast<message_filters::Subscriber<InType0>*>(
172 ros_msg_subs_[0].get()),
173 *dynamic_cast<message_filters::Subscriber<InType1>*>(
174 ros_msg_subs_[1].get()));
175 syncApproximate_->registerCallback(
177 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
178 OutputTypes<std::shared_ptr<OutType0>,
179 std::shared_ptr<OutType1>>>::TopicCallback,
180 this);
181
182 ros_spin_thread_ =
183 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
184#endif
185 return true;
186 }
187
188 protected:
189 virtual bool ConvertMsg(
190 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
191 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>&
192 output) = 0;
193
194#ifdef RCLCPP__RCLCPP_HPP_
195 private:
196 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
197 std::shared_ptr<InType1> ros_msg1) {
198 auto out_0 = std::make_shared<OutType0>();
199 auto out_1 = std::make_shared<OutType1>();
200 typename InType0::SharedPtr internal_in_prt_0 =
201 std::make_shared<InType0>(*ros_msg0.get());
202 typename InType1::SharedPtr internal_in_prt_1 =
203 std::make_shared<InType1>(*ros_msg1.get());
204 auto in_container =
205 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
206 std::make_tuple(internal_in_prt_0, internal_in_prt_1)};
207 auto out_container =
208 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>{
209 std::make_tuple(out_0, out_1)};
210 this->ConvertMsg(in_container, out_container);
211 apollo_writer_0_->Write(out_0);
212 apollo_writer_1_->Write(out_1);
213 }
214#endif
215};
216
217template <typename InType0, typename InType1, typename OutType0,
218 typename OutType1, typename OutType2>
220 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
221 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
222 std::shared_ptr<OutType2>>> : public MessageConverter {
223 private:
224#ifdef RCLCPP__RCLCPP_HPP_
225 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1>
226 approximate_sync_policy;
227 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
228 syncApproximate_;
229#endif
230 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
231 std::shared_ptr<Writer<OutType1>> apollo_writer_1_ = nullptr;
232 std::shared_ptr<Writer<OutType2>> apollo_writer_2_ = nullptr;
233
234 public:
237
238 bool Init() override {
240 if (!init_.load()) {
241 return false;
242 }
244 return false;
245 }
246
247 apollo_attrs_.push_back(
248 std::make_shared<apollo::cyber::proto::RoleAttributes>());
250
251 apollo_attrs_.push_back(
252 std::make_shared<apollo::cyber::proto::RoleAttributes>());
254
255 apollo_attrs_.push_back(
256 std::make_shared<apollo::cyber::proto::RoleAttributes>());
258
259 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
260 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
261
262 apollo_writer_0_ =
263 cyber_node_->template CreateWriter<OutType0>(*apollo_attrs_[0]);
264 apollo_writer_1_ =
265 cyber_node_->template CreateWriter<OutType1>(*apollo_attrs_[1]);
266 apollo_writer_2_ =
267 cyber_node_->template CreateWriter<OutType2>(*apollo_attrs_[2]);
268#ifdef RCLCPP__RCLCPP_HPP_
269 ros_msg_subs_.push_back(
270 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
271 ros_node_, ros_topic_name_0)));
272 ros_msg_subs_.push_back(
273 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
274 ros_node_, ros_topic_name_1)));
275 syncApproximate_ = std::make_shared<
276 message_filters::Synchronizer<approximate_sync_policy>>(
277 approximate_sync_policy(10),
278 *dynamic_cast<message_filters::Subscriber<InType0>*>(
279 ros_msg_subs_[0].get()),
280 *dynamic_cast<message_filters::Subscriber<InType1>*>(
281 ros_msg_subs_[1].get()));
282 syncApproximate_->registerCallback(
284 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
285 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
286 std::shared_ptr<OutType2>>>::TopicCallback,
287 this);
288
289 ros_spin_thread_ =
290 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
291#endif
292 return true;
293 }
294
295 protected:
296 virtual bool ConvertMsg(
297 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
298 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
299 std::shared_ptr<OutType2>>& output) = 0;
300
301#ifdef RCLCPP__RCLCPP_HPP_
302 private:
303 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
304 std::shared_ptr<InType1> ros_msg1) {
305 auto out_0 = std::make_shared<OutType0>();
306 auto out_1 = std::make_shared<OutType1>();
307 auto out_2 = std::make_shared<OutType2>();
308 typename InType0::SharedPtr internal_in_prt_0 =
309 std::make_shared<InType0>(*ros_msg0.get());
310 typename InType1::SharedPtr internal_in_prt_1 =
311 std::make_shared<InType1>(*ros_msg1.get());
312 auto in_container =
313 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
314 std::make_tuple(internal_in_prt_0, internal_in_prt_1)};
315 auto out_container =
316 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
317 std::shared_ptr<OutType2>>{
318 std::make_tuple(out_0, out_1, out_2)};
319 this->ConvertMsg(in_container, out_container);
320 apollo_writer_0_->Write(out_0);
321 apollo_writer_1_->Write(out_1);
322 apollo_writer_2_->Write(out_2);
323 }
324#endif
325};
326
327template <typename InType0, typename InType1, typename OutType0,
328 typename OutType1, typename OutType2, typename OutType3>
330 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
331 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
332 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>>
333 : public MessageConverter {
334 private:
335#ifdef RCLCPP__RCLCPP_HPP_
336 typedef message_filters::sync_policies::ApproximateTime<InType0, InType1>
337 approximate_sync_policy;
338 std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>>
339 syncApproximate_;
340#endif
341 std::shared_ptr<Writer<OutType0>> apollo_writer_0_ = nullptr;
342 std::shared_ptr<Writer<OutType1>> apollo_writer_1_ = nullptr;
343 std::shared_ptr<Writer<OutType2>> apollo_writer_2_ = nullptr;
344 std::shared_ptr<Writer<OutType3>> apollo_writer_3_ = nullptr;
345
346 public:
349
350 bool Init() override {
352 if (!init_.load()) {
353 return false;
354 }
356 return false;
357 }
358
359 apollo_attrs_.push_back(
360 std::make_shared<apollo::cyber::proto::RoleAttributes>());
362
363 apollo_attrs_.push_back(
364 std::make_shared<apollo::cyber::proto::RoleAttributes>());
366
367 apollo_attrs_.push_back(
368 std::make_shared<apollo::cyber::proto::RoleAttributes>());
370
371 apollo_attrs_.push_back(
372 std::make_shared<apollo::cyber::proto::RoleAttributes>());
374
375 auto ros_topic_name_0 = converter_conf_.ros_topic_name_0();
376 auto ros_topic_name_1 = converter_conf_.ros_topic_name_1();
377
378 apollo_writer_0_ =
379 cyber_node_->template CreateWriter<OutType0>(*apollo_attrs_[0]);
380 apollo_writer_1_ =
381 cyber_node_->template CreateWriter<OutType1>(*apollo_attrs_[1]);
382 apollo_writer_2_ =
383 cyber_node_->template CreateWriter<OutType2>(*apollo_attrs_[2]);
384 apollo_writer_3_ =
385 cyber_node_->template CreateWriter<OutType3>(*apollo_attrs_[3]);
386#ifdef RCLCPP__RCLCPP_HPP_
387 ros_msg_subs_.push_back(
388 std::move(std::make_shared<message_filters::Subscriber<InType0>>(
389 ros_node_, ros_topic_name_0)));
390 ros_msg_subs_.push_back(
391 std::move(std::make_shared<message_filters::Subscriber<InType1>>(
392 ros_node_, ros_topic_name_1)));
393 syncApproximate_ = std::make_shared<
394 message_filters::Synchronizer<approximate_sync_policy>>(
395 approximate_sync_policy(10),
396 *dynamic_cast<message_filters::Subscriber<InType0>*>(
397 ros_msg_subs_[0].get()),
398 *dynamic_cast<message_filters::Subscriber<InType1>*>(
399 ros_msg_subs_[1].get()));
400 syncApproximate_->registerCallback(
402 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
403 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
404 std::shared_ptr<OutType2>,
405 std::shared_ptr<OutType3>>>::TopicCallback,
406 this);
407
408 ros_spin_thread_ =
409 std::make_shared<std::thread>(&MessageConverter::NodeSpin, this);
410#endif
411 return true;
412 }
413
414 protected:
415 virtual bool ConvertMsg(
416 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
417 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
418 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>&
419 output) = 0;
420
421#ifdef RCLCPP__RCLCPP_HPP_
422 private:
423 void TopicCallback(std::shared_ptr<InType0> ros_msg0,
424 std::shared_ptr<InType1> ros_msg1) {
425 auto out_0 = std::make_shared<OutType0>();
426 auto out_1 = std::make_shared<OutType1>();
427 auto out_2 = std::make_shared<OutType2>();
428 auto out_3 = std::make_shared<OutType3>();
429 typename InType0::SharedPtr internal_in_prt_0 =
430 std::make_shared<InType0>(*ros_msg0.get());
431 typename InType1::SharedPtr internal_in_prt_1 =
432 std::make_shared<InType1>(*ros_msg1.get());
433 auto in_container =
434 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
435 std::make_tuple(internal_in_prt_0, internal_in_prt_1)};
436 auto out_container =
437 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
438 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>{
439 std::make_tuple(out_0, out_1, out_2, out_3)};
440 this->ConvertMsg(in_container, out_container);
441 apollo_writer_0_->Write(out_0);
442 apollo_writer_1_->Write(out_1);
443 apollo_writer_2_->Write(out_2);
444 apollo_writer_3_->Write(out_3);
445 }
446#endif
447};
448
449} // namespace cyber
450} // namespace apollo
451
452#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 > > &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 > > &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 > > &input, OutputTypes< std::shared_ptr< OutType0 > > &output)=0
virtual bool ConvertMsg(InputTypes< std::shared_ptr< InType0 >, std::shared_ptr< InType1 > > &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