00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <functional>
00036 #include "ros_type_introspection/deserializer.hpp"
00037
00038
00039 namespace RosIntrospection{
00040
00041
00042 void buildRosFlatTypeImpl(const ROSTypeList& type_list,
00043 const ROSType &type,
00044 StringTreeLeaf tree_node,
00045 uint8_t** buffer_ptr,
00046 ROSTypeFlat* flat_container,
00047 const uint32_t max_array_size,
00048 bool do_store)
00049 {
00050 int array_size = type.arraySize();
00051 if( array_size == -1)
00052 {
00053 array_size = ReadFromBuffer<int32_t>( buffer_ptr );
00054 }
00055
00056
00057
00058 std::function<void(StringTreeLeaf, bool)> deserializeAndStore;
00059
00060 if( type.typeID() == STRING )
00061 {
00062 deserializeAndStore = [&flat_container, &buffer_ptr](StringTreeLeaf tree_node, bool store)
00063 {
00064 size_t string_size = (size_t) ReadFromBuffer<int32_t>( buffer_ptr );
00065 SString id( (const char*)(*buffer_ptr), string_size );
00066 (*buffer_ptr) += string_size;
00067 if( store ) flat_container->name.push_back( std::make_pair( std::move(tree_node), id ) );
00068 };
00069 }
00070 else if( type.isBuiltin())
00071 {
00072 deserializeAndStore = [&flat_container, &buffer_ptr, type](StringTreeLeaf tree_node, bool store)
00073 {
00074 auto p = std::make_pair( std::move(tree_node), type.deserializeFromBuffer(buffer_ptr) );
00075 if( store ) flat_container->value.push_back( p );
00076 };
00077 }
00078 else if( type.typeID() == OTHER)
00079 {
00080 const ROSMessage* mg_definition = nullptr;
00081
00082 for(const ROSMessage& msg: type_list)
00083 {
00084 if( msg.type().msgName() == type.msgName() &&
00085 msg.type().pkgName() == type.pkgName() )
00086 {
00087 mg_definition = &msg;
00088 break;
00089 }
00090 }
00091 if( !mg_definition )
00092 {
00093 std::string output( "can't deserialize this stuff: ");
00094 output += type.baseName().toStdString() + "\n\n";
00095 output += "Available types are: \n\n";
00096 for(const ROSMessage& msg: type_list)
00097 {
00098 output += " " +msg.type().baseName().toStdString() + "\n";
00099 }
00100 throw std::runtime_error( output );
00101 }
00102
00103 deserializeAndStore = [&flat_container, &buffer_ptr, mg_definition, &type_list, max_array_size]
00104 (StringTreeLeaf tree_node, bool STORE_RESULT)
00105 {
00106 if( STORE_RESULT == false)
00107 {
00108 for (const ROSField& field : mg_definition->fields() )
00109 {
00110 buildRosFlatTypeImpl(type_list,
00111 field.type(),
00112 (tree_node),
00113 buffer_ptr,
00114 flat_container,
00115 max_array_size,
00116 false);
00117 }
00118 }
00119 else{
00120 auto& children_nodes = tree_node.node_ptr->children();
00121
00122 bool to_add = false;
00123 if( children_nodes.empty() )
00124 {
00125 children_nodes.reserve( mg_definition->fields().size() );
00126 to_add = true;
00127 }
00128
00129 size_t index = 0;
00130
00131 for (const ROSField& field : mg_definition->fields() )
00132 {
00133 if(field.isConstant() == false) {
00134
00135 if( to_add ){
00136 SString node_name( field.name() ) ;
00137 tree_node.node_ptr->addChild( node_name );
00138 }
00139 auto new_tree_node = tree_node;
00140 new_tree_node.node_ptr = &children_nodes[index++];
00141
00142 buildRosFlatTypeImpl(type_list,
00143 field.type(),
00144 (new_tree_node),
00145 buffer_ptr,
00146 flat_container,
00147 max_array_size,
00148 true);
00149
00150 }
00151 }
00152 }
00153 };
00154 }
00155 else {
00156 throw std::runtime_error( "can't deserialize this stuff");
00157 }
00158
00159 const bool STORE = ( do_store ) && ( array_size <= max_array_size );
00160
00161 if( array_size > max_array_size && do_store)
00162 {
00163 std::cout << "Warning: skipped a vector of type "
00164 << type.baseName() << " and size "
00165 << array_size << " because max_array_size = "
00166 << max_array_size << "\n";
00167 }
00168
00169 StringTreeNode* node = tree_node.node_ptr;
00170
00171 if( type.isArray() == false )
00172 {
00173 deserializeAndStore( tree_node, STORE );
00174 }
00175 else
00176 {
00177 if(STORE)
00178 {
00179 node->children().reserve(1);
00180 node->addChild( "#" );
00181 tree_node.node_ptr = &node->children().back();
00182 tree_node.array_size++;
00183
00184 for (int v=0; v<array_size; v++)
00185 {
00186 tree_node.index_array[ tree_node.array_size-1 ] = static_cast<uint16_t>(v);
00187 deserializeAndStore( tree_node, STORE );
00188 }
00189 }
00190 else{
00191 size_t previous_size = flat_container->value.size();
00192 for (int v=0; v<array_size; v++)
00193 {
00194 deserializeAndStore( tree_node, STORE );
00195 }
00196 size_t new_size = flat_container->value.size();
00197 if( new_size > previous_size)
00198 {
00199 throw std::runtime_error("something wrong when STORE == false");
00200 }
00201 }
00202 }
00203 }
00204
00205 void buildRosFlatType(const ROSTypeList& type_map,
00206 ROSType type,
00207 SString prefix,
00208 uint8_t *buffer_ptr,
00209 ROSTypeFlat* flat_container_output,
00210 const uint32_t max_array_size )
00211 {
00212 uint8_t** buffer = &buffer_ptr;
00213
00214 flat_container_output->tree.root()->children().clear();
00215 flat_container_output->tree.root()->value() = prefix;
00216 flat_container_output->name.clear();
00217 flat_container_output->value.clear();
00218
00219 StringTreeLeaf rootnode;
00220 rootnode.node_ptr = flat_container_output->tree.root();
00221
00222 buildRosFlatTypeImpl( type_map,
00223 type,
00224 rootnode,
00225 buffer,
00226 flat_container_output,
00227 max_array_size,
00228 true);
00229 }
00230
00231 StringTreeLeaf::StringTreeLeaf(): node_ptr(nullptr), array_size(0)
00232 { for (int i=0; i<7; i++) index_array[i] = 0;}
00233
00234
00235
00236
00237 bool StringTreeLeaf::toStr(SString& destination) const
00238 {
00239 char buffer[1024];
00240 int offset = this->toStr(buffer);
00241
00242 if( offset < 0 ) {
00243 destination.clear();
00244 return false;
00245 }
00246 destination.assign(buffer, offset);
00247 return true;
00248 }
00249
00250 bool StringTreeLeaf::toStr(std::string& destination) const
00251 {
00252 char buffer[512];
00253 int offset = this->toStr(buffer);
00254
00255 if( offset < 0 ) {
00256 destination.clear();
00257 return false;
00258 }
00259 destination.assign(buffer, offset);
00260 return true;
00261 }
00262
00263 int StringTreeLeaf::toStr(char* buffer) const
00264 {
00265
00266 const StringTreeNode* leaf_node = this->node_ptr;
00267 if( !leaf_node ){
00268 return -1;
00269 }
00270
00271 const StringTreeNode* nodes_from_leaf_to_root[64];
00272 int index = 0;
00273
00274 int char_count = 0;
00275
00276 while(leaf_node)
00277 {
00278 char_count += leaf_node->value().size();
00279 nodes_from_leaf_to_root[index] = leaf_node;
00280 index++;
00281 leaf_node = leaf_node->parent();
00282 };
00283
00284 nodes_from_leaf_to_root[index] = nullptr;
00285 index--;
00286
00287 int array_count = 0;
00288 int off = 0;
00289
00290 while ( index >=0 )
00291 {
00292 const SString& value = nodes_from_leaf_to_root[index]->value();
00293 if( value.size()== 1 && value.at(0) == '#' )
00294 {
00295 buffer[off-1] = '.';
00296 off += print_number(&buffer[off], this->index_array[ array_count++ ] );
00297 }
00298 else{
00299 memcpy( &buffer[off], value.data(), value.size() );
00300 off += value.size();
00301 }
00302 if( index > 0 ){
00303 buffer[off] = '/';
00304 off += 1;
00305 }
00306 index--;
00307 }
00308 buffer[off] = '\0';
00309 return off;
00310 }
00311
00312
00313 }