Skip to content

Commit

Permalink
Prototype (syncs correctly now)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mojomex
mojomex committed Jun 20, 2024
1 parent 1db76a6 commit 8e0bfc5
Show file tree
Hide file tree
Showing 4 changed files with 102 additions and 218 deletions.
222 changes: 17 additions & 205 deletions nebula_common/include/nebula_common/aeva/types.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <functional>
#include <iomanip>
#include <memory>
#include <optional>
Expand Down Expand Up @@ -159,218 +160,29 @@ enum class TelemetryDataType : uint8_t {
kChar = 10
};

struct TelemetryData
template <typename OutT>
inline std::vector<OutT> parseNumericTelemetryData(
std::function<OutT(const uint8_t *)> f, const std::vector<uint8_t> & buffer)
{
TelemetryData() = default;
TelemetryData(const TelemetryData &) = default;
TelemetryData(TelemetryData &&) = delete;
TelemetryData & operator=(const TelemetryData &) = default;
TelemetryData & operator=(TelemetryData &&) = delete;
virtual ~TelemetryData() = default;

[[nodiscard]] virtual std::string toString() const = 0;
};

template <typename StorageT, bool PrintHex>
struct NumericTelemetryData : public TelemetryData
{
[[nodiscard]] std::string toString() const override
{
std::stringstream result{};
for (const auto & val : data) {
if constexpr (PrintHex) {
result << std::hex << std::setw(sizeof(StorageT) * 2) << val << " ";
} else {
result << +val << " ";
}
}

return result.str();
}

std::vector<StorageT> data;
};

struct UInt8TelemetryData : public NumericTelemetryData<uint8_t, true>
{
explicit UInt8TelemetryData(std::vector<uint8_t> raw) { data.swap(raw); }
};

struct Int8TelemetryData : public NumericTelemetryData<int8_t, false>
{
explicit Int8TelemetryData(const std::vector<uint8_t> & raw)
{
data.reserve(raw.size());
for (const auto & byte : raw) {
data.emplace_back(static_cast<int8_t>(byte));
}
}
};

struct UInt16TelemetryData : public NumericTelemetryData<uint16_t, true>
{
explicit UInt16TelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 2);
for (size_t i = 0; i < raw.size() / 2; ++i) {
data.emplace_back(load_little_u16(&raw[i * 2]));
}
}
};

struct Int16TelemetryData : public NumericTelemetryData<int16_t, false>
{
explicit Int16TelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 2);
for (size_t i = 0; i < raw.size() / 2; ++i) {
data.emplace_back(load_little_s16(&raw[i * 2]));
}
}
};

struct UInt32TelemetryData : public NumericTelemetryData<uint32_t, true>
{
explicit UInt32TelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 4);
for (size_t i = 0; i < raw.size() / 4; ++i) {
data.emplace_back(load_little_u32(&raw[i * 4]));
}
}
};

struct Int32TelemetryData : public NumericTelemetryData<int32_t, false>
{
explicit Int32TelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 4);
for (size_t i = 0; i < raw.size() / 4; ++i) {
data.emplace_back(load_little_s32(&raw[i * 4]));
}
size_t type_size = sizeof(OutT);
if (buffer.size() % type_size != 0) {
throw std::runtime_error("Buffer length is not divisible by requested type's size");
}
};

struct UInt64TelemetryData : public NumericTelemetryData<uint64_t, true>
{
explicit UInt64TelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 8);
for (size_t i = 0; i < raw.size() / 8; ++i) {
data.emplace_back(load_little_u64(&raw[i * 8]));
}
size_t n_entries = buffer.size() / type_size;
std::vector<OutT> result{};
result.reserve(n_entries);
for (size_t i = 0; i < n_entries; ++i) {
result.emplace_back(f(&buffer[i * type_size]));
}
};

struct Int64TelemetryData : public NumericTelemetryData<int64_t, false>
{
explicit Int64TelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 8);
for (size_t i = 0; i < raw.size() / 8; ++i) {
data.emplace_back(load_little_s64(&raw[i * 8]));
}
}
};
return result;
}

struct FloatTelemetryData : public NumericTelemetryData<float, false>
inline std::string parseStringTelemetryData(const std::vector<uint8_t> & buffer)
{
explicit FloatTelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 4);
for (size_t i = 0; i < raw.size() / 4; ++i) {
float & val = data.emplace_back();
memcpy(&val, &raw[i * 4], 4);
}
}
};

struct DoubleTelemetryData : public NumericTelemetryData<double, false>
{
explicit DoubleTelemetryData(std::vector<uint8_t> raw)
{
data.reserve(raw.size() / 8);
for (size_t i = 0; i < raw.size() / 8; ++i) {
double & val = data.emplace_back();
memcpy(&val, &raw[i * 8], 8);
}
}
};

struct StringTelemetryData : public TelemetryData
{
explicit StringTelemetryData(std::vector<uint8_t> raw) : value(raw.begin(), raw.end()) {}

[[nodiscard]] std::string toString() const override { return value; }

const std::string value;
};

