Skip to content

Commit

Permalink
Merge pull request facontidavide#840 from jbendes/zcm-improvements
Browse files Browse the repository at this point in the history 
Zcm improvements
  • Loading branch information
@facontidavide
facontidavide authored Jun 13, 2023
2 parents d83f814 + 48635b5 commit 2327ec0
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 51 deletions.
82 changes: 49 additions & 33 deletions plotjuggler_plugins/PluginsZcm/dataload_zcm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,29 @@ double toDouble(const void* data) {
return static_cast<double>(*reinterpret_cast<const T*>(data));
}

struct ProcessUsr
{
vector<pair<string, double>>& numerics;
vector<pair<string, string>>& strings;
};
static void processData(const string& name, zcm_field_type_t type,
const void* data, void* usr)
{
ProcessUsr* v = (ProcessUsr*)usr;
switch (type) {
case ZCM_FIELD_INT8_T: v->numerics.emplace_back(name, toDouble<int8_t>(data)); break;
case ZCM_FIELD_INT16_T: v->numerics.emplace_back(name, toDouble<int16_t>(data)); break;
case ZCM_FIELD_INT32_T: v->numerics.emplace_back(name, toDouble<int32_t>(data)); break;
case ZCM_FIELD_INT64_T: v->numerics.emplace_back(name, toDouble<int64_t>(data)); break;
case ZCM_FIELD_BYTE: v->numerics.emplace_back(name, toDouble<uint8_t>(data)); break;
case ZCM_FIELD_FLOAT: v->numerics.emplace_back(name, toDouble<float>(data)); break;
case ZCM_FIELD_DOUBLE: v->numerics.emplace_back(name, toDouble<double>(data)); break;
case ZCM_FIELD_BOOLEAN: v->numerics.emplace_back(name, toDouble<bool>(data)); break;
case ZCM_FIELD_STRING: v->strings.emplace_back(name, string((const char*)data)); break;
case ZCM_FIELD_USER_TYPE: assert(false && "Should not be possble");
}
};

