test_utils.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef ROS_IGN_BRIDGE__TEST_UTILS_H_
19 #define ROS_IGN_BRIDGE__TEST_UTILS_H_
20 
21 #include <gtest/gtest.h>
22 #include <ros/ros.h>
23 #include <std_msgs/Bool.h>
24 #include <std_msgs/Empty.h>
25 #include <std_msgs/Float32.h>
26 #include <std_msgs/Float64.h>
27 #include <std_msgs/Header.h>
28 #include <std_msgs/Int32.h>
29 #include <std_msgs/String.h>
30 #include <geometry_msgs/Point.h>
31 #include <geometry_msgs/Pose.h>
32 #include <geometry_msgs/PoseArray.h>
33 #include <geometry_msgs/PoseStamped.h>
34 #include <geometry_msgs/Transform.h>
35 #include <geometry_msgs/TransformStamped.h>
36 #include <geometry_msgs/Twist.h>
37 #include <geometry_msgs/Quaternion.h>
38 #include <geometry_msgs/Vector3.h>
39 // #include <mav_msgs/Actuators.h>
40 #include <nav_msgs/OccupancyGrid.h>
41 #include <nav_msgs/Odometry.h>
42 #include <rosgraph_msgs/Clock.h>
43 #include <sensor_msgs/BatteryState.h>
44 #include <sensor_msgs/CameraInfo.h>
45 #include <sensor_msgs/FluidPressure.h>
46 #include <sensor_msgs/Image.h>
47 #include <sensor_msgs/Imu.h>
48 #include <sensor_msgs/JointState.h>
49 #include <sensor_msgs/LaserScan.h>
50 #include <sensor_msgs/MagneticField.h>
51 #include <sensor_msgs/NavSatFix.h>
52 #include <sensor_msgs/PointCloud2.h>
53 #include <sensor_msgs/PointField.h>
54 #include <tf2_msgs/TFMessage.h>
55 #include <visualization_msgs/Marker.h>
56 #include <visualization_msgs/MarkerArray.h>
57 
58 #include <chrono>
59 #include <string>
60 #include <thread>
61 #include <ignition/msgs.hh>
62 
63 namespace ros_ign_bridge
64 {
65 namespace testing
66 {
76  template <class Rep, class Period>
77  void waitUntilBoolVar(
78  bool &_boolVar,
79  const std::chrono::duration<Rep, Period> &_sleepEach,
80  const int _retries)
81  {
82  int i = 0;
83  while (!_boolVar && i < _retries)
84  {
85  ++i;
86  std::this_thread::sleep_for(_sleepEach);
87  }
88  }
89 
99  template <class Rep, class Period>
100  void waitUntilBoolVarAndSpin(
101  bool &_boolVar,
102  const std::chrono::duration<Rep, Period> &_sleepEach,
103  const int _retries)
104  {
105  int i = 0;
106  while (!_boolVar && i < _retries)
107  {
108  ++i;
109  std::this_thread::sleep_for(_sleepEach);
110  ros::spinOnce();
111  }
112  }
113 
117 
120  void createTestMsg(std_msgs::Bool &_msg)
121  {
122  _msg.data = true;
123  }
124 
127  void compareTestMsg(const std_msgs::Bool &_msg)
128  {
129  std_msgs::Bool expected_msg;
130  createTestMsg(expected_msg);
131 
132  EXPECT_EQ(expected_msg.data, _msg.data);
133  }
134 
137  void createTestMsg(std_msgs::ColorRGBA &_msg)
138  {
139  _msg.r = 10.0;
140  _msg.g = 11.0;
141  _msg.b = 12.0;
142  _msg.a = 13.0;
143  }
144 
147  void compareTestMsg(const std_msgs::ColorRGBA &_msg)
148  {
149  std_msgs::ColorRGBA expected_msg;
150  createTestMsg(expected_msg);
151 
152  EXPECT_EQ(expected_msg.r, _msg.r);
153  EXPECT_EQ(expected_msg.g, _msg.g);
154  EXPECT_EQ(expected_msg.b, _msg.b);
155  EXPECT_EQ(expected_msg.a, _msg.a);
156  }
157 
160  void compareTestMsg(const std_msgs::Empty &)
161  {
162  }
163 
166  void createTestMsg(std_msgs::Int32 &_msg)
167  {
168  _msg.data = 5;
169  }
170 
173  void createTestMsg(std_msgs::Float32 &_msg)
174  {
175  _msg.data = 1.5;
176  }
177 
180  void createTestMsg(std_msgs::Float64 &_msg)
181  {
182  _msg.data = 1.5;
183  }
184 
187  void compareTestMsg(const std_msgs::Int32 &_msg)
188  {
189  std_msgs::Int32 expected_msg;
190  createTestMsg(expected_msg);
191 
192  EXPECT_EQ(expected_msg.data, _msg.data);
193  }
194 
197  void compareTestMsg(const std_msgs::Float32 &_msg)
198  {
199  std_msgs::Float32 expected_msg;
200  createTestMsg(expected_msg);
201 
202  EXPECT_FLOAT_EQ(expected_msg.data, _msg.data);
203  }
204 
207  void compareTestMsg(const std_msgs::Float64 &_msg)
208  {
209  std_msgs::Float64 expected_msg;
210  createTestMsg(expected_msg);
211 
212  EXPECT_DOUBLE_EQ(expected_msg.data, _msg.data);
213  }
214 
217  void createTestMsg(std_msgs::Header &_msg)
218  {
219  _msg.seq = 1;
220  _msg.stamp.sec = 2;
221  _msg.stamp.nsec = 3;
222  _msg.frame_id = "frame_id_value";
223  }
224 
227  void compareTestMsg(const std_msgs::Header &_msg)
228  {
229  std_msgs::Header expected_msg;
230  createTestMsg(expected_msg);
231 
232  EXPECT_GE(expected_msg.seq, 0u);
233  EXPECT_EQ(expected_msg.stamp.sec, _msg.stamp.sec);
234  EXPECT_EQ(expected_msg.stamp.nsec, _msg.stamp.nsec);
235  EXPECT_EQ(expected_msg.frame_id, _msg.frame_id);
236  }
237 
240  void createTestMsg(std_msgs::String &_msg)
241  {
242  _msg.data = "string";
243  }
244 
247  void compareTestMsg(const std_msgs::String &_msg)
248  {
249  std_msgs::String expected_msg;
250  createTestMsg(expected_msg);
251 
252  EXPECT_EQ(expected_msg.data, _msg.data);
253  }
254 
257  void createTestMsg(geometry_msgs::Quaternion &_msg)
258  {
259  _msg.x = 1;
260  _msg.y = 2;
261  _msg.z = 3;
262  _msg.w = 4;
263  }
264 
267  void compareTestMsg(const geometry_msgs::Quaternion &_msg)
268  {
269  geometry_msgs::Quaternion expected_msg;
270  createTestMsg(expected_msg);
271 
272  EXPECT_EQ(expected_msg.x, _msg.x);
273  EXPECT_EQ(expected_msg.y, _msg.y);
274  EXPECT_EQ(expected_msg.z, _msg.z);
275  EXPECT_EQ(expected_msg.w, _msg.w);
276  }
277 
280  void createTestMsg(geometry_msgs::Vector3 &_msg)
281  {
282  _msg.x = 1;
283  _msg.y = 2;
284  _msg.z = 3;
285  }
286 
289  void compareTestMsg(const geometry_msgs::Vector3 &_msg)
290  {
291  geometry_msgs::Vector3 expected_msg;
292  createTestMsg(expected_msg);
293 
294  EXPECT_EQ(expected_msg.x, _msg.x);
295  EXPECT_EQ(expected_msg.y, _msg.y);
296  EXPECT_EQ(expected_msg.z, _msg.z);
297  }
298 
301  void createTestMsg(rosgraph_msgs::Clock &_msg)
302  {
303  _msg.clock.sec = 1;
304  _msg.clock.nsec = 2;
305  }
306 
309  void createTestMsg(geometry_msgs::Point &_msg)
310  {
311  _msg.x = 1;
312  _msg.y = 2;
313  _msg.z = 3;
314  }
315 
318  void compareTestMsg(const rosgraph_msgs::Clock &_msg)
319  {
320  rosgraph_msgs::Clock expected_msg;
321  createTestMsg(expected_msg);
322 
323  EXPECT_EQ(expected_msg.clock.sec, _msg.clock.sec);
324  EXPECT_EQ(expected_msg.clock.nsec, _msg.clock.nsec);
325  }
326 
329  void compareTestMsg(const geometry_msgs::Point &_msg)
330  {
331  geometry_msgs::Point expected_msg;
332  createTestMsg(expected_msg);
333 
334  EXPECT_EQ(expected_msg.x, _msg.x);
335  EXPECT_EQ(expected_msg.y, _msg.y);
336  EXPECT_EQ(expected_msg.z, _msg.z);
337  }
338 
341  void createTestMsg(geometry_msgs::Pose &_msg)
342  {
343  createTestMsg(_msg.position);
344  createTestMsg(_msg.orientation);
345  }
346 
349  void compareTestMsg(const geometry_msgs::Pose &_msg)
350  {
351  compareTestMsg(_msg.position);
352  compareTestMsg(_msg.orientation);
353  }
354 
357  void createTestMsg(geometry_msgs::PoseArray &_msg)
358  {
359  geometry_msgs::Pose pose;
360  createTestMsg(pose);
361  _msg.poses.push_back(pose);
362 
363  createTestMsg(_msg.header);
364  }
365 
368  void compareTestMsg(const geometry_msgs::PoseArray &_msg)
369  {
370  compareTestMsg(_msg.poses[0]);
371  compareTestMsg(_msg.header);
372  }
373 
376  void createTestMsg(geometry_msgs::PoseStamped &_msg)
377  {
378  createTestMsg(_msg.header);
379  createTestMsg(_msg.pose);
380  }
381 
384  void compareTestMsg(const geometry_msgs::PoseStamped &_msg)
385  {
386  compareTestMsg(_msg.header);
387  compareTestMsg(_msg.pose);
388  }
389 
392  void createTestMsg(geometry_msgs::Transform &_msg)
393  {
394  createTestMsg(_msg.translation);
395  createTestMsg(_msg.rotation);
396  }
397 
400  void compareTestMsg(const geometry_msgs::Transform &_msg)
401  {
402  compareTestMsg(_msg.translation);
403  compareTestMsg(_msg.rotation);
404  }
405 
408  void createTestMsg(geometry_msgs::TransformStamped &_msg)
409  {
410  createTestMsg(_msg.header);
411  createTestMsg(_msg.transform);
412  _msg.child_frame_id = "child_frame_id_value";
413  }
414 
417  void compareTestMsg(const geometry_msgs::TransformStamped &_msg)
418  {
419  geometry_msgs::TransformStamped expected_msg;
420  createTestMsg(expected_msg);
421 
422  compareTestMsg(_msg.header);
423  compareTestMsg(_msg.transform);
424  EXPECT_EQ(expected_msg.child_frame_id, _msg.child_frame_id);
425  }
426 
429  void createTestMsg(tf2_msgs::TFMessage &_msg)
430  {
431  geometry_msgs::TransformStamped tf;
432  createTestMsg(tf);
433  _msg.transforms.push_back(tf);
434  }
435 
438  void compareTestMsg(const tf2_msgs::TFMessage &_msg)
439  {
440  tf2_msgs::TFMessage expected_msg;
441  createTestMsg(expected_msg);
442 
443  compareTestMsg(_msg.transforms[0]);
444  }
445 
448  void createTestMsg(geometry_msgs::Twist &_msg)
449  {
450  createTestMsg(_msg.linear);
451  createTestMsg(_msg.angular);
452  }
453 
456  void compareTestMsg(const geometry_msgs::Twist &_msg)
457  {
458  compareTestMsg(_msg.linear);
459  compareTestMsg(_msg.angular);
460  }
461 
464 // void createTestMsg(mav_msgs::Actuators &_msg)
465 // {
466 // createTestMsg(_msg.header);
467 // for (auto i = 0u; i < 5; ++i)
468 // {
469 // _msg.angles.push_back(i);
470 // _msg.angular_velocities.push_back(i);
471 // _msg.normalized.push_back(i);
472 // }
473 // }
474 
477 // void compareTestMsg(const mav_msgs::Actuators &_msg)
478 // {
479 // mav_msgs::Actuators expected_msg;
480 // createTestMsg(expected_msg);
481 //
482 // compareTestMsg(_msg.header);
483 //
484 // ASSERT_EQ(expected_msg.angles.size(), _msg.angles.size());
485 // ASSERT_EQ(expected_msg.angular_velocities.size(),
486 // _msg.angular_velocities.size());
487 // ASSERT_EQ(expected_msg.normalized.size(), _msg.normalized.size());
488 //
489 // for (auto i = 0u; i < _msg.angles.size(); ++i)
490 // {
491 // EXPECT_EQ(expected_msg.angles[i], _msg.angles[i]);
492 // EXPECT_EQ(expected_msg.angular_velocities[i], _msg.angular_velocities[i]);
493 // EXPECT_EQ(expected_msg.normalized[i], _msg.normalized[i]);
494 // }
495 // }
496 
499  void createTestMsg(nav_msgs::OccupancyGrid &_msg)
500  {
501  createTestMsg(_msg.header);
502 
503  _msg.info.map_load_time.sec = 100;
504  _msg.info.map_load_time.nsec = 200;
505  _msg.info.resolution = 0.05;
506  _msg.info.width = 10;
507  _msg.info.height = 20;
508  createTestMsg(_msg.info.origin);
509  _msg.data.resize(_msg.info.width * _msg.info.height, 1);
510  }
511 
514  void compareTestMsg(const nav_msgs::OccupancyGrid &_msg)
515  {
516  nav_msgs::OccupancyGrid expected_msg;
517  createTestMsg(expected_msg);
518 
519  compareTestMsg(_msg.header);
520  EXPECT_EQ(expected_msg.info.map_load_time.sec,
521  _msg.info.map_load_time.sec);
522  EXPECT_EQ(expected_msg.info.map_load_time.nsec,
523  _msg.info.map_load_time.nsec);
524  EXPECT_FLOAT_EQ(expected_msg.info.resolution, _msg.info.resolution);
525  EXPECT_EQ(expected_msg.info.width, _msg.info.width);
526  EXPECT_EQ(expected_msg.info.height, _msg.info.height);
527 
528  compareTestMsg(_msg.info.origin);
529 
530  EXPECT_EQ(expected_msg.data.size(), _msg.data.size());
531  EXPECT_EQ(expected_msg.data[0], _msg.data[0]);
532  }
533 
536  void createTestMsg(nav_msgs::Odometry &_msg)
537  {
538  createTestMsg(_msg.header);
539  createTestMsg(_msg.pose.pose);
540  createTestMsg(_msg.twist.twist);
541  }
542 
545  void compareTestMsg(const nav_msgs::Odometry &_msg)
546  {
547  compareTestMsg(_msg.header);
548  compareTestMsg(_msg.pose.pose);
549  compareTestMsg(_msg.twist.twist);
550  }
551 
554  void createTestMsg(sensor_msgs::Image &_msg)
555  {
556  std_msgs::Header header_msg;
557  createTestMsg(header_msg);
558 
559  _msg.header = header_msg;
560  _msg.width = 320;
561  _msg.height = 240;
562  _msg.encoding = "rgb8";
563  _msg.is_bigendian = false;
564  _msg.step = _msg.width * 3;
565  _msg.data.resize(_msg.height * _msg.step, '1');
566  }
567 
570  void compareTestMsg(const sensor_msgs::Image &_msg)
571  {
572  sensor_msgs::Image expected_msg;
573  createTestMsg(expected_msg);
574 
575  compareTestMsg(_msg.header);
576  EXPECT_EQ(expected_msg.width, _msg.width);
577  EXPECT_EQ(expected_msg.height, _msg.height);
578  EXPECT_EQ(expected_msg.encoding, _msg.encoding);
579  EXPECT_EQ(expected_msg.is_bigendian, _msg.is_bigendian);
580  EXPECT_EQ(expected_msg.step, _msg.step);
581  }
582 
585  void createTestMsg(sensor_msgs::CameraInfo &_msg)
586  {
587  std_msgs::Header header_msg;
588  createTestMsg(header_msg);
589 
590  _msg.header = header_msg;
591  _msg.width = 320;
592  _msg.height = 240;
593  _msg.distortion_model = "plumb_bob";
594  _msg.D.resize(5);
595  _msg.D[0] = 1;
596  _msg.D[1] = 2;
597  _msg.D[2] = 3;
598  _msg.D[3] = 4;
599  _msg.D[4] = 5;
600 
601  _msg.K[0] = 1;
602  _msg.K[1] = 0;
603  _msg.K[2] = 0;
604  _msg.K[3] = 0;
605  _msg.K[4] = 1;
606  _msg.K[5] = 0;
607  _msg.K[6] = 0;
608  _msg.K[7] = 0;
609  _msg.K[8] = 1;
610 
611  _msg.R[0] = 1;
612  _msg.R[1] = 0;
613  _msg.R[2] = 0;
614  _msg.R[3] = 0;
615  _msg.R[4] = 1;
616  _msg.R[5] = 0;
617  _msg.R[6] = 0;
618  _msg.R[7] = 0;
619  _msg.R[8] = 1;
620 
621  _msg.P[0] = 1;
622  _msg.P[1] = 0;
623  _msg.P[2] = 0;
624  _msg.P[3] = 0;
625  _msg.P[4] = 0;
626  _msg.P[5] = 1;
627  _msg.P[6] = 0;
628  _msg.P[7] = 0;
629  _msg.P[8] = 0;
630  _msg.P[9] = 0;
631  _msg.P[10] = 1;
632  _msg.P[11] = 0;
633  }
634 
637  void compareTestMsg(const sensor_msgs::CameraInfo &_msg)
638  {
639  sensor_msgs::CameraInfo expected_msg;
640  createTestMsg(expected_msg);
641 
642  compareTestMsg(_msg.header);
643  EXPECT_EQ(expected_msg.width, _msg.width);
644  EXPECT_EQ(expected_msg.height, _msg.height);
645  EXPECT_EQ(expected_msg.distortion_model, _msg.distortion_model);
646 
647  for (auto i = 0; i < 12; ++i)
648  {
649  EXPECT_EQ(expected_msg.P[i], _msg.P[i]);
650 
651  if (i > 8)
652  continue;
653 
654  EXPECT_EQ(expected_msg.K[i], _msg.K[i]);
655  EXPECT_EQ(expected_msg.R[i], _msg.R[i]);
656 
657  if (i > 4)
658  continue;
659 
660  EXPECT_EQ(expected_msg.D[i], _msg.D[i]);
661  }
662  }
663 
666  void createTestMsg(sensor_msgs::FluidPressure &_msg)
667  {
668  std_msgs::Header header_msg;
669  createTestMsg(header_msg);
670 
671  _msg.header = header_msg;
672  _msg.fluid_pressure = 0.123;
673  _msg.variance = 0.456;
674  }
675 
678  void compareTestMsg(const sensor_msgs::FluidPressure &_msg)
679  {
680  sensor_msgs::FluidPressure expected_msg;
681  createTestMsg(expected_msg);
682 
683  compareTestMsg(_msg.header);
684  EXPECT_FLOAT_EQ(expected_msg.fluid_pressure, _msg.fluid_pressure);
685  EXPECT_FLOAT_EQ(expected_msg.variance, _msg.variance);
686  }
687 
690  void createTestMsg(sensor_msgs::Imu &_msg)
691  {
692  std_msgs::Header header_msg;
693  geometry_msgs::Quaternion quaternion_msg;
694  geometry_msgs::Vector3 vector3_msg;
695 
696  createTestMsg(header_msg);
697  createTestMsg(quaternion_msg);
698  createTestMsg(vector3_msg);
699 
700  _msg.header = header_msg;
701  _msg.orientation = quaternion_msg;
702  _msg.orientation_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
703  _msg.angular_velocity = vector3_msg;
704  _msg.angular_velocity_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
705  _msg.linear_acceleration = vector3_msg;
706  _msg.linear_acceleration_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
707  }
708 
711  void compareTestMsg(const sensor_msgs::Imu &_msg)
712  {
713  compareTestMsg(_msg.header);
714  compareTestMsg(_msg.orientation);
715  compareTestMsg(_msg.angular_velocity);
716  compareTestMsg(_msg.linear_acceleration);
717 
718  for (auto i = 0; i < 9; ++i)
719  {
720  EXPECT_FLOAT_EQ(0, _msg.orientation_covariance[i]);
721  EXPECT_FLOAT_EQ(0, _msg.angular_velocity_covariance[i]);
722  EXPECT_FLOAT_EQ(0, _msg.linear_acceleration_covariance[i]);
723  }
724  }
725 
728  void createTestMsg(sensor_msgs::JointState &_msg)
729  {
730  std_msgs::Header header_msg;
731  createTestMsg(header_msg);
732 
733  _msg.header = header_msg;
734  _msg.name = {"joint_0", "joint_1", "joint_2"};
735  _msg.position = {1, 1, 1};
736  _msg.velocity = {2, 2, 2};
737  _msg.effort = {3, 3, 3};
738  }
739 
742  void compareTestMsg(const sensor_msgs::JointState &_msg)
743  {
744  sensor_msgs::JointState expected_msg;
745  createTestMsg(expected_msg);
746 
747  compareTestMsg(_msg.header);
748 
749  ASSERT_EQ(expected_msg.name.size(), _msg.name.size());
750  ASSERT_EQ(expected_msg.position.size(), _msg.position.size());
751  ASSERT_EQ(expected_msg.velocity.size(), _msg.velocity.size());
752  ASSERT_EQ(expected_msg.effort.size(), _msg.effort.size());
753 
754  for (auto i = 0u; i < _msg.position.size(); ++i)
755  {
756  EXPECT_EQ(expected_msg.name[i], _msg.name[i]);
757  EXPECT_FLOAT_EQ(expected_msg.position[i], _msg.position[i]);
758  EXPECT_FLOAT_EQ(expected_msg.velocity[i], _msg.velocity[i]);
759  EXPECT_FLOAT_EQ(expected_msg.effort[i], _msg.effort[i]);
760  }
761  }
762 
765  void createTestMsg(sensor_msgs::LaserScan &_msg)
766  {
767  const unsigned int num_readings = 100u;
768  const double laser_frequency = 40;
769 
770  std_msgs::Header header_msg;
771  createTestMsg(header_msg);
772 
773  _msg.header = header_msg;
774  _msg.angle_min = -1.57;
775  _msg.angle_max = 1.57;
776  _msg.angle_increment = 3.14 / num_readings;
777  _msg.time_increment = (1 / laser_frequency) / (num_readings);
778  _msg.scan_time = 0;
779  _msg.range_min = 1;
780  _msg.range_max = 2;
781  _msg.ranges.resize(num_readings, 0);
782  _msg.intensities.resize(num_readings, 1);
783  }
784 
787  void compareTestMsg(const sensor_msgs::LaserScan &_msg)
788  {
789  sensor_msgs::LaserScan expected_msg;
790  createTestMsg(expected_msg);
791 
792  compareTestMsg(_msg.header);
793  EXPECT_FLOAT_EQ(expected_msg.angle_min, _msg.angle_min);
794  EXPECT_FLOAT_EQ(expected_msg.angle_max, _msg.angle_max);
795  EXPECT_FLOAT_EQ(expected_msg.angle_increment, _msg.angle_increment);
796  EXPECT_FLOAT_EQ(0, _msg.time_increment);
797  EXPECT_FLOAT_EQ(0, _msg.scan_time);
798  EXPECT_FLOAT_EQ(expected_msg.range_min, _msg.range_min);
799  EXPECT_FLOAT_EQ(expected_msg.range_max, _msg.range_max);
800 
801  const unsigned int num_readings =
802  (_msg.angle_max - _msg.angle_min) / _msg.angle_increment;
803  for (auto i = 0u; i < num_readings; ++i)
804  {
805  EXPECT_FLOAT_EQ(expected_msg.ranges[i], _msg.ranges[i]);
806  EXPECT_FLOAT_EQ(expected_msg.intensities[i], _msg.intensities[i]);
807  }
808  }
809 
812  void createTestMsg(sensor_msgs::MagneticField &_msg)
813  {
814  std_msgs::Header header_msg;
815  geometry_msgs::Vector3 vector3_msg;
816 
817  createTestMsg(header_msg);
818  createTestMsg(vector3_msg);
819 
820  _msg.header = header_msg;
821  _msg.magnetic_field = vector3_msg;
822  _msg.magnetic_field_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
823  }
824 
827  void compareTestMsg(const sensor_msgs::MagneticField &_msg)
828  {
829  compareTestMsg(_msg.header);
830  compareTestMsg(_msg.magnetic_field);
831 
832  for (auto i = 0u; i < 9; ++i)
833  EXPECT_FLOAT_EQ(0, _msg.magnetic_field_covariance[i]);
834  }
835 
838  void createTestMsg(sensor_msgs::NavSatFix &_msg)
839  {
840  std_msgs::Header header_msg;
841  createTestMsg(header_msg);
842 
843  _msg.header = header_msg;
844  _msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
845  _msg.latitude = 0.00;
846  _msg.longitude = 0.00;
847  _msg.altitude = 0.00;
848  _msg.position_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9};
849  _msg.position_covariance_type =
850  sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
851  }
852 
855  void compareTestMsg(const sensor_msgs::NavSatFix &_msg)
856  {
857  sensor_msgs::NavSatFix expected_msg;
858  createTestMsg(expected_msg);
859 
860  compareTestMsg(_msg.header);
861  EXPECT_EQ(expected_msg.status, _msg.status);
862  EXPECT_FLOAT_EQ(expected_msg.latitude, _msg.latitude);
863  EXPECT_FLOAT_EQ(expected_msg.longitude, _msg.longitude);
864  EXPECT_FLOAT_EQ(expected_msg.altitude, _msg.altitude);
865  EXPECT_EQ(expected_msg.position_covariance_type,
866  _msg.position_covariance_type);
867 
868  for (auto i = 0u; i < 9; ++i)
869  EXPECT_FLOAT_EQ(0, _msg.position_covariance[i]);
870  }
871 
874  void createTestMsg(sensor_msgs::PointCloud2 &_msg)
875  {
876  createTestMsg(_msg.header);
877 
878  sensor_msgs::PointField field;
879  field.name = "x";
880  field.offset = 0;
881  field.datatype = sensor_msgs::PointField::FLOAT32;
882  field.count = 1;
883  _msg.fields.push_back(field);
884 
885  uint32_t height = 4;
886  uint32_t width = 2;
887 
888  _msg.height = height;
889  _msg.width = width;
890  _msg.is_bigendian = false;
891  _msg.point_step = 4;
892  _msg.row_step = 4 * width;
893  _msg.is_dense = true;
894 
895  _msg.data.resize(_msg.row_step * _msg.height);
896  uint8_t *msgBufferIndex = _msg.data.data();
897 
898  for (uint32_t j = 0; j < height; ++j)
899  {
900  for (uint32_t i = 0; i < width; ++i)
901  {
902  *reinterpret_cast<float*>(msgBufferIndex + _msg.fields[0].offset) =
903  j * width + i;
904  msgBufferIndex += _msg.point_step;
905  }
906  }
907  }
908 
911  void compareTestMsg(const sensor_msgs::PointCloud2 &_msg)
912  {
913  compareTestMsg(_msg.header);
914 
915  uint32_t height = 4;
916  uint32_t width = 2;
917 
918  EXPECT_EQ(height, _msg.height);
919  EXPECT_EQ(width, _msg.width);
920  EXPECT_FALSE(_msg.is_bigendian);
921  EXPECT_EQ(4u, _msg.point_step);
922  EXPECT_EQ(4U * width, _msg.row_step);
923  EXPECT_TRUE(_msg.is_dense);
924 
925  unsigned char *msgBufferIndex =
926  const_cast<unsigned char*>(_msg.data.data());
927 
928  for (uint32_t j = 0; j < height; ++j)
929  {
930  for (uint32_t i = 0; i < width; ++i)
931  {
932  float *value =
933  reinterpret_cast<float*>(msgBufferIndex + _msg.fields[0].offset);
934 
935  EXPECT_FLOAT_EQ(static_cast<float>(j * width + i), *value);
936  msgBufferIndex += _msg.point_step;
937  }
938  }
939  }
940 
943  void createTestMsg(sensor_msgs::BatteryState &_msg)
944  {
945  std_msgs::Header header_msg;
946  createTestMsg(header_msg);
947 
948  _msg.header = header_msg;
949  _msg.voltage = 123;
950  _msg.current = 456;
951  _msg.charge = 789;
952  _msg.capacity = 321;
953  _msg.percentage = 654;
954  _msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
955  }
956 
959  void compareTestMsg(const sensor_msgs::BatteryState &_msg)
960  {
961  sensor_msgs::BatteryState expected_msg;
962  createTestMsg(expected_msg);
963 
964  compareTestMsg(_msg.header);
965  EXPECT_EQ(expected_msg.voltage, _msg.voltage);
966  EXPECT_EQ(expected_msg.current, _msg.current);
967  EXPECT_EQ(expected_msg.charge, _msg.charge);
968  EXPECT_EQ(expected_msg.capacity, _msg.capacity);
969  EXPECT_EQ(expected_msg.percentage, _msg.percentage);
970  EXPECT_EQ(expected_msg.power_supply_status, _msg.power_supply_status);
971  }
972 
975  void createTestMsg(visualization_msgs::Marker &_msg)
976  {
977  createTestMsg(_msg.header);
978 
979  _msg.ns = "foo";
980  _msg.id = 123;
981  _msg.type = visualization_msgs::Marker::CUBE;
982  _msg.action = visualization_msgs::Marker::ADD;
983 
984  createTestMsg(_msg.pose);
985  createTestMsg(_msg.scale);
986  createTestMsg(_msg.color);
987 
988  _msg.lifetime.sec = 100;
989  _msg.lifetime.nsec = 200;
990 
991  geometry_msgs::Point pt;
992  createTestMsg(pt);
993  _msg.points.push_back(pt);
994  _msg.text = "bar";
995  }
996 
999  void compareTestMsg(const visualization_msgs::Marker &_msg)
1000  {
1001  visualization_msgs::Marker expected_msg;
1002  createTestMsg(expected_msg);
1003 
1004  compareTestMsg(_msg.header);
1005 
1006  EXPECT_EQ(expected_msg.ns, _msg.ns);
1007  EXPECT_EQ(expected_msg.id, _msg.id);
1008  EXPECT_EQ(expected_msg.type, _msg.type);
1009  EXPECT_EQ(expected_msg.action, _msg.action);
1010 
1011  compareTestMsg(_msg.pose);
1012  compareTestMsg(_msg.scale);
1013  compareTestMsg(_msg.color);
1014 
1015  EXPECT_EQ(expected_msg.lifetime.sec, _msg.lifetime.sec);
1016  EXPECT_EQ(expected_msg.lifetime.nsec, _msg.lifetime.nsec);
1017 
1018  compareTestMsg(_msg.points[0]);
1019 
1020  EXPECT_EQ(expected_msg.text, _msg.text);
1021  }
1022 
1025  void createTestMsg(visualization_msgs::MarkerArray &_msg)
1026  {
1027  _msg.markers.clear();
1028  visualization_msgs::Marker marker;
1029  createTestMsg(marker);
1030  _msg.markers.push_back(marker);
1031  }
1032 
1035  void compareTestMsg(const visualization_msgs::MarkerArray &_msg)
1036  {
1037  visualization_msgs::MarkerArray expected_msg;
1038  createTestMsg(expected_msg);
1039 
1040  EXPECT_EQ(1u, _msg.markers.size());
1041  compareTestMsg(_msg.markers[0]);
1042  }
1043 
1047 
1050  void createTestMsg(ignition::msgs::Boolean &_msg)
1051  {
1052  _msg.set_data(true);
1053  }
1054 
1057  void compareTestMsg(const ignition::msgs::Boolean &_msg)
1058  {
1059  ignition::msgs::Boolean expected_msg;
1060  createTestMsg(expected_msg);
1061 
1062  EXPECT_EQ(expected_msg.data(), _msg.data());
1063  }
1064 
1067  void createTestMsg(ignition::msgs::Color &_msg)
1068  {
1069  _msg.set_r(10.0);
1070  _msg.set_g(11.0);
1071  _msg.set_b(12.0);
1072  _msg.set_a(13.0);
1073  }
1074 
1077  void compareTestMsg(const ignition::msgs::Color &_msg)
1078  {
1079  ignition::msgs::Color expected_msg;
1080  createTestMsg(expected_msg);
1081 
1082  EXPECT_EQ(expected_msg.r(), _msg.r());
1083  EXPECT_EQ(expected_msg.g(), _msg.g());
1084  EXPECT_EQ(expected_msg.b(), _msg.b());
1085  EXPECT_EQ(expected_msg.a(), _msg.a());
1086  }
1087 
1090  void compareTestMsg(const ignition::msgs::Empty &)
1091  {
1092  }
1093 
1096  void createTestMsg(ignition::msgs::Int32 &_msg)
1097  {
1098  _msg.set_data(5);
1099  }
1100 
1103  void compareTestMsg(const ignition::msgs::Int32 &_msg)
1104  {
1105  ignition::msgs::Int32 expected_msg;
1106  createTestMsg(expected_msg);
1107 
1108  EXPECT_EQ(expected_msg.data(), _msg.data());
1109  }
1110 
1113  void createTestMsg(ignition::msgs::Float &_msg)
1114  {
1115  _msg.set_data(1.5);
1116  }
1117 
1120  void compareTestMsg(const ignition::msgs::Float &_msg)
1121  {
1122  ignition::msgs::Float expected_msg;
1123  createTestMsg(expected_msg);
1124 
1125  EXPECT_FLOAT_EQ(expected_msg.data(), _msg.data());
1126  }
1127 
1130  void createTestMsg(ignition::msgs::Double &_msg)
1131  {
1132  _msg.set_data(1.5);
1133  }
1134 
1137  void compareTestMsg(const ignition::msgs::Double &_msg)
1138  {
1139  ignition::msgs::Double expected_msg;
1140  createTestMsg(expected_msg);
1141 
1142  EXPECT_DOUBLE_EQ(expected_msg.data(), _msg.data());
1143  }
1144 
1147  void createTestMsg(ignition::msgs::Header &_msg)
1148  {
1149  auto seq_entry = _msg.add_data();
1150  seq_entry->set_key("seq");
1151  seq_entry->add_value("1");
1152  _msg.mutable_stamp()->set_sec(2);
1153  _msg.mutable_stamp()->set_nsec(3);
1154  auto frame_id_entry = _msg.add_data();
1155  frame_id_entry->set_key("frame_id");
1156  frame_id_entry->add_value("frame_id_value");
1157  }
1158 
1161  void compareTestMsg(const ignition::msgs::Header &_msg)
1162  {
1163  ignition::msgs::Header expected_msg;
1164  createTestMsg(expected_msg);
1165 
1166  EXPECT_EQ(expected_msg.stamp().sec(), _msg.stamp().sec());
1167  EXPECT_EQ(expected_msg.stamp().nsec(), _msg.stamp().nsec());
1168  EXPECT_GE(_msg.data_size(), 2);
1169  EXPECT_EQ(expected_msg.data(0).key(), _msg.data(0).key());
1170  EXPECT_EQ(1, _msg.data(0).value_size());
1171  std::string value = _msg.data(0).value(0);
1172  try
1173  {
1174  uint32_t ul = std::stoul(value, nullptr);
1175  EXPECT_GE(ul, 0u);
1176  }
1177  catch (std::exception & e)
1178  {
1179  FAIL();
1180  }
1181  EXPECT_EQ(expected_msg.data(1).key(), _msg.data(1).key());
1182  EXPECT_EQ(1, _msg.data(1).value_size());
1183  EXPECT_EQ(expected_msg.data(1).value(0), _msg.data(1).value(0));
1184  }
1185 
1188  void createTestMsg(ignition::msgs::Clock &_msg)
1189  {
1190  _msg.mutable_sim()->set_sec(1);
1191  _msg.mutable_sim()->set_nsec(2);
1192  }
1193 
1196  void compareTestMsg(const ignition::msgs::Clock &_msg)
1197  {
1198  ignition::msgs::Clock expected_msg;
1199  createTestMsg(expected_msg);
1200 
1201  EXPECT_EQ(expected_msg.sim().sec(), _msg.sim().sec());
1202  EXPECT_EQ(expected_msg.sim().nsec(), _msg.sim().nsec());
1203  }
1204 
1207  void createTestMsg(ignition::msgs::StringMsg &_msg)
1208  {
1209  _msg.set_data("string");
1210  }
1211 
1214  void compareTestMsg(const ignition::msgs::StringMsg &_msg)
1215  {
1216  ignition::msgs::StringMsg expected_msg;
1217  createTestMsg(expected_msg);
1218 
1219  EXPECT_EQ(expected_msg.data(), _msg.data());
1220  }
1221 
1224  void createTestMsg(ignition::msgs::Quaternion &_msg)
1225  {
1226  _msg.set_x(1.0);
1227  _msg.set_y(2.0);
1228  _msg.set_z(3.0);
1229  _msg.set_w(4.0);
1230  }
1231 
1234  void compareTestMsg(const ignition::msgs::Quaternion &_msg)
1235  {
1236  ignition::msgs::Quaternion expected_msg;
1237  createTestMsg(expected_msg);
1238 
1239  EXPECT_EQ(expected_msg.x(), _msg.x());
1240  EXPECT_EQ(expected_msg.y(), _msg.y());
1241  EXPECT_EQ(expected_msg.z(), _msg.z());
1242  EXPECT_EQ(expected_msg.w(), _msg.w());
1243  }
1244 
1247  void createTestMsg(ignition::msgs::Vector3d &_msg)
1248  {
1249  _msg.set_x(1.0);
1250  _msg.set_y(2.0);
1251  _msg.set_z(3.0);
1252  }
1253 
1256  void compareTestMsg(const ignition::msgs::Vector3d &_msg)
1257  {
1258  ignition::msgs::Vector3d expected_msg;
1259  createTestMsg(expected_msg);
1260 
1261  EXPECT_EQ(expected_msg.x(), _msg.x());
1262  EXPECT_EQ(expected_msg.y(), _msg.y());
1263  EXPECT_EQ(expected_msg.z(), _msg.z());
1264  }
1265 
1268  void createTestMsg(ignition::msgs::Pose &_msg)
1269  {
1270  createTestMsg(*_msg.mutable_header());
1271  auto child_frame_id_entry = _msg.mutable_header()->add_data();
1272  child_frame_id_entry->set_key("child_frame_id");
1273  child_frame_id_entry->add_value("child_frame_id_value");
1274 
1275  createTestMsg(*_msg.mutable_position());
1276  createTestMsg(*_msg.mutable_orientation());
1277  }
1278 
1281  void compareTestMsg(const ignition::msgs::Pose &_msg)
1282  {
1283  if (_msg.header().data_size() > 0)
1284  {
1285  compareTestMsg(_msg.header());
1286 
1287  ignition::msgs::Pose expected_msg;
1288  createTestMsg(expected_msg);
1289 
1290  if (_msg.header().data_size() > 2)
1291  {
1292  // child_frame_id
1293  ASSERT_EQ(3, expected_msg.header().data_size());
1294  ASSERT_EQ(3, _msg.header().data_size());
1295  EXPECT_EQ(expected_msg.header().data(2).key(),
1296  _msg.header().data(2).key());
1297  EXPECT_EQ(1, _msg.header().data(2).value_size());
1298  EXPECT_EQ(expected_msg.header().data(2).value(0),
1299  _msg.header().data(2).value(0));
1300  }
1301  }
1302 
1303  compareTestMsg(_msg.position());
1304  compareTestMsg(_msg.orientation());
1305  }
1306 
1309  void createTestMsg(ignition::msgs::Pose_V &_msg)
1310  {
1311  createTestMsg(*_msg.mutable_header());
1312  createTestMsg(*_msg.add_pose());
1313  }
1314 
1317  void compareTestMsg(const ignition::msgs::Pose_V &_msg)
1318  {
1319  ignition::msgs::Pose_V expected_msg;
1320  createTestMsg(expected_msg);
1321 
1322  compareTestMsg(_msg.header());
1323  compareTestMsg(_msg.pose(0));
1324  }
1325 
1328  void createTestMsg(ignition::msgs::Twist &_msg)
1329  {
1330  ignition::msgs::Header header_msg;
1331  ignition::msgs::Vector3d linear_msg;
1332  ignition::msgs::Vector3d angular_msg;
1333 
1334  createTestMsg(header_msg);
1335  createTestMsg(linear_msg);
1336  createTestMsg(angular_msg);
1337 
1338  _msg.mutable_header()->CopyFrom(header_msg);
1339  _msg.mutable_linear()->CopyFrom(linear_msg);
1340  _msg.mutable_angular()->CopyFrom(angular_msg);
1341  }
1342 
1345  void compareTestMsg(const ignition::msgs::Twist &_msg)
1346  {
1347  compareTestMsg(_msg.linear());
1348  compareTestMsg(_msg.angular());
1349  }
1350 
1353  void createTestMsg(ignition::msgs::Image &_msg)
1354  {
1355  ignition::msgs::Header header_msg;
1356  createTestMsg(header_msg);
1357 
1358  _msg.mutable_header()->CopyFrom(header_msg);
1359  _msg.set_width(320);
1360  _msg.set_height(240);
1361  _msg.set_pixel_format_type(ignition::msgs::PixelFormatType::RGB_INT8);
1362  _msg.set_step(_msg.width() * 3);
1363  _msg.set_data(std::string(_msg.height() * _msg.step(), '1'));
1364  }
1365 
1368  void compareTestMsg(const ignition::msgs::Image &_msg)
1369  {
1370  ignition::msgs::Image expected_msg;
1371  createTestMsg(expected_msg);
1372 
1373  compareTestMsg(_msg.header());
1374  EXPECT_EQ(expected_msg.width(), _msg.width());
1375  EXPECT_EQ(expected_msg.height(), _msg.height());
1376  EXPECT_EQ(expected_msg.pixel_format_type(), _msg.pixel_format_type());
1377  EXPECT_EQ(expected_msg.step(), _msg.step());
1378  EXPECT_EQ(expected_msg.data(), _msg.data());
1379  }
1380 
1383  void createTestMsg(ignition::msgs::CameraInfo &_msg)
1384  {
1385  ignition::msgs::Header header_msg;
1386  createTestMsg(header_msg);
1387 
1388  _msg.mutable_header()->CopyFrom(header_msg);
1389  _msg.set_width(320);
1390  _msg.set_height(240);
1391 
1392  auto distortion = _msg.mutable_distortion();
1393  distortion->set_model(ignition::msgs::CameraInfo::Distortion::PLUMB_BOB);
1394  distortion->add_k(1);
1395  distortion->add_k(2);
1396  distortion->add_k(3);
1397  distortion->add_k(4);
1398  distortion->add_k(5);
1399 
1400  auto intrinsics = _msg.mutable_intrinsics();
1401  intrinsics->add_k(1);
1402  intrinsics->add_k(0);
1403  intrinsics->add_k(0);
1404  intrinsics->add_k(0);
1405  intrinsics->add_k(1);
1406  intrinsics->add_k(0);
1407  intrinsics->add_k(0);
1408  intrinsics->add_k(0);
1409  intrinsics->add_k(1);
1410 
1411  auto projection = _msg.mutable_projection();
1412  projection->add_p(1);
1413  projection->add_p(0);
1414  projection->add_p(0);
1415  projection->add_p(0);
1416  projection->add_p(0);
1417  projection->add_p(1);
1418  projection->add_p(0);
1419  projection->add_p(0);
1420  projection->add_p(0);
1421  projection->add_p(0);
1422  projection->add_p(1);
1423  projection->add_p(0);
1424 
1425  _msg.add_rectification_matrix(1);
1426  _msg.add_rectification_matrix(0);
1427  _msg.add_rectification_matrix(0);
1428  _msg.add_rectification_matrix(0);
1429  _msg.add_rectification_matrix(1);
1430  _msg.add_rectification_matrix(0);
1431  _msg.add_rectification_matrix(0);
1432  _msg.add_rectification_matrix(0);
1433  _msg.add_rectification_matrix(1);
1434  }
1435 
1438  void compareTestMsg(const ignition::msgs::CameraInfo &_msg)
1439  {
1440  ignition::msgs::CameraInfo expected_msg;
1441  createTestMsg(expected_msg);
1442 
1443  ASSERT_TRUE(expected_msg.has_header());
1444  ASSERT_TRUE(_msg.has_header());
1445 
1446  compareTestMsg(_msg.header());
1447  EXPECT_EQ(expected_msg.width(), _msg.width());
1448  EXPECT_EQ(expected_msg.height(), _msg.height());
1449 
1450  ASSERT_TRUE(expected_msg.has_distortion());
1451  ASSERT_TRUE(_msg.has_distortion());
1452 
1453  auto distortion = _msg.distortion();
1454  auto expected_distortion = expected_msg.distortion();
1455  EXPECT_EQ(expected_distortion.model(), distortion.model());
1456  ASSERT_EQ(expected_distortion.k_size(), distortion.k_size());
1457  for (auto i = 0; i < expected_distortion.k_size(); ++i)
1458  EXPECT_EQ(expected_distortion.k(i), distortion.k(i));
1459 
1460  ASSERT_TRUE(expected_msg.has_intrinsics());
1461  ASSERT_TRUE(_msg.has_intrinsics());
1462 
1463  auto intrinsics = _msg.intrinsics();
1464  auto expected_intrinsics = expected_msg.intrinsics();
1465  ASSERT_EQ(expected_intrinsics.k_size(), intrinsics.k_size());
1466  for (auto i = 0; i < expected_intrinsics.k_size(); ++i)
1467  EXPECT_EQ(expected_intrinsics.k(i), intrinsics.k(i));
1468 
1469  ASSERT_TRUE(expected_msg.has_projection());
1470  ASSERT_TRUE(_msg.has_projection());
1471 
1472  auto projection = _msg.projection();
1473  auto expected_projection = expected_msg.projection();
1474  ASSERT_EQ(expected_projection.p_size(), projection.p_size());
1475  for (auto i = 0; i < expected_projection.p_size(); ++i)
1476  EXPECT_EQ(expected_projection.p(i), projection.p(i));
1477 
1478  ASSERT_EQ(expected_msg.rectification_matrix_size(), _msg.rectification_matrix_size());
1479  for (auto i = 0; i < expected_msg.rectification_matrix_size(); ++i)
1480  EXPECT_EQ(expected_msg.rectification_matrix(i), _msg.rectification_matrix(i));
1481  }
1482 
1485  void createTestMsg(ignition::msgs::FluidPressure &_msg)
1486  {
1487  ignition::msgs::Header header_msg;
1488  createTestMsg(header_msg);
1489 
1490  _msg.mutable_header()->CopyFrom(header_msg);
1491  _msg.set_pressure(0.123);
1492  _msg.set_variance(0.456);
1493  }
1494 
1497  void compareTestMsg(const ignition::msgs::FluidPressure &_msg)
1498  {
1499  ignition::msgs::FluidPressure expected_msg;
1500  createTestMsg(expected_msg);
1501 
1502  compareTestMsg(_msg.header());
1503  EXPECT_FLOAT_EQ(expected_msg.pressure(), _msg.pressure());
1504  EXPECT_FLOAT_EQ(expected_msg.variance(), _msg.variance());
1505  }
1506 
1509  void createTestMsg(ignition::msgs::IMU &_msg)
1510  {
1511  ignition::msgs::Header header_msg;
1512  ignition::msgs::Quaternion quaternion_msg;
1513  ignition::msgs::Vector3d vector3_msg;
1514 
1515  createTestMsg(header_msg);
1516  createTestMsg(quaternion_msg);
1517  createTestMsg(vector3_msg);
1518 
1519  _msg.mutable_header()->CopyFrom(header_msg);
1520  _msg.mutable_orientation()->CopyFrom(quaternion_msg);
1521  _msg.mutable_angular_velocity()->CopyFrom(vector3_msg);
1522  _msg.mutable_linear_acceleration()->CopyFrom(vector3_msg);
1523  }
1524 
1527  void compareTestMsg(const ignition::msgs::IMU &_msg)
1528  {
1529  compareTestMsg(_msg.header());
1530  compareTestMsg(_msg.orientation());
1531  compareTestMsg(_msg.angular_velocity());
1532  compareTestMsg(_msg.linear_acceleration());
1533  }
1534 
1537  void createTestMsg(ignition::msgs::Axis &_msg)
1538  {
1539  _msg.set_position(1.0);
1540  _msg.set_velocity(2.0);
1541  _msg.set_force(3.0);
1542  }
1543 
1546  void compareTestMsg(const ignition::msgs::Axis &_msg)
1547  {
1548  ignition::msgs::Axis expected_msg;
1549  createTestMsg(expected_msg);
1550 
1551  EXPECT_DOUBLE_EQ(expected_msg.position(), _msg.position());
1552  EXPECT_DOUBLE_EQ(expected_msg.velocity(), _msg.velocity());
1553  EXPECT_DOUBLE_EQ(expected_msg.force(), _msg.force());
1554  }
1555 
1558  void createTestMsg(ignition::msgs::Model &_msg)
1559  {
1560  ignition::msgs::Header header_msg;
1561  createTestMsg(header_msg);
1562  _msg.mutable_header()->CopyFrom(header_msg);
1563 
1564  for (auto i = 0; i < 3; ++i)
1565  {
1566  auto newJoint = _msg.add_joint();
1567  newJoint->set_name("joint_" + std::to_string(i));
1568 
1569  ignition::msgs::Axis axis_msg;
1570  createTestMsg(axis_msg);
1571  newJoint->mutable_axis1()->CopyFrom(axis_msg);
1572  }
1573  }
1574 
1577  void compareTestMsg(const ignition::msgs::Model &_msg)
1578  {
1579  ignition::msgs::Model expected_msg;
1580  createTestMsg(expected_msg);
1581 
1582  compareTestMsg(_msg.header());
1583 
1584  ASSERT_EQ(expected_msg.joint_size(), _msg.joint_size());
1585  for (auto i = 0; i < _msg.joint_size(); ++i)
1586  {
1587  EXPECT_EQ(expected_msg.joint(i).name(), _msg.joint(i).name());
1588  compareTestMsg(_msg.joint(i).axis1());
1589  }
1590  }
1591 
1594  void createTestMsg(ignition::msgs::LaserScan &_msg)
1595  {
1596  ignition::msgs::Header header_msg;
1597  createTestMsg(header_msg);
1598 
1599  const unsigned int num_readings = 100u;
1600  _msg.mutable_header()->CopyFrom(header_msg);
1601  _msg.set_frame("frame_id_value");
1602  _msg.set_angle_min(-1.57);
1603  _msg.set_angle_max(1.57);
1604  _msg.set_angle_step(3.14 / num_readings);
1605  _msg.set_range_min(1);
1606  _msg.set_range_max(2);
1607  _msg.set_count(num_readings);
1608  _msg.set_vertical_angle_min(0);
1609  _msg.set_vertical_angle_max(0);
1610  _msg.set_vertical_angle_step(0);
1611  _msg.set_vertical_count(0);
1612 
1613  for (auto i = 0u; i < _msg.count(); ++i)
1614  {
1615  _msg.add_ranges(0);
1616  _msg.add_intensities(1);
1617  }
1618  }
1619 
1622  void compareTestMsg(const ignition::msgs::LaserScan &_msg)
1623  {
1624  ignition::msgs::LaserScan expected_msg;
1625  createTestMsg(expected_msg);
1626 
1627  compareTestMsg(_msg.header());
1628  EXPECT_FLOAT_EQ(expected_msg.angle_min(), _msg.angle_min());
1629  EXPECT_FLOAT_EQ(expected_msg.angle_max(), _msg.angle_max());
1630  EXPECT_FLOAT_EQ(expected_msg.angle_step(), _msg.angle_step());
1631  EXPECT_DOUBLE_EQ(expected_msg.range_min(), _msg.range_min());
1632  EXPECT_DOUBLE_EQ(expected_msg.range_max(), _msg.range_max());
1633  EXPECT_EQ(expected_msg.count(), _msg.count());
1634  EXPECT_DOUBLE_EQ(expected_msg.vertical_angle_min(),
1635  _msg.vertical_angle_min());
1636  EXPECT_DOUBLE_EQ(expected_msg.vertical_angle_max(),
1637  _msg.vertical_angle_max());
1638  EXPECT_DOUBLE_EQ(expected_msg.vertical_angle_step(),
1639  _msg.vertical_angle_step());
1640  EXPECT_EQ(expected_msg.vertical_count(), _msg.vertical_count());
1641  EXPECT_EQ(expected_msg.ranges_size(), _msg.ranges_size());
1642  EXPECT_EQ(expected_msg.intensities_size(), _msg.intensities_size());
1643 
1644  for (auto i = 0u; i < _msg.count(); ++i)
1645  {
1646  EXPECT_DOUBLE_EQ(expected_msg.ranges(i), _msg.ranges(i));
1647  EXPECT_DOUBLE_EQ(expected_msg.intensities(i), _msg.intensities(i));
1648  }
1649  }
1650 
1653  void createTestMsg(ignition::msgs::Magnetometer &_msg)
1654  {
1655  ignition::msgs::Header header_msg;
1656  ignition::msgs::Vector3d vector3_msg;
1657 
1658  createTestMsg(header_msg);
1659  createTestMsg(vector3_msg);
1660 
1661  _msg.mutable_header()->CopyFrom(header_msg);
1662  _msg.mutable_field_tesla()->CopyFrom(vector3_msg);
1663  }
1664 
1667  void compareTestMsg(const ignition::msgs::Magnetometer &_msg)
1668  {
1669  compareTestMsg(_msg.header());
1670  compareTestMsg(_msg.field_tesla());
1671  }
1672 
1675  void createTestMsg(ignition::msgs::NavSat &_msg)
1676  {
1677  ignition::msgs::Header header_msg;
1678  createTestMsg(header_msg);
1679 
1680  _msg.mutable_header()->CopyFrom(header_msg);
1681  _msg.set_frame_id("frame_id_value");
1682  _msg.set_latitude_deg(0.00);
1683  _msg.set_longitude_deg(0.00);
1684  _msg.set_altitude(0.00);
1685  _msg.set_velocity_east(0.00);
1686  _msg.set_velocity_north(0.00);
1687  _msg.set_velocity_up(0.00);
1688  }
1689 
1692  void compareTestMsg(const ignition::msgs::NavSat &_msg)
1693  {
1694  ignition::msgs::NavSat expected_msg;
1695  createTestMsg(expected_msg);
1696 
1697  compareTestMsg(_msg.header());
1698  EXPECT_FLOAT_EQ(expected_msg.latitude_deg(), _msg.latitude_deg());
1699  EXPECT_FLOAT_EQ(expected_msg.longitude_deg(), _msg.longitude_deg());
1700  EXPECT_FLOAT_EQ(expected_msg.altitude(), _msg.altitude());
1701  EXPECT_FLOAT_EQ(expected_msg.velocity_east(), _msg.velocity_east());
1702  EXPECT_FLOAT_EQ(expected_msg.velocity_north(), _msg.velocity_north());
1703  EXPECT_FLOAT_EQ(expected_msg.velocity_up(), _msg.velocity_up());
1704  }
1705 
1708  void createTestMsg(ignition::msgs::Actuators &_msg)
1709  {
1710  ignition::msgs::Header header_msg;
1711 
1712  createTestMsg(header_msg);
1713  _msg.mutable_header()->CopyFrom(header_msg);
1714 
1715  for (int i = 0u; i < 5; ++i)
1716  {
1717  _msg.add_position(i);
1718  _msg.add_velocity(i);
1719  _msg.add_normalized(i);
1720  }
1721  }
1722 
1725  void compareTestMsg(const ignition::msgs::Actuators &_msg)
1726  {
1727  ignition::msgs::Actuators expected_msg;
1728  createTestMsg(expected_msg);
1729 
1730  compareTestMsg(_msg.header());
1731 
1732  for (int i = 0; i < expected_msg.position_size(); ++i)
1733  {
1734  EXPECT_EQ(expected_msg.position(i), _msg.position(i));
1735  EXPECT_EQ(expected_msg.velocity(i), _msg.velocity(i));
1736  EXPECT_EQ(expected_msg.normalized(i), _msg.normalized(i));
1737  }
1738  }
1739 
1742  void createTestMsg(ignition::msgs::OccupancyGrid &_msg)
1743  {
1744  ignition::msgs::Header header_msg;
1745  ignition::msgs::Pose pose_msg;
1746 
1747  createTestMsg(header_msg);
1748  createTestMsg(pose_msg);
1749 
1750  _msg.mutable_header()->CopyFrom(header_msg);
1751 
1752  _msg.mutable_info()->mutable_map_load_time()->set_sec(100);
1753  _msg.mutable_info()->mutable_map_load_time()->set_nsec(200);
1754  _msg.mutable_info()->set_resolution(0.05);
1755  _msg.mutable_info()->set_width(10);
1756  _msg.mutable_info()->set_height(20);
1757 
1758  _msg.mutable_info()->mutable_origin()->CopyFrom(pose_msg);
1759 
1760  std::vector<int8_t> data(_msg.info().height() * _msg.info().width(), 1);
1761  _msg.set_data(&data[0], data.size());
1762  }
1763 
1766  void compareTestMsg(const ignition::msgs::OccupancyGrid &_msg)
1767  {
1768  compareTestMsg(_msg.header());
1769 
1770  EXPECT_EQ(100, _msg.info().map_load_time().sec());
1771  EXPECT_EQ(200, _msg.info().map_load_time().nsec());
1772  EXPECT_FLOAT_EQ(0.05, _msg.info().resolution());
1773  EXPECT_EQ(10u, _msg.info().width());
1774  EXPECT_EQ(20u, _msg.info().height());
1775  compareTestMsg(_msg.info().origin());
1776 
1777  EXPECT_EQ(20u * 10u, _msg.data().size());
1778  EXPECT_EQ('\1', _msg.data()[0]);
1779  }
1780 
1783  void createTestMsg(ignition::msgs::Odometry &_msg)
1784  {
1785  ignition::msgs::Header header_msg;
1786  ignition::msgs::Pose pose_msg;
1787  ignition::msgs::Twist twist_msg;
1788 
1789  createTestMsg(header_msg);
1790  createTestMsg(pose_msg);
1791  createTestMsg(twist_msg);
1792 
1793  _msg.mutable_header()->CopyFrom(header_msg);
1794  _msg.mutable_pose()->CopyFrom(pose_msg);
1795  _msg.mutable_twist()->CopyFrom(twist_msg);
1796  }
1797 
1800  void compareTestMsg(const ignition::msgs::Odometry &_msg)
1801  {
1802  compareTestMsg(_msg.header());
1803  compareTestMsg(_msg.pose());
1804  compareTestMsg(_msg.twist());
1805  }
1806 
1809  void createTestMsg(ignition::msgs::PointCloudPacked &_msg)
1810  {
1811  ignition::msgs::Header header_msg;
1812 
1813  createTestMsg(header_msg);
1814 
1815  _msg.mutable_header()->CopyFrom(header_msg);
1816  ignition::msgs::PointCloudPacked::Field *field = _msg.add_field();
1817  field->set_name("x");
1818  field->set_offset(0);
1819  field->set_datatype(ignition::msgs::PointCloudPacked::Field::FLOAT32);
1820  field->set_count(1);
1821 
1822  uint32_t height = 4;
1823  uint32_t width = 2;
1824 
1825  _msg.set_height(height);
1826  _msg.set_width(width);
1827  _msg.set_is_bigendian(false);
1828  _msg.set_point_step(4);
1829  _msg.set_row_step(4 * width);
1830  _msg.set_is_dense(true);
1831 
1832  std::string *msgBuffer = _msg.mutable_data();
1833  msgBuffer->resize(_msg.row_step() * _msg.height());
1834  char *msgBufferIndex = msgBuffer->data();
1835 
1836  for (uint32_t j = 0; j < height; ++j)
1837  {
1838  for (uint32_t i = 0; i < width; ++i)
1839  {
1840  *reinterpret_cast<float*>(msgBufferIndex + _msg.field(0).offset()) =
1841  j * width + i;
1842  msgBufferIndex += _msg.point_step();
1843  }
1844  }
1845  }
1846 
1849  void compareTestMsg(const ignition::msgs::PointCloudPacked &_msg)
1850  {
1851  compareTestMsg(_msg.header());
1852 
1853  uint32_t height = 4;
1854  uint32_t width = 2;
1855 
1856  EXPECT_EQ(height, _msg.height());
1857  EXPECT_EQ(width, _msg.width());
1858  EXPECT_FALSE(_msg.is_bigendian());
1859  EXPECT_EQ(4u, _msg.point_step());
1860  EXPECT_EQ(4U * width, _msg.row_step());
1861  EXPECT_TRUE(_msg.is_dense());
1862 
1863  char *msgBufferIndex = const_cast<char*>(_msg.data().data());
1864 
1865  for (uint32_t j = 0; j < height; ++j)
1866  {
1867  for (uint32_t i = 0; i < width; ++i)
1868  {
1869  float *value =
1870  reinterpret_cast<float*>(msgBufferIndex + _msg.field(0).offset());
1871 
1872  EXPECT_FLOAT_EQ(static_cast<float>(j * width + i), *value);
1873  msgBufferIndex += _msg.point_step();
1874  }
1875  }
1876  }
1877 
1880  void createTestMsg(ignition::msgs::BatteryState &_msg)
1881  {
1882  ignition::msgs::Header header_msg;
1883  createTestMsg(header_msg);
1884 
1885  _msg.mutable_header()->CopyFrom(header_msg);
1886  _msg.set_voltage(123);
1887  _msg.set_current(456);
1888  _msg.set_charge(789);
1889  _msg.set_capacity(321);
1890  _msg.set_percentage(654);
1891  _msg.set_power_supply_status(ignition::msgs::BatteryState::DISCHARGING);
1892  }
1893 
1896  void compareTestMsg(const ignition::msgs::BatteryState &_msg)
1897  {
1898  ignition::msgs::BatteryState expected_msg;
1899  createTestMsg(expected_msg);
1900 
1901  ASSERT_TRUE(expected_msg.has_header());
1902  ASSERT_TRUE(_msg.has_header());
1903 
1904  compareTestMsg(_msg.header());
1905  EXPECT_EQ(expected_msg.voltage(), _msg.voltage());
1906  EXPECT_EQ(expected_msg.current(), _msg.current());
1907  EXPECT_EQ(expected_msg.charge(), _msg.charge());
1908  EXPECT_EQ(expected_msg.capacity(), _msg.capacity());
1909  EXPECT_EQ(expected_msg.percentage(), _msg.percentage());
1910  EXPECT_EQ(expected_msg.power_supply_status(), _msg.power_supply_status());
1911  }
1912 
1915  void createTestMsg(ignition::msgs::Marker &_msg)
1916  {
1917  ignition::msgs::Header header_msg;
1918  createTestMsg(header_msg);
1919 
1920  _msg.mutable_header()->CopyFrom(header_msg);
1921 
1922  _msg.set_action(ignition::msgs::Marker::ADD_MODIFY);
1923  _msg.set_ns("foo");
1924  _msg.set_id(123);
1925  _msg.set_type(ignition::msgs::Marker::BOX);
1926  _msg.mutable_lifetime()->set_sec(100);
1927  _msg.mutable_lifetime()->set_nsec(200);
1928 
1929  createTestMsg(*_msg.mutable_pose());
1930  createTestMsg(*_msg.mutable_scale());
1931 
1932  createTestMsg(*_msg.mutable_material()->mutable_ambient());
1933  createTestMsg(*_msg.mutable_material()->mutable_diffuse());
1934  createTestMsg(*_msg.mutable_material()->mutable_specular());
1935 
1936  createTestMsg(*_msg.add_point());
1937 
1938  _msg.set_text("bar");
1939 
1940  }
1941 
1944  void compareTestMsg(const ignition::msgs::Marker &_msg)
1945  {
1946  ignition::msgs::Marker expected_msg;
1947  createTestMsg(expected_msg);
1948 
1949  ASSERT_TRUE(expected_msg.has_header());
1950  ASSERT_TRUE(_msg.has_header());
1951 
1952  compareTestMsg(_msg.header());
1953 
1954  EXPECT_EQ(expected_msg.action(), _msg.action());
1955  EXPECT_EQ(expected_msg.ns(), _msg.ns());
1956  EXPECT_EQ(expected_msg.id(), _msg.id());
1957  EXPECT_EQ(expected_msg.type(), _msg.type());
1958  EXPECT_EQ(expected_msg.lifetime().sec(), _msg.lifetime().sec());
1959  EXPECT_EQ(expected_msg.lifetime().nsec(), _msg.lifetime().nsec());
1960 
1961  compareTestMsg(_msg.pose());
1962  compareTestMsg(_msg.scale());
1963  compareTestMsg(_msg.material().ambient());
1964  compareTestMsg(_msg.material().diffuse());
1965  compareTestMsg(_msg.material().specular());
1966 
1967  compareTestMsg(_msg.point(0));
1968 
1969  EXPECT_EQ(expected_msg.text(), _msg.text());
1970  }
1971 
1974  void createTestMsg(ignition::msgs::Marker_V &_msg)
1975  {
1976  // Not setting header because the ROS MarkerArray doesn't use it.
1977  createTestMsg(*_msg.add_marker());
1978  }
1979 
1982  void compareTestMsg(const ignition::msgs::Marker_V &_msg)
1983  {
1984  ignition::msgs::Marker_V expected_msg;
1985  createTestMsg(expected_msg);
1986 
1987  compareTestMsg(_msg.marker(0));
1988  }
1989 }
1990 }
1991 
1992 #endif // ROS_IGN_BRIDGE__TEST_UTILS_H_
ros.h
ros::spinOnce
ROSCPP_DECL void spinOnce()
ros_ign_bridge::testing::createTestMsg
void createTestMsg(std_msgs::Bool &_msg)
ROS test utils.
Definition: test_utils.h:120
ros_ign_bridge::testing::compareTestMsg
void compareTestMsg(const std_msgs::Bool &_msg)
Compare a message with the populated for testing.
Definition: test_utils.h:127
ros_ign_bridge::testing::waitUntilBoolVar
void waitUntilBoolVar(bool &_boolVar, const std::chrono::duration< Rep, Period > &_sleepEach, const int _retries)
Wait until a boolean variable is set to true for a given number of times.
Definition: test_utils.h:77
ros_ign_bridge::testing::waitUntilBoolVarAndSpin
void waitUntilBoolVarAndSpin(bool &_boolVar, const std::chrono::duration< Rep, Period > &_sleepEach, const int _retries)
Wait until a boolean variable is set to true for a given number of times. This function calls ros::sp...
Definition: test_utils.h:100
ros_ign_bridge
Definition: convert.hpp:59

ros_ign_bridge
Author(s):
autogenerated on Sat Mar 11 2023 04:02:16