struct TelemetryEntry
{
TelemetryEntry(std::string key, TelemetryDataType type, std::vector<uint8_t> & data)
: data_type(type), key(std::move(key))
{
switch (data_type) {
case TelemetryDataType::kUInt8:
value = std::make_unique<UInt8TelemetryData>(data);
break;
case TelemetryDataType::kInt8:
value = std::make_unique<Int8TelemetryData>(data);
break;
case TelemetryDataType::kUInt16:
value = std::make_unique<UInt16TelemetryData>(data);
break;
case TelemetryDataType::kInt16:
value = std::make_unique<Int16TelemetryData>(data);
break;
case TelemetryDataType::kUInt32:
value = std::make_unique<UInt32TelemetryData>(data);
break;
case TelemetryDataType::kInt32:
value = std::make_unique<Int32TelemetryData>(data);
break;
case TelemetryDataType::kUInt64:
value = std::make_unique<UInt64TelemetryData>(data);
break;
case TelemetryDataType::kInt64:
value = std::make_unique<Int64TelemetryData>(data);
break;
case TelemetryDataType::kFloat:
value = std::make_unique<FloatTelemetryData>(data);
break;
case TelemetryDataType::kDouble:
value = std::make_unique<DoubleTelemetryData>(data);
break;
case TelemetryDataType::kChar:
value = std::make_unique<StringTelemetryData>(data);
break;
}
}

TelemetryDataType data_type;
std::string key;
std::unique_ptr<TelemetryData> value;

[[nodiscard]] std::string toString() const { return key + ": " + value->toString(); }
};

struct TelemetryMessage
{
std::string node_name;
std::vector<TelemetryEntry> entries;

[[nodiscard]] std::string toString() const
{
std::stringstream ss{};
ss << node_name << ":\n";
for (const auto & entry : entries) {
ss << " " << entry.toString() << "\n";
}

return ss.str();
}
};
return std::string(buffer.begin(), buffer.end());
}

enum class ReconfigRequestType : uint8_t {
kManifestRequest = 0,
Expand Down
70 changes: 64 additions & 6 deletions ...terfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,22 +17,30 @@
#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp"

#include <nebula_common/aeva/types.hpp>
#include <nlohmann/json_fwd.hpp>

#include <boost/endian/conversion.hpp>

#include <cstdint>
#include <cstring>
#include <memory>
#include <string>
#include <utility>

namespace nebula::drivers::connections
{

using nebula::drivers::aeva::parseNumericTelemetryData;
using nebula::drivers::aeva::parseStringTelemetryData;
using nebula::drivers::aeva::TelemetryDataType;
using nebula::drivers::aeva::TelemetryMessage;
using nlohmann::json;

using namespace boost::endian; // NOLINT

class TelemetryParser : public AevaParser<AevaStreamType::kTelemetry>
{
public:
using callback_t = std::function<void(const TelemetryMessage &)>;
using callback_t = std::function<void(json)>;

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

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

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

payload_size -= node_name_size;

json entries = json::object();

while (payload_size > 0) {
auto type = pull_and_parse<TelemetryDataType>(*incoming_);
auto reserved = incoming_->read(8);
Expand All @@ -69,15 +78,64 @@ class TelemetryParser : public AevaParser<AevaStreamType::kTelemetry>
auto key = std::string(entry_key_raw.begin(), entry_key_raw.end());
auto entry_data_size = pull_and_parse<uint32_t>(*incoming_);
auto entry_data_raw = incoming_->read(entry_data_size);
message.entries.emplace_back(key, type, entry_data_raw);

json value;
switch (type) {
case aeva::TelemetryDataType::kUInt8:
value = parseNumericTelemetryData<uint8_t>(
[](const auto * ref) { return *ref; }, entry_data_raw);
case aeva::TelemetryDataType::kInt8:
value = parseNumericTelemetryData<int8_t>(
[](const auto * ref) { return static_cast<int8_t>(*ref); }, entry_data_raw);
case aeva::TelemetryDataType::kUInt16:
value = parseNumericTelemetryData<uint16_t>(&load_little_u16, entry_data_raw);
case aeva::TelemetryDataType::kInt16:
value = parseNumericTelemetryData<int16_t>(&load_little_s16, entry_data_raw);
case aeva::TelemetryDataType::kUInt32:
value = parseNumericTelemetryData<uint32_t>(&load_little_u32, entry_data_raw);
case aeva::TelemetryDataType::kInt32:
value = parseNumericTelemetryData<int32_t>(&load_little_s32, entry_data_raw);
case aeva::TelemetryDataType::kUInt64:
value = parseNumericTelemetryData<uint64_t>(&load_little_u64, entry_data_raw);
case aeva::TelemetryDataType::kInt64:
value = parseNumericTelemetryData<int64_t>(&load_little_s64, entry_data_raw);
case aeva::TelemetryDataType::kFloat:
value = parseNumericTelemetryData<float>(
[](const uint8_t * ref) {
auto raw_bytes = load_little_u32(ref);
float result{};
memcpy(&result, &raw_bytes, 4);
return result;
},
entry_data_raw);
case aeva::TelemetryDataType::kDouble:
value = parseNumericTelemetryData<double>(
[](const uint8_t * ref) {
auto raw_bytes = load_little_u64(ref);
double result{};
memcpy(&result, &raw_bytes, 8);
return result;
},
entry_data_raw);
case aeva::TelemetryDataType::kChar:
value = parseStringTelemetryData(entry_data_raw);
break;
}

if (value.is_array() && value.size() == 1) {
value = value[0];
}

entries[key] = value;
payload_size -= 1 + 8 + 1 + entry_key_size + 4 + entry_data_size;
}

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

json result = {{node_name, entries}};

if (callback_) {
callback_(message);
callback_(result);
}
}

Expand Down
5 changes: 5 additions & 0 deletions nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <thread>
#include <vector>

Expand All @@ -58,6 +59,10 @@ class AevaRosWrapper final : public rclcpp::Node
drivers::connections::TelemetryParser telemetry_parser_;
drivers::connections::ReconfigParser reconfig_parser_;
drivers::AevaAries2Decoder decoder_;

std::mutex mtx_tel_;
std::string frame_sync_status_ = "<>";
std::string frame_sync_converged_ = "<>";
};

} // namespace nebula::ros
Loading

0 comments on commit 8e0bfc5

Please sign in to comment.