bool DataLoadZcm::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_data)
{
string filepath = info->filename.toStdString();
Expand All @@ -205,45 +228,31 @@ bool DataLoadZcm::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_data

vector<pair<string, double>> numerics;
vector<pair<string, string>> strings;
ProcessUsr usr = { numerics, strings };

auto processData = [&](const string& name, zcm_field_type_t type, const void* data){
switch (type) {
case ZCM_FIELD_INT8_T: numerics.emplace_back(name, toDouble<int8_t>(data)); break;
case ZCM_FIELD_INT16_T: numerics.emplace_back(name, toDouble<int16_t>(data)); break;
case ZCM_FIELD_INT32_T: numerics.emplace_back(name, toDouble<int32_t>(data)); break;
case ZCM_FIELD_INT64_T: numerics.emplace_back(name, toDouble<int64_t>(data)); break;
case ZCM_FIELD_BYTE: numerics.emplace_back(name, toDouble<uint8_t>(data)); break;
case ZCM_FIELD_FLOAT: numerics.emplace_back(name, toDouble<float>(data)); break;
case ZCM_FIELD_DOUBLE: numerics.emplace_back(name, toDouble<double>(data)); break;
case ZCM_FIELD_BOOLEAN: numerics.emplace_back(name, toDouble<bool>(data)); break;
case ZCM_FIELD_STRING: strings.emplace_back(name, string((const char*)data)); break;
case ZCM_FIELD_USER_TYPE: assert(false && "Should not be possble");
auto processEvent = [&](const zcm::LogEvent* evt){
if (_selected_channels.find(evt->channel) == _selected_channels.end()) return;
zcm::Introspection::processEncodedType(evt->channel,
evt->data, evt->datalen,
"/",
types, processData, &usr);
for (auto& n : usr.numerics) {
auto itr = plot_data.numeric.find(n.first);
if (itr == plot_data.numeric.end()) itr = plot_data.addNumeric(n.first);
itr->second.pushBack({ (double)evt->timestamp / 1e6, n.second });
}
for (auto& s : usr.strings) {
auto itr = plot_data.strings.find(s.first);
if (itr == plot_data.strings.end()) itr = plot_data.addStringSeries(s.first);
itr->second.pushBack({ (double)evt->timestamp / 1e6, s.second });
}
};

auto processEvent = [&](const zcm::LogEvent* evt){
if (_selected_channels.find(evt->channel) == _selected_channels.end()) return;
zcm::Introspection::processEncodedType(evt->channel,
evt->data, evt->datalen,
"/",
types, processData);
for (auto& n : numerics) {
auto itr = plot_data.numeric.find(n.first);
if (itr == plot_data.numeric.end()) itr = plot_data.addNumeric(n.first);
itr->second.pushBack({ (double)evt->timestamp / 1e6, n.second });
}
for (auto& s : strings) {
auto itr = plot_data.strings.find(s.first);
if (itr == plot_data.strings.end()) itr = plot_data.addStringSeries(s.first);
itr->second.pushBack({ (double)evt->timestamp / 1e6, s.second });
}

numerics.clear();
strings.clear();
usr.numerics.clear();
usr.strings.clear();
};

if (processInputLog(filepath, processEvent) != 0)
return false;
return false;

return true;
}
Expand Down Expand Up @@ -305,6 +314,13 @@ bool DataLoadZcm::xmlLoadState(const QDomElement& parent_element)
QDomElement selected_channels = parent_element.firstChildElement("selected_channels");
if (!selected_channels.isNull()) {
_selected_channels = deserialize(selected_channels);

QStringList selected_channels;
for (auto& c : _selected_channels)
selected_channels.push_back(QString::fromStdString(c));

QSettings settings;
settings.setValue("DataLoadZcm.selected_channels", selected_channels);
}

return true;
Expand Down
37 changes: 21 additions & 16 deletions plotjuggler_plugins/PluginsZcm/datastream_zcm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,21 +23,6 @@ double toDouble(const void* data) {

DataStreamZcm::DataStreamZcm(): _subs(nullptr), _running(false)
{
_processData = [&](const string& name, zcm_field_type_t type, const void* data){
switch (type) {
case ZCM_FIELD_INT8_T: _numerics.emplace_back(name, toDouble<int8_t>(data)); break;
case ZCM_FIELD_INT16_T: _numerics.emplace_back(name, toDouble<int16_t>(data)); break;
case ZCM_FIELD_INT32_T: _numerics.emplace_back(name, toDouble<int32_t>(data)); break;
case ZCM_FIELD_INT64_T: _numerics.emplace_back(name, toDouble<int64_t>(data)); break;
case ZCM_FIELD_BYTE: _numerics.emplace_back(name, toDouble<uint8_t>(data)); break;
case ZCM_FIELD_FLOAT: _numerics.emplace_back(name, toDouble<float>(data)); break;
case ZCM_FIELD_DOUBLE: _numerics.emplace_back(name, toDouble<double>(data)); break;
case ZCM_FIELD_BOOLEAN: _numerics.emplace_back(name, toDouble<bool>(data)); break;
case ZCM_FIELD_STRING: _strings.emplace_back(name, string((const char*)data)); break;
case ZCM_FIELD_USER_TYPE: assert(false && "Should not be possble");
}
};

_dialog = new QDialog;
_ui = new Ui::DialogZcm;
_ui->setupUi(_dialog);
Expand Down Expand Up @@ -187,12 +172,32 @@ bool DataStreamZcm::xmlLoadState(const QDomElement& parent_element)
return true;
}


void DataStreamZcm::processData(const string& name, zcm_field_type_t type,
const void* data, void* usr)
{
DataStreamZcm *me = (DataStreamZcm*)usr;
switch (type) {
case ZCM_FIELD_INT8_T: me->_numerics.emplace_back(name, toDouble<int8_t>(data)); break;
case ZCM_FIELD_INT16_T: me->_numerics.emplace_back(name, toDouble<int16_t>(data)); break;
case ZCM_FIELD_INT32_T: me->_numerics.emplace_back(name, toDouble<int32_t>(data)); break;
case ZCM_FIELD_INT64_T: me->_numerics.emplace_back(name, toDouble<int64_t>(data)); break;
case ZCM_FIELD_BYTE: me->_numerics.emplace_back(name, toDouble<uint8_t>(data)); break;
case ZCM_FIELD_FLOAT: me->_numerics.emplace_back(name, toDouble<float>(data)); break;
case ZCM_FIELD_DOUBLE: me->_numerics.emplace_back(name, toDouble<double>(data)); break;
case ZCM_FIELD_BOOLEAN: me->_numerics.emplace_back(name, toDouble<bool>(data)); break;
case ZCM_FIELD_STRING: me->_strings.emplace_back(name, string((const char*)data)); break;
case ZCM_FIELD_USER_TYPE: assert(false && "Should not be possble");
}
};


void DataStreamZcm::handler(const zcm::ReceiveBuffer* rbuf, const string& channel)
{
zcm::Introspection::processEncodedType(channel,
rbuf->data, rbuf->data_size,
"/",
*_types.get(), _processData);
*_types.get(), processData, this);
{
std::lock_guard<std::mutex> lock(mutex());

Expand Down
6 changes: 4 additions & 2 deletions plotjuggler_plugins/PluginsZcm/datastream_zcm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,14 +39,16 @@ class DataStreamZcm : public PJ::DataStreamer
private slots:
void on_pushButtonUrl_clicked();

private:
private:
std::unique_ptr<zcm::TypeDb> _types;

std::unique_ptr<zcm::ZCM> _zcm;

zcm::Subscription* _subs = nullptr;

zcm::Introspection::ProcessFn _processData;
static void processData(const std::string& name,
zcm_field_type_t type,
const void* data, void* usr);

std::vector<std::pair<std::string, double>> _numerics;
std::vector<std::pair<std::string, std::string>> _strings;
Expand Down

0 comments on commit 2327ec0

Please sign in to comment.