Skip to content

Commit 8e0bfc5

Browse files
mojomexmojomex
committed
Prototype (syncs correctly now)
1 parent 1db76a6 commit 8e0bfc5

File tree

4 files changed

+102
-218
lines changed

4 files changed

+102
-218
lines changed

‎nebula_common/include/nebula_common/aeva/types.hpp

+17-205
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include <cstddef>
2525
#include <cstdint>
2626
#include <cstring>
27+
#include <functional>
2728
#include <iomanip>
2829
#include <memory>
2930
#include <optional>
@@ -159,218 +160,29 @@ enum class TelemetryDataType : uint8_t {
159160
kChar = 10
160161
};
161162

162-
struct TelemetryData
163+
template <typename OutT>
164+
inline std::vector<OutT> parseNumericTelemetryData(
165+
std::function<OutT(const uint8_t *)> f, const std::vector<uint8_t> & buffer)
163166
{
164-
TelemetryData() = default;
165-
TelemetryData(const TelemetryData &) = default;
166-
TelemetryData(TelemetryData &&) = delete;
167-
TelemetryData & operator=(const TelemetryData &) = default;
168-
TelemetryData & operator=(TelemetryData &&) = delete;
169-
virtual ~TelemetryData() = default;
170-
171-
[[nodiscard]] virtual std::string toString() const = 0;
172-
};
173-
174-
template <typename StorageT, bool PrintHex>
175-
struct NumericTelemetryData : public TelemetryData
176-
{
177-
[[nodiscard]] std::string toString() const override
178-
{
179-
std::stringstream result{};
180-
for (const auto & val : data) {
181-
if constexpr (PrintHex) {
182-
result << std::hex << std::setw(sizeof(StorageT) * 2) << val << " ";
183-
} else {
184-
result << +val << " ";
185-
}
186-
}
187-
188-
return result.str();
189-
}
190-
191-
std::vector<StorageT> data;
192-
};
193-
194-
struct UInt8TelemetryData : public NumericTelemetryData<uint8_t, true>
195-
{
196-
explicit UInt8TelemetryData(std::vector<uint8_t> raw) { data.swap(raw); }
197-
};
198-
199-
struct Int8TelemetryData : public NumericTelemetryData<int8_t, false>
200-
{
201-
explicit Int8TelemetryData(const std::vector<uint8_t> & raw)
202-
{
203-
data.reserve(raw.size());
204-
for (const auto & byte : raw) {
205-
data.emplace_back(static_cast<int8_t>(byte));
206-
}
207-
}
208-
};
209-
210-
struct UInt16TelemetryData : public NumericTelemetryData<uint16_t, true>
211-
{
212-
explicit UInt16TelemetryData(std::vector<uint8_t> raw)
213-
{
214-
data.reserve(raw.size() / 2);
215-
for (size_t i = 0; i < raw.size() / 2; ++i) {
216-
data.emplace_back(load_little_u16(&raw[i * 2]));
217-
}
218-
}
219-
};
220-
221-
struct Int16TelemetryData : public NumericTelemetryData<int16_t, false>
222-
{
223-
explicit Int16TelemetryData(std::vector<uint8_t> raw)
224-
{
225-
data.reserve(raw.size() / 2);
226-
for (size_t i = 0; i < raw.size() / 2; ++i) {
227-
data.emplace_back(load_little_s16(&raw[i * 2]));
228-
}
229-
}
230-
};
231-
232-
struct UInt32TelemetryData : public NumericTelemetryData<uint32_t, true>
233-
{
234-
explicit UInt32TelemetryData(std::vector<uint8_t> raw)
235-
{
236-
data.reserve(raw.size() / 4);
237-
for (size_t i = 0; i < raw.size() / 4; ++i) {
238-
data.emplace_back(load_little_u32(&raw[i * 4]));
239-
}
240-
}
241-
};
242-
243-
struct Int32TelemetryData : public NumericTelemetryData<int32_t, false>
244-
{
245-
explicit Int32TelemetryData(std::vector<uint8_t> raw)
246-
{
247-
data.reserve(raw.size() / 4);
248-
for (size_t i = 0; i < raw.size() / 4; ++i) {
249-
data.emplace_back(load_little_s32(&raw[i * 4]));
250-
}
167+
size_t type_size = sizeof(OutT);
168+
if (buffer.size() % type_size != 0) {
169+
throw std::runtime_error("Buffer length is not divisible by requested type's size");
251170
}
252-
};
253171

254-
struct UInt64TelemetryData : public NumericTelemetryData<uint64_t, true>
255-
{
256-
explicit UInt64TelemetryData(std::vector<uint8_t> raw)
257-
{
258-
data.reserve(raw.size() / 8);
259-
for (size_t i = 0; i < raw.size() / 8; ++i) {
260-
data.emplace_back(load_little_u64(&raw[i * 8]));
261-
}
172+
size_t n_entries = buffer.size() / type_size;
173+
std::vector<OutT> result{};
174+
result.reserve(n_entries);
175+
for (size_t i = 0; i < n_entries; ++i) {
176+
result.emplace_back(f(&buffer[i * type_size]));
262177
}
263-
};
264178

265-
struct Int64TelemetryData : public NumericTelemetryData<int64_t, false>
266-
{
267-
explicit Int64TelemetryData(std::vector<uint8_t> raw)
268-
{
269-
data.reserve(raw.size() / 8);
270-
for (size_t i = 0; i < raw.size() / 8; ++i) {
271-
data.emplace_back(load_little_s64(&raw[i * 8]));
272-
}
273-
}
274-
};
179+
return result;
180+
}
275181

276-
struct FloatTelemetryData : public NumericTelemetryData<float, false>
182+
inline std::string parseStringTelemetryData(const std::vector<uint8_t> & buffer)
277183
{
278-
explicit FloatTelemetryData(std::vector<uint8_t> raw)
279-
{
280-
data.reserve(raw.size() / 4);
281-
for (size_t i = 0; i < raw.size() / 4; ++i) {
282-
float & val = data.emplace_back();
283-
memcpy(&val, &raw[i * 4], 4);
284-
}
285-
}
286-
};
287-
288-
struct DoubleTelemetryData : public NumericTelemetryData<double, false>
289-
{
290-
explicit DoubleTelemetryData(std::vector<uint8_t> raw)
291-
{
292-
data.reserve(raw.size() / 8);
293-
for (size_t i = 0; i < raw.size() / 8; ++i) {
294-
double & val = data.emplace_back();
295-
memcpy(&val, &raw[i * 8], 8);
296-
}
297-
}
298-
};
299-
300-
struct StringTelemetryData : public TelemetryData
301-
{
302-
explicit StringTelemetryData(std::vector<uint8_t> raw) : value(raw.begin(), raw.end()) {}
303-
304-
[[nodiscard]] std::string toString() const override { return value; }
305-
306-
const std::string value;
307-
};
308-
309-
struct TelemetryEntry
310-
{
311-
TelemetryEntry(std::string key, TelemetryDataType type, std::vector<uint8_t> & data)
312-
: data_type(type), key(std::move(key))
313-
{
314-
switch (data_type) {
315-
case TelemetryDataType::kUInt8:
316-
value = std::make_unique<UInt8TelemetryData>(data);
317-
break;
318-
case TelemetryDataType::kInt8:
319-
value = std::make_unique<Int8TelemetryData>(data);
320-
break;
321-
case TelemetryDataType::kUInt16:
322-
value = std::make_unique<UInt16TelemetryData>(data);
323-
break;
324-
case TelemetryDataType::kInt16:
325-
value = std::make_unique<Int16TelemetryData>(data);
326-
break;
327-
case TelemetryDataType::kUInt32:
328-
value = std::make_unique<UInt32TelemetryData>(data);
329-
break;
330-
case TelemetryDataType::kInt32:
331-
value = std::make_unique<Int32TelemetryData>(data);
332-
break;
333-
case TelemetryDataType::kUInt64:
334-
value = std::make_unique<UInt64TelemetryData>(data);
335-
break;
336-
case TelemetryDataType::kInt64:
337-
value = std::make_unique<Int64TelemetryData>(data);
338-
break;
339-
case TelemetryDataType::kFloat:
340-
value = std::make_unique<FloatTelemetryData>(data);
341-
break;
342-
case TelemetryDataType::kDouble:
343-
value = std::make_unique<DoubleTelemetryData>(data);
344-
break;
345-
case TelemetryDataType::kChar:
346-
value = std::make_unique<StringTelemetryData>(data);
347-
break;
348-
}
349-
}
350-
351-
TelemetryDataType data_type;
352-
std::string key;
353-
std::unique_ptr<TelemetryData> value;
354-
355-
[[nodiscard]] std::string toString() const { return key + ": " + value->toString(); }
356-
};
357-
358-
struct TelemetryMessage
359-
{
360-
std::string node_name;
361-
std::vector<TelemetryEntry> entries;
362-
363-
[[nodiscard]] std::string toString() const
364-
{
365-
std::stringstream ss{};
366-
ss << node_name << ":\n";
367-
for (const auto & entry : entries) {
368-
ss << " " << entry.toString() << "\n";
369-
}
370-
371-
return ss.str();
372-
}
373-
};
184+
return std::string(buffer.begin(), buffer.end());
185+
}
374186

