Skip to content

Commit

Permalink
fixed some issue with reloading rosbags and addressing issue facontid…
Browse files Browse the repository at this point in the history 
…avide#54
  • Loading branch information
@facontidavide
facontidavide committed Nov 19, 2017
1 parent eaf7104 commit 2ccc5de
Show file tree
Hide file tree
Showing 12 changed files with 2,857 additions and 150 deletions.
2,726 changes: 2,726 additions & 0 deletions datasamples/fake_joints.csv
View file

Large diffs are not rendered by default.

7 changes: 6 additions & 1 deletion plotter_gui/filterablelistwidget.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -305,6 +305,11 @@ border: 0.5px solid rgb(150, 150, 150);</string>
<verstretch>1</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<pointsize>9</pointsize>
</font>
</property>
<property name="toolTip">
<string/>
</property>
Expand All @@ -313,7 +318,7 @@ border: 0.5px solid rgb(150, 150, 150);</string>
</property>
<property name="styleSheet">
<string notr="true">border-radius: 4px;
border: 0.5px solid rgb(150, 150, 150);</string>
border: 1px solid rgb(150, 150, 150);</string>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAsNeeded</enum>
Expand Down
36 changes: 19 additions & 17 deletions plotter_gui/mainwindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -806,7 +806,7 @@ void MainWindow::onDeleteLoadedData()

}

