Apollo 10.0
自动驾驶开放平台
convert_apollo_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_APOLLO_ROS_SINGLE_H_
18#define CYBER_APOLLO_ROS_SINGLE_H_
19
20#include <memory>
21#include <string>
22#include <utility>
23
25
26#include "cyber/cyber.h"
29
30namespace apollo {
31namespace cyber {
32
33template <typename InType0, typename InType1, typename OutType0>
35 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
36 OutputTypes<std::shared_ptr<OutType0>>> : public MessageConverter {
37 public:
40
41 bool Init() override;
42
43 protected:
44 virtual bool ConvertMsg(
45 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
46 OutputTypes<std::shared_ptr<OutType0>>& output) = 0;
47};
48
49template <typename InType0, typename InType1, typename OutType0>
51 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
54 if (!init_.load()) {
55 return false;
56 }
57 if (!LoadConfig(&converter_conf_)) {
58 return false;
59 }
60
61 apollo::cyber::ReaderConfig reader_cfg_0;
62 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
63
64 apollo::cyber::ReaderConfig reader_cfg_1;
65 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
66 auto apollo_reader_1 =
67 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
68 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
69
70 auto apollo_blocker_1 =
71 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
72 reader_cfg_1.channel_name);
73#ifdef RCLCPP__RCLCPP_HPP_
74 auto ros_publisher_0 =
75 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
76 ros_publishers_.push_back(std::move(ros_publisher_0));
77 auto func = [this, ros_publisher_0,
78 apollo_blocker_1](const std::shared_ptr<InType0> in) {
79#else
80 auto func = [this, apollo_blocker_1](const std::shared_ptr<InType0> in) {
81#endif
82 auto out = std::make_shared<OutType0>();
83 if (!apollo_blocker_1->IsPublishedEmpty()) {
84 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
85 auto in_container =
86 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
87 std::make_tuple(in, msg1)};
88 auto out_container =
89 OutputTypes<std::shared_ptr<OutType0>>{std::make_tuple(out)};
90 this->ConvertMsg(in_container, out_container);
91#ifdef RCLCPP__RCLCPP_HPP_
92 ros_publisher_0->publish(*out);
93#endif
94 }
95 };
96 auto apollo_reader_0 =
97 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
98 apollo_readers_.push_back(std::move(apollo_reader_0));
99 apollo_readers_.push_back(std::move(apollo_reader_1));
100
101 return true;
102}
103
104template <typename InType0, typename InType1, typename OutType0,
105 typename OutType1>
107 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
108 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>>
109 : public MessageConverter {
110 public:
113
114 bool Init() override;
115
116 protected:
117 virtual bool ConvertMsg(
118 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
119 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>&
120 output) = 0;
121};
122
123template <typename InType0, typename InType1, typename OutType0,
124 typename OutType1>
126 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
127 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>>::Init() {
129 if (!init_.load()) {
130 return false;
131 }
132 if (!LoadConfig(&converter_conf_)) {
133 return false;
134 }
135
136 apollo::cyber::ReaderConfig reader_cfg_0;
137 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
138
139 apollo::cyber::ReaderConfig reader_cfg_1;
140 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
141 auto apollo_reader_1 =
142 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
143
144 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
145 std::string ros_topic_name_1 = converter_conf_.ros_topic_name_1();
146
147 auto apollo_blocker_1 =
148 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
149 reader_cfg_1.channel_name);
150
151#ifdef RCLCPP__RCLCPP_HPP_
152 auto ros_publisher_0 =
153 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
154 auto ros_publisher_1 =
155 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
156 ros_publishers_.push_back(std::move(ros_publisher_0));
157 ros_publishers_.push_back(std::move(ros_publisher_1));
158 auto func = [this, apollo_blocker_1, ros_publisher_0,
159 ros_publisher_1](const std::shared_ptr<InType0> in) {
160#else
161 auto func = [this, apollo_blocker_1](const std::shared_ptr<InType0> in) {
162#endif
163 if (!apollo_blocker_1->IsPublishedEmpty()) {
164 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
165 auto out_0 = std::make_shared<OutType0>();
166 auto out_1 = std::make_shared<OutType1>();
167 auto in_container =
168 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
169 std::make_tuple(in, msg1)};
170 auto out_container =
171 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>>{
172 std::make_tuple(out_0, out_1)};
173 this->ConvertMsg(in_container, out_container);
174#ifdef RCLCPP__RCLCPP_HPP_
175 ros_publisher_0->publish(*out_0);
176 ros_publisher_1->publish(*out_1);
177#endif
178 }
179 };
180 auto apollo_reader_0 =
181 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
182 apollo_readers_.push_back(std::move(apollo_reader_0));
183 apollo_readers_.push_back(std::move(apollo_reader_1));
184
185 return true;
186}
187
188template <typename InType0, typename InType1, typename OutType0,
189 typename OutType1, typename OutType2>
191 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
192 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
193 std::shared_ptr<OutType2>>> : public MessageConverter {
194 public:
197
198 bool Init() override;
199
200 protected:
201 virtual bool ConvertMsg(
202 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
203 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
204 std::shared_ptr<OutType2>>& output) = 0;
205};
206
207template <typename InType0, typename InType1, typename OutType0,
208 typename OutType1, typename OutType2>
210 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
211 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
212 std::shared_ptr<OutType2>>>::Init() {
214 if (!init_.load()) {
215 return false;
216 }
217 if (!LoadConfig(&converter_conf_)) {
218 return false;
219 }
220
221 apollo::cyber::ReaderConfig reader_cfg_0;
222 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
223
224 apollo::cyber::ReaderConfig reader_cfg_1;
225 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
226 auto apollo_reader_1 =
227 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
228
229 auto apollo_blocker_1 =
230 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
231 reader_cfg_1.channel_name);
232
233 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
234 std::string ros_topic_name_1 = converter_conf_.ros_topic_name_1();
235 std::string ros_topic_name_2 = converter_conf_.ros_topic_name_2();
236
237#ifdef RCLCPP__RCLCPP_HPP_
238 auto ros_publisher_0 =
239 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
240 auto ros_publisher_1 =
241 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
242 auto ros_publisher_2 =
243 ros_node_->create_publisher<OutType2>(ros_topic_name_2, 10);
244 ros_publishers_.push_back(std::move(ros_publisher_0));
245 ros_publishers_.push_back(std::move(ros_publisher_1));
246 ros_publishers_.push_back(std::move(ros_publisher_2));
247 auto func = [this, apollo_blocker_1, ros_publisher_0, ros_publisher_1,
248 ros_publisher_2](const std::shared_ptr<InType0> in) {
249#else
250 auto func = [this, apollo_blocker_1](const std::shared_ptr<InType0> in) {
251#endif
252 if (!apollo_blocker_1->IsPublishedEmpty()) {
253 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
254 auto out_0 = std::make_shared<OutType0>();
255 auto out_1 = std::make_shared<OutType1>();
256 auto out_2 = std::make_shared<OutType2>();
257 auto in_container =
258 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
259 std::make_tuple(in, msg1)};
260 auto out_container =
261 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
262 std::shared_ptr<OutType2>>{
263 std::make_tuple(out_0, out_1, out_2)};
264 this->ConvertMsg(in_container, out_container);
265#ifdef RCLCPP__RCLCPP_HPP_
266 ros_publisher_0->publish(*out_0);
267 ros_publisher_1->publish(*out_1);
268 ros_publisher_2->publish(*out_2);
269#endif
270 }
271 };
272 auto apollo_reader_0 =
273 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
274 apollo_readers_.push_back(std::move(apollo_reader_0));
275 apollo_readers_.push_back(std::move(apollo_reader_1));
276
277 return true;
278}
279
280template <typename InType0, typename InType1, typename OutType0,
281 typename OutType1, typename OutType2, typename OutType3>
283 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
284 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
285 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>>
286 : public MessageConverter {
287 public:
290
291 bool Init() override;
292
293 protected:
294 virtual bool ConvertMsg(
295 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>& input,
296 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
297 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>&
298 output) = 0;
299};
300
301template <typename InType0, typename InType1, typename OutType0,
302 typename OutType1, typename OutType2, typename OutType3>
304 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>,
305 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
306 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>>::Init() {
308 if (!init_.load()) {
309 return false;
310 }
311 if (!LoadConfig(&converter_conf_)) {
312 return false;
313 }
314
315 apollo::cyber::ReaderConfig reader_cfg_0;
316 reader_cfg_0.channel_name = converter_conf_.apollo_channel_name_0();
317
318 apollo::cyber::ReaderConfig reader_cfg_1;
319 reader_cfg_1.channel_name = converter_conf_.apollo_channel_name_1();
320 auto apollo_reader_1 =
321 cyber_node_->template CreateReader<InType1>(reader_cfg_1);
322
323 auto apollo_blocker_1 =
324 blocker::BlockerManager::Instance()->GetBlocker<InType1>(
325 reader_cfg_1.channel_name);
326
327 std::string ros_topic_name_0 = converter_conf_.ros_topic_name_0();
328 std::string ros_topic_name_1 = converter_conf_.ros_topic_name_1();
329 std::string ros_topic_name_2 = converter_conf_.ros_topic_name_2();
330 std::string ros_topic_name_3 = converter_conf_.ros_topic_name_3();
331
332#ifdef RCLCPP__RCLCPP_HPP_
333 auto ros_publisher_0 =
334 ros_node_->create_publisher<OutType0>(ros_topic_name_0, 10);
335 auto ros_publisher_1 =
336 ros_node_->create_publisher<OutType1>(ros_topic_name_1, 10);
337 auto ros_publisher_2 =
338 ros_node_->create_publisher<OutType2>(ros_topic_name_2, 10);
339 auto ros_publisher_3 =
340 ros_node_->create_publisher<OutType2>(ros_topic_name_3, 10);
341 ros_publishers_.push_back(std::move(ros_publisher_0));
342 ros_publishers_.push_back(std::move(ros_publisher_1));
343 ros_publishers_.push_back(std::move(ros_publisher_2));
344 ros_publishers_.push_back(std::move(ros_publisher_3));
345 auto func = [this, apollo_blocker_1, ros_publisher_0, ros_publisher_1,
346 ros_publisher_2,
347 ros_publisher_3](const std::shared_ptr<InType0> in) {
348#else
349 auto func = [this, apollo_blocker_1](const std::shared_ptr<InType0> in) {
350#endif
351 if (!apollo_blocker_1->IsPublishedEmpty()) {
352 auto msg1 = apollo_blocker_1->GetLatestPublishedPtr();
353 auto out_0 = std::make_shared<OutType0>();
354 auto out_1 = std::make_shared<OutType1>();
355 auto out_2 = std::make_shared<OutType2>();
356 auto out_3 = std::make_shared<OutType3>();
357 auto in_container =
358 InputTypes<std::shared_ptr<InType0>, std::shared_ptr<InType1>>{
359 std::make_tuple(in, msg1)};
360 auto out_container =
361 OutputTypes<std::shared_ptr<OutType0>, std::shared_ptr<OutType1>,
362 std::shared_ptr<OutType2>, std::shared_ptr<OutType3>>{
363 std::make_tuple(out_0, out_1, out_2, out_3)};
364 this->ConvertMsg(in_container, out_container);
365#ifdef RCLCPP__RCLCPP_HPP_
366 ros_publisher_0->publish(*out_0);
367 ros_publisher_1->publish(*out_1);
368 ros_publisher_2->publish(*out_2);
369 ros_publisher_3->publish(*out_2);
370#endif
371 }
372 };
373 auto apollo_reader_0 =
374 cyber_node_->template CreateReader<InType0>(reader_cfg_0, func);
375 apollo_readers_.push_back(std::move(apollo_reader_0));
376 apollo_readers_.push_back(std::move(apollo_reader_1));
377
378 return true;
379}
380
381} // namespace cyber
382} // namespace apollo
383
384#endif // CYBER_APOLLO_ROS_MESSAGE_CONVERTER_H_
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(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 > > &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
static const std::shared_ptr< BlockerManager > & Instance()
bool Init(const char *binary_name, const std::string &dag_info)
Definition init.cc:98
class register implement
Definition arena_queue.h:37
Definition future.h:29