375187
enum class ReconfigRequestType : uint8_t {
376188
kManifestRequest = 0,

‎nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp

+64-6
Original file line numberDiff line numberDiff line change
@@ -17,22 +17,30 @@
1717
#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp"
1818

1919
#include <nebula_common/aeva/types.hpp>
20+
#include <nlohmann/json_fwd.hpp>
21+
22+
#include <boost/endian/conversion.hpp>
2023

2124
#include <cstdint>
25+
#include <cstring>
2226
#include <memory>
2327
#include <string>
2428
#include <utility>
2529

2630
namespace nebula::drivers::connections
2731
{
2832

33+
using nebula::drivers::aeva::parseNumericTelemetryData;
34+
using nebula::drivers::aeva::parseStringTelemetryData;
2935
using nebula::drivers::aeva::TelemetryDataType;
30-
using nebula::drivers::aeva::TelemetryMessage;
36+
using nlohmann::json;
37+
38+
using namespace boost::endian; // NOLINT
3139

3240
class TelemetryParser : public AevaParser<AevaStreamType::kTelemetry>
3341
{
3442
public:
35-
using callback_t = std::function<void(const TelemetryMessage &)>;
43+
using callback_t = std::function<void(json)>;
3644

3745
explicit TelemetryParser(std::shared_ptr<PullableByteStream> incoming_byte_stream)
3846
: AevaParser<AevaStreamType::kTelemetry>(std::move(incoming_byte_stream))
@@ -44,7 +52,6 @@ class TelemetryParser : public AevaParser<AevaStreamType::kTelemetry>
4452
protected:
4553
void onMessage(const MessageHeader & message_header) override
4654
{
47-
TelemetryMessage message{};
4855
auto payload_size = pull_and_parse<uint32_t>(*incoming_);
4956

5057
auto node_name_size = pull_and_parse<uint8_t>(*incoming_);
@@ -57,10 +64,12 @@ class TelemetryParser : public AevaParser<AevaStreamType::kTelemetry>
5764
"Unexpected payload size field");
5865

5966
auto node_name_raw = incoming_->read(node_name_size);
60-
message.node_name = std::string(node_name_raw.begin(), node_name_raw.end());
67+
auto node_name = std::string(node_name_raw.begin(), node_name_raw.end());
6168

6269
payload_size -= node_name_size;
6370

71+
json entries = json::object();
72+
6473
while (payload_size > 0) {
6574
auto type = pull_and_parse<TelemetryDataType>(*incoming_);
6675
auto reserved = incoming_->read(8);
@@ -69,15 +78,64 @@ class TelemetryParser : public AevaParser<AevaStreamType::kTelemetry>
6978
auto key = std::string(entry_key_raw.begin(), entry_key_raw.end());
7079
auto entry_data_size = pull_and_parse<uint32_t>(*incoming_);
7180
auto entry_data_raw = incoming_->read(entry_data_size);
72-
message.entries.emplace_back(key, type, entry_data_raw);
7381

82+
json value;
83+
switch (type) {
84+
case aeva::TelemetryDataType::kUInt8:
85+
value = parseNumericTelemetryData<uint8_t>(
86+
[](const auto * ref) { return *ref; }, entry_data_raw);
87+
case aeva::TelemetryDataType::kInt8:
88+
value = parseNumericTelemetryData<int8_t>(
89+
[](const auto * ref) { return static_cast<int8_t>(*ref); }, entry_data_raw);
90+
case aeva::TelemetryDataType::kUInt16:
91+
value = parseNumericTelemetryData<uint16_t>(&load_little_u16, entry_data_raw);
92+
case aeva::TelemetryDataType::kInt16:
93+
value = parseNumericTelemetryData<int16_t>(&load_little_s16, entry_data_raw);
94+
case aeva::TelemetryDataType::kUInt32:
95+
value = parseNumericTelemetryData<uint32_t>(&load_little_u32, entry_data_raw);
96+
case aeva::TelemetryDataType::kInt32:
97+
value = parseNumericTelemetryData<int32_t>(&load_little_s32, entry_data_raw);
98+
case aeva::TelemetryDataType::kUInt64:
99+
value = parseNumericTelemetryData<uint64_t>(&load_little_u64, entry_data_raw);
100+
case aeva::TelemetryDataType::kInt64:
101+
value = parseNumericTelemetryData<int64_t>(&load_little_s64, entry_data_raw);
102+
case aeva::TelemetryDataType::kFloat:
103+
value = parseNumericTelemetryData<float>(
104+
[](const uint8_t * ref) {
105+
auto raw_bytes = load_little_u32(ref);
106+
float result{};
107+
memcpy(&result, &raw_bytes, 4);
108+
return result;
109+
},
110+
entry_data_raw);
111+
case aeva::TelemetryDataType::kDouble:
112+
value = parseNumericTelemetryData<double>(
113+
[](const uint8_t * ref) {
114+
auto raw_bytes = load_little_u64(ref);
115+
double result{};
116+
memcpy(&result, &raw_bytes, 8);
117+
return result;
118+
},
119+
entry_data_raw);
120+
case aeva::TelemetryDataType::kChar:
121+
value = parseStringTelemetryData(entry_data_raw);
122+
break;
123+
}
124+
125+
if (value.is_array() && value.size() == 1) {
126+
value = value[0];
127+
}
128+
129+
entries[key] = value;
74130
payload_size -= 1 + 8 + 1 + entry_key_size + 4 + entry_data_size;
75131
}
76132

77133
expect_eq(payload_size, 0, "Payload and payload size mismatch");
78134

135+
json result = {{node_name, entries}};
136+
79137
if (callback_) {
80-
callback_(message);
138+
callback_(result);
81139
}
82140
}
83141

‎nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp

+5
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
#include <memory>
4040
#include <mutex>
4141
#include <optional>
42+
#include <string>
4243
#include <thread>
4344
#include <vector>
4445

@@ -58,6 +59,10 @@ class AevaRosWrapper final : public rclcpp::Node
5859
drivers::connections::TelemetryParser telemetry_parser_;
5960
drivers::connections::ReconfigParser reconfig_parser_;
6061
drivers::AevaAries2Decoder decoder_;
62+
63+
std::mutex mtx_tel_;
64+
std::string frame_sync_status_ = "<>";
65+
std::string frame_sync_converged_ = "<>";
6166
};
6267

6368
} // namespace nebula::ros

0 commit comments

Comments
 (0)