void MainWindow::onActionLoadDataFile(bool reload_from_settings)
void MainWindow::onActionLoadDataFile()
{
if( _data_loader.empty())
{
Expand Down Expand Up @@ -837,18 +837,10 @@ void MainWindow::onActionLoadDataFile(bool reload_from_settings)

QString directory_path = settings.value("MainWindow.lastDatafileDirectory", QDir::currentPath() ).toString();

QString filename;
if( reload_from_settings && settings.contains("MainWindow.recentlyLoadedDatafile") )
{
filename = settings.value("MainWindow.recentlyLoadedDatafile").toString();
}
else{
filename = QFileDialog::getOpenFileName(this, "Open Datafile",
QString filename = QFileDialog::getOpenFileName(this, "Open Datafile",
directory_path,
file_extension_filter);
}

if (filename.isEmpty()) {
if (filename.isEmpty()) {
return;
}

Expand All @@ -864,9 +856,25 @@ void MainWindow::onActionLoadDataFile(bool reload_from_settings)

void MainWindow::onReloadDatafile()
{
QSettings settings( "IcarusTechnology", "PlotJuggler");
if( settings.contains("MainWindow.recentlyLoadedDatafile") )
{
QString filename = settings.value("MainWindow.recentlyLoadedDatafile").toString();
onActionLoadDataFileImpl(filename, true);
}
}

void MainWindow::onActionReloadRecentDataFile()
{
QSettings settings( "IcarusTechnology", "PlotJuggler");
if( settings.contains("MainWindow.recentlyLoadedDatafile") )
{
QString filename = settings.value("MainWindow.recentlyLoadedDatafile").toString();
onActionLoadDataFileImpl(filename, false);
}
}


void MainWindow::importPlotDataMap(const PlotDataMap& new_data, bool delete_older)
{
// overwrite the old user_defined map
Expand Down Expand Up @@ -1020,12 +1028,6 @@ void MainWindow::onActionLoadDataFileImpl(QString filename, bool reuse_last_conf
_curvelist_widget->updateFilter();
}


void MainWindow::onActionReloadRecentDataFile()
{
onActionLoadDataFile( true );
}

void MainWindow::onActionReloadRecentLayout()
{
onActionLoadLayout( true );
Expand Down
2 changes: 1 addition & 1 deletion plotter_gui/mainwindow.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ private slots:

void onActionLoadLayoutFromFile(QString filename, bool load_data);

void onActionLoadDataFile(bool reload_from_settings = false);
void onActionLoadDataFile();

void onReloadDatafile();

Expand Down
4 changes: 2 additions & 2 deletions plotter_gui/mainwindow.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -435,7 +435,7 @@ background: white;
<x>0</x>
<y>0</y>
<width>1000</width>
<height>20</height>
<height>19</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
Expand Down Expand Up @@ -550,7 +550,7 @@ background: white;
</action>
<action name="actionReloadPrevious">
<property name="text">
<string>Reload previous</string>
<string>Quick Data Reload</string>
</property>
</action>
</widget>
Expand Down
84 changes: 19 additions & 65 deletions plugins/DataLoadCSV/dataload_csv.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,62 +32,28 @@ int DataLoadCSV::parseHeader(QFile *file,
QStringList string_items = first_line.split(csv_separator);
QStringList secondline_items = second_line.split(csv_separator);

if( string_items.count() != secondline_items.count() )
{
throw std::runtime_error("DataLoadCSV: problem parsing the first two lines");
}

for (int i=0; i < string_items.size(); i++ )
{
// remove annoying prefix
QStringRef field_name ( &string_items[i] );
QString field_name ( string_items[i] );
if( field_name.startsWith( "field." ) )
{
field_name = field_name.mid(6);
}

QString qname = field_name.toString();
if( qname.isEmpty())
if( field_name.isEmpty())
{
qname = QString("_Column_%1").arg(i);
field_name = QString("_Column_%1").arg(i);
}
ordered_names.push_back( std::make_pair(true,qname) );
}

for (unsigned i=0; i < ordered_names.size(); i++ )
{
QString field_name ( ordered_names[i].second );
if( field_name.endsWith( ".name" ) && i < ordered_names.size()-1)
{
QString prefix = field_name.left(field_name.size() - 5 );
QString replace_name = secondline_items[i];

ordered_names[i].first = false;
i++;
//----------------------------------------

QString value_prefix = prefix + ".value";

if( value_prefix.contains("vectors3d") )
{
ordered_names[i].second = prefix + "/" + replace_name + "/x";
i++;
ordered_names[i].second = prefix + "/" + replace_name + "/y";
i++;
ordered_names[i].second = prefix + "/" + replace_name + "/z";
}
else
{
while( i < ordered_names.size() &&
ordered_names[i].second.startsWith( value_prefix ) )
{
QString name = ordered_names[i].second;
QString suffix = name.right( name.size() - value_prefix.size());
ordered_names[i].second = prefix + "/" + replace_name;
if( suffix.size() > 0)
{
ordered_names[i].second.append( "/" + suffix );
}
i++;
}
i--;
}
}
bool is_number;
secondline_items[i].toDouble(&is_number);
ordered_names.push_back( std::make_pair(is_number,field_name) );
}

while (!inA.atEnd())
Expand Down Expand Up @@ -135,39 +101,28 @@ PlotDataMap DataLoadCSV::readDataFromFile(const QString &file_name, bool use_pre

double prev_time = -1;

// remove first line (header
// remove first line (header)
inB.readLine();

//---- build plots_vector from header ------
QStringList valid_field_names;

QSettings settings( "IcarusTechnology", "PlotJuggler");
if( _default_time_axis.isEmpty() )
{
QVariant def = settings.value("DataLoadCSV/default_time_axis");
if( !def.isNull() && def.isValid())
{
_default_time_axis = def.toString();
}
}

for (unsigned i=0; i < ordered_names.size(); i++ )
{
bool valid = ordered_names[i].first;
if( valid )
if( ordered_names[i].first )
{
QString& qname = ( ordered_names[i].second );
std::string name = qname.toStdString();
const QString& field_name = ( ordered_names[i].second );
std::string name = field_name.toStdString();

PlotDataPtr plot( new PlotData(name.c_str()) );
plot_data.numeric.insert( std::make_pair( name, plot ) );

valid_field_names.push_back( qname );
valid_field_names.push_back( field_name );
plots_vector.push_back( plot );

if (time_index == TIME_INDEX_NOT_DEFINED)
if (time_index == TIME_INDEX_NOT_DEFINED && use_previous_configuration)
{
if( _default_time_axis == qname )
if( _default_time_axis == field_name )
{
time_index = valid_field_names.size() ;
}
Expand All @@ -193,7 +148,6 @@ PlotDataMap DataLoadCSV::readDataFromFile(const QString &file_name, bool use_pre
// vector is supposed to have only one element
time_index = dialog->getSelectedRowNumber().at(0) -1;
_default_time_axis = field_names.at( time_index + 1 ) ;
settings.setValue("DataLoadCSV/default_time_axis", _default_time_axis);
}

//-----------------
Expand All @@ -212,7 +166,7 @@ PlotDataMap DataLoadCSV::readDataFromFile(const QString &file_name, bool use_pre
{
QMessageBox::StandardButton reply;
reply = QMessageBox::question(0, tr("Error reading file"),
tr("Selected time in notstrictly monotonic. Do you want to abort?\n"
tr("Selected time in not strictly monotonic. Do you want to abort?\n"
"(Clicking \"NO\" you continue loading)") );

interrupted = (reply == QMessageBox::Yes);
Expand Down
33 changes: 22 additions & 11 deletions plugins/ROS/DataLoadROS/dataload_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ PlotDataMap DataLoadROS::readDataFromFile(const QString &file_name, bool use_pre
if( _bag ) _bag->close();

_bag = std::make_shared<rosbag::Bag>();
_parser.reset( new RosIntrospection::Parser );

using namespace RosIntrospection;

Expand Down Expand Up @@ -67,15 +68,18 @@ PlotDataMap DataLoadROS::readDataFromFile(const QString &file_name, bool use_pre
const auto& definition = connections[i]->msg_def;

all_topics.push_back( std::make_pair(QString( topic.c_str()), QString( datatype.c_str()) ) );
RosIntrospectionFactory::registerMessage(topic, md5sum, datatype, definition);
_parser->registerMessageDefinition(topic, ROSType(datatype), definition);
RosIntrospectionFactory::registerMessage(topic, md5sum, datatype, definition );
}

int count = 0;

//----------------------------------
QSettings settings( "IcarusTechnology", "PlotJuggler");

if( _default_topic_names.empty())
_use_renaming_rules = settings.value("DataLoadROS/use_renaming").toBool();

if( _default_topic_names.empty() )
{
// if _default_topic_names is empty (xmlLoad didn't work) use QSettings.
QVariant def = settings.value("DataLoadROS/default_topics");
Expand All @@ -97,16 +101,24 @@ PlotDataMap DataLoadROS::readDataFromFile(const QString &file_name, bool use_pre
if( dialog->checkBoxUseRenamingRules()->isChecked())
{
_rules = RuleEditing::getRenamingRules();
_use_renaming_rules = true;
}
else{
_rules.clear();
}
for(const auto& it: _rules) {
RosIntrospectionFactory::parser().registerRenamingRules( ROSType(it.first) , it.second );
_use_renaming_rules = false;
}
}
settings.setValue("DataLoadROS/default_topics", _default_topic_names);
settings.setValue("DataLoadROS/use_renaming", _use_renaming_rules);
}

if( _use_renaming_rules )
{
for(const auto& it: _rules) {
_parser->registerRenamingRules( ROSType(it.first) , it.second );
}
}

//-----------------------------------
std::set<std::string> topic_selected;
for(const auto& topic: _default_topic_names)
Expand All @@ -129,14 +141,13 @@ PlotDataMap DataLoadROS::readDataFromFile(const QString &file_name, bool use_pre
QElapsedTimer timer;
timer.start();

static FlatMessage flat_container;
static std::vector<uint8_t> buffer;
static RenamedValues renamed_value;
FlatMessage flat_container;
std::vector<uint8_t> buffer;
RenamedValues renamed_value;

for(rosbag::MessageInstance msg_instance: bag_view_selected )
{
const std::string& topic_name = msg_instance.getTopic();
const std::string& datatype = msg_instance.getDataType();
const size_t msg_size = msg_instance.size();

buffer.resize(msg_size);
Expand All @@ -154,8 +165,8 @@ PlotDataMap DataLoadROS::readDataFromFile(const QString &file_name, bool use_pre
ros::serialization::OStream stream(buffer.data(), buffer.size());
msg_instance.write(stream);

RosIntrospectionFactory::parser().deserializeIntoFlatContainer( topic_name, absl::Span<uint8_t>(buffer), &flat_container, 250 );
RosIntrospectionFactory::parser().applyNameTransform( topic_name, flat_container, &renamed_value );
_parser->deserializeIntoFlatContainer( topic_name, absl::Span<uint8_t>(buffer), &flat_container, 250 );
_parser->applyNameTransform( topic_name, flat_container, &renamed_value );

// apply time offsets
double msg_time = 0;
Expand Down
4 changes: 4 additions & 0 deletions plugins/ROS/DataLoadROS/dataload_ros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,10 @@ class DataLoadROS: public QObject, DataLoader

QStringList _default_topic_names;

std::unique_ptr<RosIntrospection::Parser> _parser;

bool _use_renaming_rules;

};

#endif // DATALOAD_CSV_H
Loading

0 comments on commit 2ccc5de

Please sign in to comment.