Go to the documentation of this file.00001 #include "ros_parser.h"
00002 #include "dialog_with_itemlist.h"
00003 #include "geometry_twist_msg.h"
00004 #include "diagnostic_msg.h"
00005 #include "pal_statistics_msg.h"
00006 #include "odometry_msg.h"
00007 #include "fiveai_stamped_diagnostic.h"
00008 #include <absl/strings/charconv.h>
00009
00010 RosMessageParser::RosMessageParser()
00011 {
00012 }
00013
00014 void RosMessageParser::clear()
00015 {
00016 _plot_map.numeric.clear();
00017 _registered_md5sum.clear();
00018 _introspection_parser.reset( new RosIntrospection::Parser );
00019 _builtin_parsers.clear();
00020 _warn_cancellation.clear();
00021 _warn_max_arraysize.clear();
00022 }
00023
00024 double RosMessageParser::extractRealValue(
00025 const RosIntrospection::Variant& value,
00026 const std::string& item_name)
00027 {
00028 if( value.getTypeID() == RosIntrospection::UINT64)
00029 {
00030 uint64_t val_i = value.extract<uint64_t>();
00031 double val_d = static_cast<double>(val_i);
00032 bool error = (val_i != static_cast<uint64_t>(val_d));
00033 if(error && _warnings_enabled)
00034 {
00035 _warn_cancellation.insert( item_name );
00036 }
00037 return val_d;
00038 }
00039
00040 if( value.getTypeID() == RosIntrospection::INT64)
00041 {
00042 int64_t val_i = value.extract<int64_t>();
00043 double val_d = static_cast<double>(val_i);
00044 bool error = (val_i != static_cast<int64_t>(val_d));
00045 if(error && _warnings_enabled)
00046 {
00047 _warn_cancellation.insert( item_name );
00048 }
00049 return val_d;
00050 }
00051
00052 double val_d = value.convert<double>();
00053 return val_d;
00054 }
00055
00056
00057
00058 void RosMessageParser::setMaxArrayPolicy(size_t max_array_size, bool discard_entire_array)
00059 {
00060 _max_array_size = max_array_size;
00061 _discard_large_array = discard_entire_array;
00062 _introspection_parser->setMaxArrayPolicy( discard_entire_array );
00063 }
00064
00065 template <typename T>
00066 bool InsertParser(RosMessageParser::ParsersMap& parsers,
00067 const std::string &topic_name,
00068 const std::string &md5sum)
00069 {
00070 if( md5sum != T::getCompatibleKey() )
00071 {
00072 return false;
00073 }
00074 if( parsers.find(topic_name) == parsers.end())
00075 {
00076 parsers.emplace( std::piecewise_construct,
00077 std::forward_as_tuple(topic_name),
00078 std::forward_as_tuple(new T()) );
00079 }
00080 return true;
00081 }
00082
00083 bool RosMessageParser::registerSchema(const std::string &topic_name,
00084 const std::string &md5sum,
00085 RosIntrospection::ROSType type,
00086 const std::string &definition)
00087 {
00088 _registered_md5sum.insert( md5sum );
00089
00090 bool inserted =
00091 InsertParser<GeometryMsgTwist>( _builtin_parsers, topic_name, md5sum ) ||
00092 InsertParser<OdometryMsgParser>( _builtin_parsers, topic_name, md5sum ) ||
00093 InsertParser<DiagnosticMsg>( _builtin_parsers, topic_name, md5sum ) ||
00094 InsertParser<FiveAiDiagnosticMsg>( _builtin_parsers, topic_name, md5sum ) ||
00095 InsertParser<PalStatisticsNamesParser>( _builtin_parsers, topic_name, md5sum ) ||
00096 InsertParser<PalStatisticsValuesParser>( _builtin_parsers, topic_name, md5sum );
00097
00098 if( !inserted ) {
00099 _introspection_parser->registerMessageDefinition(topic_name, type, definition);
00100 }
00101 return inserted;
00102 }
00103
00104 void RosMessageParser::pushMessageRef(const std::string &topic_name,
00105 const MessageRef &msg,
00106 double timestamp)
00107 {
00108 auto builtin_it = _builtin_parsers.find( topic_name );
00109 if( builtin_it != _builtin_parsers.end() )
00110 {
00111 builtin_it->second->pushMessageRef( builtin_it->first, msg, timestamp );
00112 return;
00113 }
00114
00115 using namespace RosIntrospection;
00116
00117 FlatMessage flat_container;
00118 RenamedValues renamed_values;
00119
00120 absl::Span<uint8_t> msg_view ( const_cast<uint8_t*>(msg.data()), msg.size() );
00121
00122 bool max_size_ok = _introspection_parser->deserializeIntoFlatContainer(
00123 topic_name,
00124 msg_view,
00125 &flat_container,
00126 _max_array_size );
00127
00128 if( !max_size_ok && _warnings_enabled )
00129 {
00130 _warn_max_arraysize.insert(topic_name);
00131 }
00132
00133 _introspection_parser->applyNameTransform( topic_name,
00134 flat_container,
00135 &renamed_values );
00136 if(_use_header_stamp)
00137 {
00138 for (const auto& it: flat_container.value)
00139 {
00140 if( it.second.getTypeID() != RosIntrospection::TIME)
00141 {
00142 continue;
00143 }
00144 const RosIntrospection::StringTreeNode* leaf1 = it.first.node_ptr;
00145 const RosIntrospection::StringTreeNode* leaf2 = leaf1->parent();
00146 if( leaf2 && leaf2->value() == "header" && leaf1->value() == "stamp")
00147 {
00148 double heder_stamp = it.second.convert<double>();
00149
00150 if( heder_stamp > 0 ) {
00151 timestamp = heder_stamp;
00152 }
00153 break;
00154 }
00155 }
00156 }
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190 for(const auto& it: renamed_values )
00191 {
00192 const auto& field_name = it.first;
00193
00194 const RosIntrospection::Variant& value = it.second;
00195
00196 auto plot_pair = _plot_map.numeric.find( field_name );
00197 if( (plot_pair == _plot_map.numeric.end()) )
00198 {
00199 plot_pair = _plot_map.addNumeric( field_name );
00200 }
00201
00202 PlotData& plot_data = plot_pair->second;
00203 size_t data_size = plot_data.size();
00204
00205 try {
00206 double val_d = extractRealValue(value , field_name);
00207 if( !std::isnan(val_d) && !std::isinf(val_d) )
00208 {
00209 plot_data.pushBack( PlotData::Point(timestamp, val_d) );
00210 }
00211 } catch (...) {}
00212 }
00213 }
00214
00215 void RosMessageParser::showWarnings()
00216 {
00217 if( !_warn_max_arraysize.empty() )
00218 {
00219 QString message = QString("The following topics contain arrays with more than %1 elements.\n").arg(_max_array_size);
00220 if( _discard_large_array )
00221 {
00222 message += QString("The fields containing the extra large arrays have been discarded\n");
00223 }
00224 else{
00225 message += QString("These arrays were trunkated to the maximum size %1\n").arg(_max_array_size);
00226 }
00227 DialogWithItemList::warning( message, _warn_max_arraysize );
00228 }
00229
00230 if( !_warn_cancellation.empty() )
00231 {
00232 QString message = "During the parsing process, one or more conversions to double failed"
00233 " because of numerical cancellation.\n"
00234 "This happens when the absolute value of a long integer exceed 2^52.\n\n"
00235 "You have been warned... don't trust the following timeseries\n";
00236 DialogWithItemList::warning( message, _warn_cancellation );
00237 }
00238 }
00239
00240 void RosMessageParser::extractData(PlotDataMapRef &destination, const std::string &prefix)
00241 {
00242 for (auto& it: _plot_map.numeric)
00243 {
00244 appendData( destination, prefix + it.first, it.second );
00245 }
00246 _plot_map.numeric.clear();
00247
00248 for (auto& it: _builtin_parsers)
00249 {
00250 it.second->extractData( destination, prefix + it.first );
00251 }
00252 }
00253