deserializer_test.cpp
Go to the documentation of this file.
1 #include "config.h"
2 #include <gtest/gtest.h>
3 
4 #include "ros_type_introspection/ros_introspection.hpp"
5 #include <sensor_msgs/JointState.h>
6 #include <sensor_msgs/NavSatStatus.h>
7 #include <sensor_msgs/Imu.h>
8 #include <sensor_msgs/Image.h>
9 #include <std_msgs/Int16MultiArray.h>
10 
11 using namespace ros::message_traits;
12 using namespace RosIntrospection;
13 
14 
15 TEST(Deserialize, JointState)
16 
17 {
18  RosIntrospection::Parser parser;
19 
20  parser.registerMessageDefinition(
21  "JointState",
22  ROSType(DataType<sensor_msgs::JointState>::value()),
23  Definition<sensor_msgs::JointState>::value());
24 
25  sensor_msgs::JointState joint_state;
26 
27  const int NUM = 15;
28 
29  joint_state.header.seq = 2016;
30  joint_state.header.stamp.sec = 1234;
31  joint_state.header.stamp.nsec = 567*1000*1000;
32  joint_state.header.frame_id = "pippo";
33 
34  joint_state.name.resize( NUM );
35  joint_state.position.resize( NUM );
36  joint_state.velocity.resize( NUM );
37  joint_state.effort.resize( NUM );
38 
39  std::string names[NUM];
40  names[0] = ("hola");
41  names[1] = ("ciao");
42  names[2] = ("bye");
43 
44  for (int i=0; i<NUM; i++)
45  {
46  joint_state.name[i] = names[i%3];
47  joint_state.position[i]= 10+i;
48  joint_state.velocity[i]= 30+i;
49  joint_state.effort[i]= 50+i;
50  }
51 
52  std::vector<uint8_t> buffer( ros::serialization::serializationLength(joint_state) );
53  ros::serialization::OStream stream(buffer.data(), buffer.size());
54  ros::serialization::Serializer<sensor_msgs::JointState>::write(stream, joint_state);
55 
56  FlatMessage flat_container;
57  parser.deserializeIntoFlatContainer("JointState", absl::Span<uint8_t>(buffer), &flat_container,100);
58 
59  if(VERBOSE_TEST){
60  for(auto&it: flat_container.value) {
61  std::cout << it.first << " >> " << it.second.convert<double>() << std::endl;
62  }
63 
64  for(auto&it: flat_container.name) {
65  std::cout << it.first << " >> " << it.second << std::endl;
66  }
67  }
68 
69  EXPECT_EQ( flat_container.value[0].first.toStdString() , ("JointState/header/seq"));
70  EXPECT_EQ( flat_container.value[0].second.convert<int>(), 2016 );
71  EXPECT_EQ( flat_container.value[1].first.toStdString() , ("JointState/header/stamp"));
72  EXPECT_EQ( flat_container.value[1].second.convert<double>(), double(1234.567) );
73  EXPECT_EQ( flat_container.value[1].second.convert<ros::Time>(), joint_state.header.stamp );
74 
75  EXPECT_EQ( flat_container.value[2].first.toStdString() , ("JointState/position.0"));
76  EXPECT_EQ( flat_container.value[2].second.convert<int>(), 10 );
77  EXPECT_EQ( flat_container.value[3].first.toStdString() , ("JointState/position.1"));
78  EXPECT_EQ( flat_container.value[3].second.convert<int>(), 11 );
79  EXPECT_EQ( flat_container.value[4].first.toStdString() , ("JointState/position.2"));
80  EXPECT_EQ( flat_container.value[4].second.convert<int>(), 12 );
81  EXPECT_EQ( flat_container.value[16].first.toStdString() , ("JointState/position.14"));
82  EXPECT_EQ( flat_container.value[16].second.convert<int>(), 24 );
83 
84  EXPECT_EQ( flat_container.value[17].first.toStdString() , ("JointState/velocity.0"));
85  EXPECT_EQ( flat_container.value[17].second.convert<int>(), 30 );
86  EXPECT_EQ( flat_container.value[18].first.toStdString() , ("JointState/velocity.1"));
87  EXPECT_EQ( flat_container.value[18].second.convert<int>(), 31 );
88  EXPECT_EQ( flat_container.value[19].first.toStdString() , ("JointState/velocity.2"));
89  EXPECT_EQ( flat_container.value[19].second.convert<int>(), 32 );
90  EXPECT_EQ( flat_container.value[31].first.toStdString() , ("JointState/velocity.14"));
91  EXPECT_EQ( flat_container.value[31].second.convert<int>(), 44 );
92 
93  EXPECT_EQ( flat_container.value[32].first.toStdString() , ("JointState/effort.0"));
94  EXPECT_EQ( flat_container.value[32].second.convert<int>(), 50 );
95  EXPECT_EQ( flat_container.value[33].first.toStdString() , ("JointState/effort.1"));
96  EXPECT_EQ( flat_container.value[33].second.convert<int>(), 51 );
97  EXPECT_EQ( flat_container.value[34].first.toStdString() , ("JointState/effort.2"));
98  EXPECT_EQ( flat_container.value[34].second.convert<int>(), 52 );
99  EXPECT_EQ( flat_container.value[46].first.toStdString() , ("JointState/effort.14"));
100  EXPECT_EQ( flat_container.value[46].second.convert<int>(), 64 );
101 
102  EXPECT_EQ( flat_container.name[0].first.toStdString() , ("JointState/header/frame_id"));
103  EXPECT_EQ( flat_container.name[0].second, ("pippo") );
104 
105  EXPECT_EQ( flat_container.name[1].first.toStdString() , ("JointState/name.0"));
106  EXPECT_EQ( flat_container.name[1].second, ("hola") );
107  EXPECT_EQ( flat_container.name[2].first.toStdString() , ("JointState/name.1"));
108  EXPECT_EQ( flat_container.name[2].second, ("ciao") );
109  EXPECT_EQ( flat_container.name[3].first.toStdString() , ("JointState/name.2"));
110  EXPECT_EQ( flat_container.name[3].second, ("bye") );
111 
112  //---------------------------------
113  std::vector<std_msgs::Header> headers;
114 
115  Parser::VisitingCallback callbackReadAndStore = [&headers](const ROSType&, absl::Span<uint8_t>& raw_data)
116  {
117  std_msgs::Header msg;
118  ros::serialization::IStream s( raw_data.data(), raw_data.size() );
119  ros::serialization::deserialize(s, msg);
120  headers.push_back( std::move(msg) );
121  };
122 
123  Parser::VisitingCallback callbackOverwiteInPlace = [&headers](const ROSType&, absl::Span<uint8_t>& raw_data)
124  {
125  std_msgs::Header msg;
126  ros::serialization::IStream is( raw_data.data(), raw_data.size() );
127  ros::serialization::deserialize(is, msg);
128 
129  ASSERT_EQ(ros::serialization::serializationLength(msg), raw_data.size());
130 
131  // msg.frame_id = "here"; //NOTE: I can NOT change the size of an array, nor a string
132  msg.seq = 666;
133  msg.stamp.sec = 1;
134  msg.stamp.nsec = 2;
135 
136  ros::serialization::OStream os( raw_data.data(), raw_data.size() );
137  ros::serialization::serialize(os, msg);
138 
139  ASSERT_EQ(ros::serialization::serializationLength(msg), raw_data.size());
140  };
141 
142 
143  absl::Span<uint8_t> buffer_view(buffer);
144  const ROSType header_type( DataType<std_msgs::Header>::value() );
145 
146  parser.applyVisitorToBuffer( "JointState", header_type,
147  buffer_view, callbackReadAndStore);
148 
149  EXPECT_EQ(headers.size(), 1);
150  const std_msgs::Header& header = headers[0];
151  EXPECT_EQ(header.seq, joint_state.header.seq);
152  EXPECT_EQ(header.stamp.sec, joint_state.header.stamp.sec);
153  EXPECT_EQ(header.stamp.nsec, joint_state.header.stamp.nsec);
154  EXPECT_EQ(header.frame_id, joint_state.header.frame_id);
155  //--------------------------------------------
156  parser.applyVisitorToBuffer( "JointState", header_type,
157  buffer_view, callbackOverwiteInPlace);
158 
159  parser.applyVisitorToBuffer( "JointState", header_type,
160  buffer_view, callbackReadAndStore);
161 
162  EXPECT_EQ(headers.size(), 2);
163  const std_msgs::Header& header_mutated = headers[1];
164  EXPECT_EQ(header_mutated.seq, 666);
165  EXPECT_EQ(header_mutated.stamp.sec, 1);
166  EXPECT_EQ(header_mutated.stamp.nsec, 2);
167 
168 }
169 
170 TEST( Deserialize, NavSatStatus)
171 
172 {
173  // We test this because we want to test that constant fields are skipped.
174  RosIntrospection::Parser parser;
175 
176  parser.registerMessageDefinition(
177  "nav_stat",
178  ROSType(DataType<sensor_msgs::NavSatStatus>::value()),
179  Definition<sensor_msgs::NavSatStatus>::value());
180 
181  sensor_msgs::NavSatStatus nav_stat;
182  nav_stat.status = nav_stat.STATUS_GBAS_FIX; // 2
183  nav_stat.service = nav_stat.SERVICE_COMPASS; // 4
184 
185 
186  std::vector<uint8_t> buffer( ros::serialization::serializationLength(nav_stat) );
187  ros::serialization::OStream stream(buffer.data(), buffer.size());
188  ros::serialization::Serializer<sensor_msgs::NavSatStatus>::write(stream, nav_stat);
189 
190  FlatMessage flat_container;
191  parser.deserializeIntoFlatContainer("nav_stat", absl::Span<uint8_t>(buffer), &flat_container,100);
192 
193  if(VERBOSE_TEST){ std::cout << " -------------------- " << std::endl;
194 
195  for(auto&it: flat_container.value) {
196  std::cout << it.first << " >> " << it.second.convert<double>() << std::endl;
197  }
198  }
199 
200  EXPECT_EQ( flat_container.value[0].first.toStdString() , ("nav_stat/status"));
201  EXPECT_EQ( flat_container.value[0].second.convert<int>(), (int)nav_stat.STATUS_GBAS_FIX );
202  EXPECT_EQ( flat_container.value[1].first.toStdString() , ("nav_stat/service"));
203  EXPECT_EQ( flat_container.value[1].second.convert<int>(), (int)nav_stat.SERVICE_COMPASS );
204 }
205 
206 TEST( Deserialize, DeserializeIMU)
207 //int func()
208 {
209  // We test this because to check if arrays with fixed length work.
210  RosIntrospection::Parser parser;
211 
212  parser.registerMessageDefinition(
213  "imu",
214  ROSType(DataType<sensor_msgs::Imu>::value()),
215  Definition<sensor_msgs::Imu>::value());
216 
217  sensor_msgs::Imu imu;
218 
219  imu.header.seq = 2016;
220  imu.header.stamp.sec = 1234;
221  imu.header.stamp.nsec = 567*1000*1000;
222  imu.header.frame_id = "pippo";
223 
224  imu.orientation.x = 11;
225  imu.orientation.y = 12;
226  imu.orientation.z = 13;
227  imu.orientation.w = 14;
228 
229  imu.angular_velocity.x = 21;
230  imu.angular_velocity.y = 22;
231  imu.angular_velocity.z = 23;
232 
233  imu.linear_acceleration.x = 31;
234  imu.linear_acceleration.y = 32;
235  imu.linear_acceleration.z = 33;
236 
237  for (int i=0; i<9; i++)
238  {
239  imu.orientation_covariance[i] = 40+i;
240  imu.angular_velocity_covariance[i] = 50+i;
241  imu.linear_acceleration_covariance[i] = 60+i;
242  }
243 
244  std::vector<uint8_t> buffer( ros::serialization::serializationLength(imu) );
245  ros::serialization::OStream stream(buffer.data(), buffer.size());
246  ros::serialization::Serializer<sensor_msgs::Imu>::write(stream, imu);
247 
248  FlatMessage flat_container;
249  parser.deserializeIntoFlatContainer("imu", absl::Span<uint8_t>(buffer), &flat_container,100);
250 
251 
252  if(VERBOSE_TEST){
253 
254  std::cout << " -------------------- " << std::endl;
255  for(auto&it: flat_container.value) {
256  std::cout << it.first << " >> " << it.second.convert<double>() << std::endl;
257  }
258  }
259 
260  int index = 0;
261 
262  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/header/seq"));
263  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 2016 );
264  index++;
265  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/header/stamp"));
266  EXPECT_EQ( flat_container.value[index].second.convert<double>(), double(1234.567) );
267  EXPECT_EQ( flat_container.value[index].second.convert<ros::Time>(), imu.header.stamp );
268  index++;
269  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/orientation/x"));
270  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 11 );
271  index++;
272  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/orientation/y"));
273  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 12 );
274  index++;
275  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/orientation/z"));
276  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 13 );
277  index++;
278  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/orientation/w"));
279  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 14 );
280  index++;
281 
282  for(int i=0; i<9; i++)
283  {
284  char str[64];
285  sprintf(str, "imu/orientation_covariance.%d",i);
286  EXPECT_EQ( flat_container.value[index].first.toStdString() , (str) );
287  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 40+i );
288  index++;
289  }
290 
291  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/angular_velocity/x"));
292  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 21 );
293  index++;
294  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/angular_velocity/y"));
295  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 22 );
296  index++;
297  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/angular_velocity/z"));
298  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 23 );
299  index++;
300 
301  for(int i=0; i<9; i++)
302  {
303  char str[64];
304  sprintf(str, "imu/angular_velocity_covariance.%d",i);
305  EXPECT_EQ( flat_container.value[index].first.toStdString() , (str) );
306  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 50+i );
307  index++;
308  }
309 
310  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/linear_acceleration/x"));
311  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 31 );
312  index++;
313  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/linear_acceleration/y"));
314  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 32 );
315  index++;
316  EXPECT_EQ( flat_container.value[index].first.toStdString() , ("imu/linear_acceleration/z"));
317  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 33 );
318  index++;
319 
320  for(int i=0; i<9; i++)
321  {
322  char str[64];
323  sprintf(str, "imu/linear_acceleration_covariance.%d",i);
324  EXPECT_EQ( flat_container.value[index].first.toStdString() , (str) );
325  EXPECT_EQ( flat_container.value[index].second.convert<int>(), 60+i );
326  index++;
327  }
328 
329  //---------------------------------
330 // std::vector< std::pair<SString,std_msgs::Header>> headers;
331 // std::vector< std::pair<SString,geometry_msgs::Vector3>> vectors;
332 // std::vector< std::pair<SString,geometry_msgs::Quaternion>> quaternions;
333 
334 // ExtractSpecificROSMessages(type_map, main_type,
335 // "imu", buffer,
336 // headers);
337 
338 // EXPECT_EQ(headers.size(), 1);
339 // const std_msgs::Header& header = headers[0].second;
340 // std::string header_prefix = headers[0].first.toStdString();
341 // EXPECT_EQ( header_prefix, "imu/header");
342 // EXPECT_EQ(header.seq, imu.header.seq);
343 // EXPECT_EQ(header.stamp.sec, imu.header.stamp.sec);
344 // EXPECT_EQ(header.stamp.nsec, imu.header.stamp.nsec);
345 // EXPECT_EQ(header.frame_id, imu.header.frame_id);
346 
347 // ExtractSpecificROSMessages(type_map, main_type,
348 // "imu", buffer,
349 // quaternions);
350 
351 // EXPECT_EQ(quaternions.size(), 1);
352 // const geometry_msgs::Quaternion& quaternion = quaternions[0].second;
353 // std::string quaternion_prefix = quaternions[0].first.toStdString();
354 // EXPECT_EQ( quaternion_prefix, "imu/orientation");
355 // EXPECT_EQ(quaternion.x, imu.orientation.x);
356 // EXPECT_EQ(quaternion.y, imu.orientation.y);
357 // EXPECT_EQ(quaternion.z, imu.orientation.z);
358 // EXPECT_EQ(quaternion.w, imu.orientation.w);
359 
360 // ExtractSpecificROSMessages(type_map, main_type,
361 // "imu", buffer,
362 // vectors);
363 
364 // EXPECT_EQ(vectors.size(), 2);
365 // for( const auto& vect_pair: vectors)
366 // {
367 // if( vect_pair.first.toStdString() == "imu/angular_velocity")
368 // {
369 // EXPECT_EQ(vect_pair.second.x, imu.angular_velocity.x);
370 // EXPECT_EQ(vect_pair.second.y, imu.angular_velocity.y);
371 // EXPECT_EQ(vect_pair.second.z, imu.angular_velocity.z);
372 // }
373 // else if( vect_pair.first.toStdString() == "imu/linear_acceleration")
374 // {
375 // EXPECT_EQ(vect_pair.second.x, imu.linear_acceleration.x);
376 // EXPECT_EQ(vect_pair.second.y, imu.linear_acceleration.y);
377 // EXPECT_EQ(vect_pair.second.z, imu.linear_acceleration.z);
378 // }
379 // else{
380 // FAIL();
381 // }
382 // }
383 }
384 
385 
386 
387 TEST( Deserialize, Int16MultiArrayDeserialize)
388 //int func()
389 {
390  RosIntrospection::Parser parser;
391 
392  parser.registerMessageDefinition(
393  "multi_array",
394  ROSType(DataType<std_msgs::Int16MultiArray>::value()),
395  Definition<std_msgs::Int16MultiArray>::value());
396 
397  std_msgs::Int16MultiArray multi_array;
398 
399  const unsigned N = 6;
400  multi_array.layout.data_offset = 42;
401  multi_array.data.resize(N);
402 
403  for (unsigned i=0; i<N; i++){
404  multi_array.data[i] = i;
405  }
406 
407  std::vector<uint8_t> buffer( ros::serialization::serializationLength(multi_array) );
408  ros::serialization::OStream stream(buffer.data(), buffer.size());
409  ros::serialization::Serializer<std_msgs::Int16MultiArray>::write(stream, multi_array);
410 
411  FlatMessage flat_container;
412 
413  EXPECT_NO_THROW(
414  parser.deserializeIntoFlatContainer("multi_array",
415  absl::Span<uint8_t>(buffer),
416  &flat_container,100)
417  );
418 
419  if(VERBOSE_TEST){
420  std::cout << " -------------------- " << std::endl;
421 
422  for(auto&it: flat_container.value) {
423  std::cout << it.first << " >> " << it.second.convert<double>() << std::endl;
424  }
425  }
426 }
427 
428 TEST( Deserialize, SensorImage)
429 //int func()
430 {
431  RosIntrospection::Parser parser;
432 
433  parser.registerMessageDefinition( "image_raw",
434  ROSType(DataType<sensor_msgs::Image>::value()),
435  Definition<sensor_msgs::Image>::value());
436 
437  sensor_msgs::Image image;
438  image.width = 640;
439  image.height = 480;
440  image.step = 3*image.width;
441  image.data.resize( image.height * image.step );
442 
443  std::vector<uint8_t> buffer( ros::serialization::serializationLength(image) );
444  ros::serialization::OStream stream(buffer.data(), buffer.size());
445  ros::serialization::Serializer<sensor_msgs::Image>::write(stream, image);
446 
447  FlatMessage flat_container;
448 
449  EXPECT_NO_THROW(
450  parser.deserializeIntoFlatContainer("image_raw",
451  absl::Span<uint8_t>(buffer),
452  &flat_container,100)
453  );
454 
455 }
456 
457 
458 // Run all the tests that were declared with TEST()
459 int main(int argc, char **argv){
460  testing::InitGoogleTest(&argc, argv);
461  return RUN_ALL_TESTS();
462 }
RosIntrospection::Parser::VisitingCallback
std::function< void(const ROSType &, absl::Span< uint8_t > &)> VisitingCallback
TEST
TEST(Deserialize, JointState)
Definition: deserializer_test.cpp:15
EXPECT_EQ
EXPECT_EQ(flat_container.value[index].first.toStdString(),("imu/header/seq"))
ros::serialization::Serializer::write
static void write(Stream &stream, typename boost::call_traits< T >::param_type t)
RosIntrospection::FlatMessage::name
std::vector< std::pair< StringTreeLeaf, std::string > > name
main
int main(int argc, char **argv)
Definition: deserializer_test.cpp:459
imu
sensor_msgs::Imu imu
Definition: deserializer_test.cpp:217
parser
RosIntrospection::Parser parser
Definition: rosbag_example.cpp:6
s
XmlRpcServer s
RosIntrospection::Parser::applyVisitorToBuffer
void applyVisitorToBuffer(const std::string &msg_identifier, const ROSType &monitored_type, absl::Span< uint8_t > &buffer, VisitingCallback callback) const
ros::message_traits::header
std_msgs::Header * header(M &m)
ros::message_traits::Definition
image
sensor_msgs::Image image
Definition: deserializer_test.cpp:437
multi_array
std_msgs::Int16MultiArray multi_array
Definition: deserializer_test.cpp:397
ros::serialization::serialize
void serialize(Stream &stream, const T &t)
RosIntrospection::FlatMessage::value
std::vector< std::pair< StringTreeLeaf, Variant > > value
VERBOSE_TEST
#define VERBOSE_TEST
Definition: config.h:4
RosIntrospection::Parser::deserializeIntoFlatContainer
bool deserializeIntoFlatContainer(const std::string &msg_identifier, absl::Span< uint8_t > buffer, FlatMessage *flat_container_output, const uint32_t max_array_size) const
flat_container
FlatMessage flat_container
Definition: deserializer_test.cpp:248
ros::message_traits
ros::serialization::serializationLength
uint32_t serializationLength(const T &t)
stream
ros::serialization::OStream stream(buffer.data(), buffer.size())
N
const unsigned N
Definition: deserializer_test.cpp:399
ros::message_traits::DataType
ros_introspection.hpp
msg
const char * msg
i
int i
buffer
std::vector< uint8_t > buffer(ros::serialization::serializationLength(imu))
ros::serialization::deserialize
void deserialize(Stream &stream, T &t)
EXPECT_NO_THROW
EXPECT_NO_THROW(parser.deserializeIntoFlatContainer("multi_array", absl::Span< uint8_t >(buffer),&flat_container, 100))
RosIntrospection::Parser::registerMessageDefinition
void registerMessageDefinition(const std::string &message_identifier, const ROSType &main_type, const std::string &definition)
index
int index
Definition: deserializer_test.cpp:260

type_introspection_tests
Author(s): Davide Faconti
autogenerated on Sun Apr 15 2018 02:22:00