system.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * C++ unit test for oxford_gps_eth
3  * Verify correct handling of UDP packets
4  *********************************************************************/
5 
6 #include <gtest/gtest.h>
7 
8 #include <ros/ros.h>
9 #include <sensor_msgs/NavSatFix.h>
10 #include <sensor_msgs/Imu.h>
11 #include <geometry_msgs/TwistWithCovarianceStamped.h>
12 
13 // Tf Quaternions
14 #include <tf/LinearMath/Quaternion.h>
15 
16 // Ethernet
17 #include <arpa/inet.h>
18 #include <sys/socket.h>
19 
20 // Helper classes
21 #include "MsgRx.h"
22 
23 // Packet structure
24 #include "../src/dispatch.h"
25 
26 // Parameters
27 std::string g_ip_address = "";
28 int g_port = 3000;
29 std::string g_frame_id_gps = "gps";
30 std::string g_frame_id_vel = "enu";
31 std::string g_frame_id_odom = "base_footprint";
32 
33 // Subscribed topics
34 ros::Subscriber g_sub_fix;
35 ros::Subscriber g_sub_vel;
36 ros::Subscriber g_sub_imu;
37 
38 // Received messages
39 MsgRx<sensor_msgs::NavSatFix> g_msg_fix;
40 MsgRx<geometry_msgs::TwistWithCovarianceStamped> g_msg_vel;
41 MsgRx<sensor_msgs::Imu> g_msg_imu;
42 void recvClear() {
43  g_msg_fix.clear();
44  g_msg_vel.clear();
45  g_msg_imu.clear();
46 }
47 
48 // Subscriber receive callbacks
49 void recvGpsFix(const sensor_msgs::NavSatFix::ConstPtr& msg) { g_msg_fix.set(*msg); }
50 void recvGpsVel(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg) { g_msg_vel.set(*msg); }
51 void recvImuData(const sensor_msgs::Imu::ConstPtr& msg) { g_msg_imu.set(*msg); }
52 
53 // Set sync byte and apply checksums
54 static void fixPacket(Packet &packet) {
55  packet.sync = 0xE7;
56  const uint8_t *ptr = ((uint8_t*)&packet) + 1;
57  uint8_t chksum = 0;
58  for (; ptr < &packet.chksum1; ptr++) {
59  chksum += *ptr;
60  }
61  packet.chksum1 = chksum;
62  for (; ptr < &packet.chksum2; ptr++) {
63  chksum += *ptr;
64  }
65  packet.chksum2 = chksum;
66  for (; ptr < &packet.chksum3; ptr++) {
67  chksum += *ptr;
68  }
69  packet.chksum3 = chksum;
70 }
71 
72 // Open socket and send UDP packet
73 static bool sendPacket(const Packet &packet) {
74  recvClear();
75  static int fd = -1;
76  if (fd == -1) {
77  fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); // Create UDP socket
78  }
79  if (fd != -1) {
80  struct sockaddr_in si_other;
81  memset(&si_other, 0x00, sizeof(si_other));
82  si_other.sin_family = AF_INET;
83  si_other.sin_port = htons(g_port);
84  if (inet_aton(g_ip_address.c_str(), &si_other.sin_addr)) {
85  if (sendto(fd, &packet, sizeof(packet), 0, (struct sockaddr *) &si_other, sizeof(si_other)) == sizeof(packet)) {
86  return true;
87  }
88  }
89  }
90  return false;
91 }
92 
93 // Helper wait functions
94 static inline double SQUARE(double x) { return x * x; }
95 static bool waitForTopics(ros::WallDuration dur) {
96  const ros::WallTime start = ros::WallTime::now();
97  while (true) {
98  if ((g_sub_fix.getNumPublishers() == 1) &&
99  (g_sub_vel.getNumPublishers() == 1) &&
100  (g_sub_imu.getNumPublishers() == 1)) {
101  return true;
102  }
103  if ((ros::WallTime::now() - start) > dur) {
104  return false;
105  }
106  ros::WallDuration(0.001).sleep();
107  ros::spinOnce();
108  }
109 }
110 static bool waitForMsgs(ros::WallDuration dur) {
111  const ros::WallTime start = ros::WallTime::now();
112  while (true) {
113  if (g_msg_fix.valid() && g_msg_vel.valid() && g_msg_imu.valid()) {
114  return true;
115  }
116  if ((ros::WallTime::now() - start) > dur) {
117  return false;
118  }
119  ros::WallDuration(0.001).sleep();
120  ros::spinOnce();
121  }
122 }
123 
124 // Wait for node to startup and verify connections
125 TEST(Main, topics)
126 {
127  EXPECT_TRUE(waitForTopics(ros::WallDuration(2.0)));
128  ASSERT_EQ(1, g_sub_fix.getNumPublishers());
129  ASSERT_EQ(1, g_sub_vel.getNumPublishers());
130  ASSERT_EQ(1, g_sub_imu.getNumPublishers());
131  EXPECT_FALSE(g_msg_fix.valid());
132  EXPECT_FALSE(g_msg_vel.valid());
133  EXPECT_FALSE(g_msg_imu.valid());
134 }
135 
136 // Verify correct handling of packets
137 TEST(Main, packets)
138 {
139  // Variables
140  Packet packet;
141  const ros::WallDuration DURATION(0.2);
142  memset(&packet, 0x00, sizeof(packet));
143 
144  // Empty packet should fail the sync byte and checksum tests
145  ASSERT_TRUE(sendPacket(packet));
146  EXPECT_FALSE(waitForMsgs(DURATION));
147  EXPECT_FALSE(g_msg_fix.valid());
148  EXPECT_FALSE(g_msg_vel.valid());
149  EXPECT_FALSE(g_msg_imu.valid());
150 
151  // Empty packet should fail because nav_status is missing
152  fixPacket(packet);
153  ASSERT_TRUE(sendPacket(packet));
154  EXPECT_FALSE(waitForMsgs(DURATION));
155  EXPECT_FALSE(g_msg_fix.valid());
156  EXPECT_FALSE(g_msg_vel.valid());
157  EXPECT_FALSE(g_msg_imu.valid());
158 
159  // Just enough data for a successful packet
160  packet.nav_status = 4;
161  fixPacket(packet);
162  ASSERT_TRUE(sendPacket(packet));
163  EXPECT_TRUE(waitForMsgs(DURATION));
164  ASSERT_TRUE(g_msg_fix.valid());
165  ASSERT_TRUE(g_msg_vel.valid());
166  ASSERT_TRUE(g_msg_imu.valid());
167 }
168 
169 // Verify message frame_ids and timestamps
170 TEST(Main, header)
171 {
172  // Variables
173  Packet packet;
174  const ros::WallDuration DURATION(0.2);
175  memset(&packet, 0x00, sizeof(packet));
176 
177  // Verify that all messages have the same non-zero timestamp and that the frame_ids are correct
178  ros::Time stamp = ros::Time::now();
179  packet.nav_status = 4;
180  fixPacket(packet);
181  ASSERT_TRUE(sendPacket(packet));
182  EXPECT_TRUE(waitForMsgs(DURATION));
183  ASSERT_TRUE(g_msg_fix.valid());
184  ASSERT_TRUE(g_msg_vel.valid());
185  ASSERT_TRUE(g_msg_imu.valid());
186  EXPECT_STREQ(g_msg_fix.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
187  EXPECT_STREQ(g_msg_vel.get().header.frame_id.c_str(), g_frame_id_vel.c_str());
188  EXPECT_STREQ(g_msg_imu.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
189  EXPECT_EQ(g_msg_fix.get().header.stamp, g_msg_vel.get().header.stamp);
190  EXPECT_EQ(g_msg_fix.get().header.stamp, g_msg_imu.get().header.stamp);
191  EXPECT_NE(g_msg_fix.get().header.stamp, ros::Time(0));
192  EXPECT_NEAR((g_msg_fix.get().header.stamp - stamp).toSec(), 0, 0.01);
193 }
194 
195 // Verify correct fields in the sensor_msgs::NavSatFix message
196 TEST(Main, fix)
197 {
198  // Variables
199  Packet packet;
200  const ros::WallDuration DURATION(0.2);
201  memset(&packet, 0x00, sizeof(packet));
202 
203  // Send valid packet
204  packet.nav_status = 4;
205  packet.latitude = 1.23456789012345;
206  packet.longitude = 9.87654321098765;
207  packet.altitude = 1.234567;
208  fixPacket(packet);
209  ASSERT_TRUE(sendPacket(packet));
210  EXPECT_TRUE(waitForMsgs(DURATION));
211  ASSERT_TRUE(g_msg_fix.valid());
212  ASSERT_TRUE(g_msg_vel.valid());
213  ASSERT_TRUE(g_msg_imu.valid());
214  EXPECT_STREQ(g_msg_fix.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
215  EXPECT_EQ(g_msg_fix.get().status.status, sensor_msgs::NavSatStatus::STATUS_NO_FIX);
216  EXPECT_EQ(g_msg_fix.get().status.service, sensor_msgs::NavSatStatus::SERVICE_GPS);
217  EXPECT_EQ(g_msg_fix.get().latitude, packet.latitude * (180 / M_PI));
218  EXPECT_EQ(g_msg_fix.get().longitude, packet.longitude * (180 / M_PI));
219  EXPECT_EQ(g_msg_fix.get().altitude, packet.altitude);
220  EXPECT_EQ(g_msg_fix.get().position_covariance_type, sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN);
221  EXPECT_EQ(g_msg_fix.get().position_covariance[0], 0);
222  EXPECT_EQ(g_msg_fix.get().position_covariance[1], 0);
223  EXPECT_EQ(g_msg_fix.get().position_covariance[2], 0);
224  EXPECT_EQ(g_msg_fix.get().position_covariance[3], 0);
225  EXPECT_EQ(g_msg_fix.get().position_covariance[4], 0);
226  EXPECT_EQ(g_msg_fix.get().position_covariance[5], 0);
227  EXPECT_EQ(g_msg_fix.get().position_covariance[6], 0);
228  EXPECT_EQ(g_msg_fix.get().position_covariance[7], 0);
229  EXPECT_EQ(g_msg_fix.get().position_covariance[8], 0);
230 
231  // Set position mode on channel 0
232  memset(&packet.chan, 0x00, sizeof(packet.chan));
233  packet.channel = 0;
234  packet.chan.chan0.position_mode = MODE_RTK_INTEGER;
235  fixPacket(packet);
236  ASSERT_TRUE(sendPacket(packet));
237  EXPECT_TRUE(waitForMsgs(DURATION));
238  ASSERT_TRUE(g_msg_fix.valid());
239  ASSERT_TRUE(g_msg_vel.valid());
240  ASSERT_TRUE(g_msg_imu.valid());
241  EXPECT_STREQ(g_msg_fix.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
242  EXPECT_EQ(g_msg_fix.get().status.status, sensor_msgs::NavSatStatus::STATUS_GBAS_FIX);
243  EXPECT_EQ(g_msg_fix.get().status.service, sensor_msgs::NavSatStatus::SERVICE_GPS);
244  EXPECT_EQ(g_msg_fix.get().latitude, packet.latitude * (180 / M_PI));
245  EXPECT_EQ(g_msg_fix.get().longitude, packet.longitude * (180 / M_PI));
246  EXPECT_EQ(g_msg_fix.get().altitude, packet.altitude);
247  EXPECT_EQ(g_msg_fix.get().position_covariance_type, sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN);
248  EXPECT_EQ(g_msg_fix.get().position_covariance[0], 0);
249  EXPECT_EQ(g_msg_fix.get().position_covariance[1], 0);
250  EXPECT_EQ(g_msg_fix.get().position_covariance[2], 0);
251  EXPECT_EQ(g_msg_fix.get().position_covariance[3], 0);
252  EXPECT_EQ(g_msg_fix.get().position_covariance[4], 0);
253  EXPECT_EQ(g_msg_fix.get().position_covariance[5], 0);
254  EXPECT_EQ(g_msg_fix.get().position_covariance[6], 0);
255  EXPECT_EQ(g_msg_fix.get().position_covariance[7], 0);
256  EXPECT_EQ(g_msg_fix.get().position_covariance[8], 0);
257 
258  // Set position covariance on channel 3
259  memset(&packet.chan, 0x00, sizeof(packet.chan));
260  packet.channel = 3;
261  packet.chan.chan3.age = 10;
262  packet.chan.chan3.acc_position_east = 123;
263  packet.chan.chan3.acc_position_north = 1234;
264  packet.chan.chan3.acc_position_down = 12345;
265  fixPacket(packet);
266  ASSERT_TRUE(sendPacket(packet));
267  EXPECT_TRUE(waitForMsgs(DURATION));
268  ASSERT_TRUE(g_msg_fix.valid());
269  ASSERT_TRUE(g_msg_vel.valid());
270  ASSERT_TRUE(g_msg_imu.valid());
271  EXPECT_STREQ(g_msg_fix.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
272  EXPECT_EQ(g_msg_fix.get().status.status, sensor_msgs::NavSatStatus::STATUS_GBAS_FIX);
273  EXPECT_EQ(g_msg_fix.get().status.service, sensor_msgs::NavSatStatus::SERVICE_GPS);
274  EXPECT_EQ(g_msg_fix.get().latitude, packet.latitude * (180 / M_PI));
275  EXPECT_EQ(g_msg_fix.get().longitude, packet.longitude * (180 / M_PI));
276  EXPECT_EQ(g_msg_fix.get().altitude, packet.altitude);
277  EXPECT_EQ(g_msg_fix.get().position_covariance_type, sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN);
278  EXPECT_EQ(g_msg_fix.get().position_covariance[0], SQUARE(packet.chan.chan3.acc_position_east * 1e-3)); // x
279  EXPECT_EQ(g_msg_fix.get().position_covariance[1], 0);
280  EXPECT_EQ(g_msg_fix.get().position_covariance[2], 0);
281  EXPECT_EQ(g_msg_fix.get().position_covariance[3], 0);
282  EXPECT_EQ(g_msg_fix.get().position_covariance[4], SQUARE(packet.chan.chan3.acc_position_north * 1e-3)); // y
283  EXPECT_EQ(g_msg_fix.get().position_covariance[5], 0);
284  EXPECT_EQ(g_msg_fix.get().position_covariance[6], 0);
285  EXPECT_EQ(g_msg_fix.get().position_covariance[7], 0);
286  EXPECT_EQ(g_msg_fix.get().position_covariance[8], SQUARE(packet.chan.chan3.acc_position_down * 1e-3)); // z
287 
288  // Set position covariance on channel 3 with old age
289  memset(&packet.chan, 0x00, sizeof(packet.chan));
290  packet.channel = 3;
291  packet.chan.chan3.age = 190;
292  packet.chan.chan3.acc_position_east = 123;
293  packet.chan.chan3.acc_position_north = 1234;
294  packet.chan.chan3.acc_position_down = 12345;
295  fixPacket(packet);
296  ASSERT_TRUE(sendPacket(packet));
297  EXPECT_TRUE(waitForMsgs(DURATION));
298  ASSERT_TRUE(g_msg_fix.valid());
299  ASSERT_TRUE(g_msg_vel.valid());
300  ASSERT_TRUE(g_msg_imu.valid());
301  EXPECT_STREQ(g_msg_fix.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
302  EXPECT_EQ(g_msg_fix.get().status.status, sensor_msgs::NavSatStatus::STATUS_GBAS_FIX);
303  EXPECT_EQ(g_msg_fix.get().status.service, sensor_msgs::NavSatStatus::SERVICE_GPS);
304  EXPECT_EQ(g_msg_fix.get().latitude, packet.latitude * (180 / M_PI));
305  EXPECT_EQ(g_msg_fix.get().longitude, packet.longitude * (180 / M_PI));
306  EXPECT_EQ(g_msg_fix.get().altitude, packet.altitude);
307  EXPECT_EQ(g_msg_fix.get().position_covariance_type, sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN);
308  EXPECT_EQ(g_msg_fix.get().position_covariance[0], 0);
309  EXPECT_EQ(g_msg_fix.get().position_covariance[1], 0);
310  EXPECT_EQ(g_msg_fix.get().position_covariance[2], 0);
311  EXPECT_EQ(g_msg_fix.get().position_covariance[3], 0);
312  EXPECT_EQ(g_msg_fix.get().position_covariance[4], 0);
313  EXPECT_EQ(g_msg_fix.get().position_covariance[5], 0);
314  EXPECT_EQ(g_msg_fix.get().position_covariance[6], 0);
315  EXPECT_EQ(g_msg_fix.get().position_covariance[7], 0);
316  EXPECT_EQ(g_msg_fix.get().position_covariance[8], 0);
317 }
318 
319 // Verify correct fields in the geometry_msgs::TwistWithCovarianceStamped message
320 TEST(Main, vel)
321 {
322  // Variables
323  Packet packet;
324  const ros::WallDuration DURATION(0.2);
325  memset(&packet, 0x00, sizeof(packet));
326 
327  // Send valid packet
328  packet.nav_status = 4;
329  packet.vel_east = 1234567;
330  packet.vel_north = -7654321;
331  packet.vel_down = 3456789;
332  fixPacket(packet);
333  ASSERT_TRUE(sendPacket(packet));
334  EXPECT_TRUE(waitForMsgs(DURATION));
335  ASSERT_TRUE(g_msg_fix.valid());
336  ASSERT_TRUE(g_msg_vel.valid());
337  ASSERT_TRUE(g_msg_imu.valid());
338  EXPECT_STREQ(g_msg_vel.get().header.frame_id.c_str(), g_frame_id_vel.c_str());
339  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.x, (double)packet.vel_east * 1e-4);
340  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.y, (double)packet.vel_north * 1e-4);
341  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.z, (double)packet.vel_down * -1e-4);
342  for (size_t i = 0; i < g_msg_vel.get().twist.covariance.size(); i++) {
343  EXPECT_EQ(g_msg_vel.get().twist.covariance[i], 0);
344  }
345 
346  // Set velocity covariance on channel 4
347  memset(&packet.chan, 0x00, sizeof(packet.chan));
348  packet.channel = 4;
349  packet.chan.chan4.age = 10;
350  packet.chan.chan4.acc_velocity_east = 123;
351  packet.chan.chan4.acc_velocity_north = 1234;
352  packet.chan.chan4.acc_velocity_down = 12345;
353  fixPacket(packet);
354  ASSERT_TRUE(sendPacket(packet));
355  EXPECT_TRUE(waitForMsgs(DURATION));
356  ASSERT_TRUE(g_msg_fix.valid());
357  ASSERT_TRUE(g_msg_vel.valid());
358  ASSERT_TRUE(g_msg_imu.valid());
359  EXPECT_STREQ(g_msg_vel.get().header.frame_id.c_str(), g_frame_id_vel.c_str());
360  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.x, (double)packet.vel_east * 1e-4);
361  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.y, (double)packet.vel_north * 1e-4);
362  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.z, (double)packet.vel_down * -1e-4);
363  EXPECT_EQ(g_msg_vel.get().twist.covariance[0], SQUARE(packet.chan.chan4.acc_velocity_east * 1e-3)); // x
364  EXPECT_EQ(g_msg_vel.get().twist.covariance[7], SQUARE(packet.chan.chan4.acc_velocity_north * 1e-3)); // y
365  EXPECT_EQ(g_msg_vel.get().twist.covariance[14], SQUARE(packet.chan.chan4.acc_velocity_down * 1e-3)); // z
366 
367  // Set velocity covariance on channel 4 with old age
368  memset(&packet.chan, 0x00, sizeof(packet.chan));
369  packet.channel = 4;
370  packet.chan.chan4.age = 190;
371  packet.chan.chan4.acc_velocity_east = 123;
372  packet.chan.chan4.acc_velocity_north = 1234;
373  packet.chan.chan4.acc_velocity_down = 12345;
374  fixPacket(packet);
375  ASSERT_TRUE(sendPacket(packet));
376  EXPECT_TRUE(waitForMsgs(DURATION));
377  ASSERT_TRUE(g_msg_fix.valid());
378  ASSERT_TRUE(g_msg_vel.valid());
379  ASSERT_TRUE(g_msg_imu.valid());
380  EXPECT_STREQ(g_msg_vel.get().header.frame_id.c_str(), g_frame_id_vel.c_str());
381  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.x, (double)packet.vel_east * 1e-4);
382  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.y, (double)packet.vel_north * 1e-4);
383  EXPECT_EQ(g_msg_vel.get().twist.twist.linear.z, (double)packet.vel_down * -1e-4);
384  for (size_t i = 0; i < g_msg_vel.get().twist.covariance.size(); i++) {
385  EXPECT_EQ(g_msg_vel.get().twist.covariance[i], 0);
386  }
387 }
388 
389 // Verify correct fields in the sensor_msgs::Imu message
390 TEST(Main, imu)
391 {
392  // Variables
393  Packet packet;
394  const ros::WallDuration DURATION(0.2);
395  memset(&packet, 0x00, sizeof(packet));
396  tf::Quaternion q;
397 
398  // Send valid packet
399  packet.nav_status = 4;
400  packet.accel_x = 1234567;
401  packet.accel_y = -7654321;
402  packet.accel_z = 3456789;
403  packet.gyro_x = 2345678;
404  packet.gyro_y = -6543210;
405  packet.gyro_z = 4567890;
406  packet.heading = 5678901;
407  packet.pitch = -123456;
408  packet.roll = 234567;
409  fixPacket(packet);
410  ASSERT_TRUE(sendPacket(packet));
411  EXPECT_TRUE(waitForMsgs(DURATION));
412  ASSERT_TRUE(g_msg_fix.valid());
413  ASSERT_TRUE(g_msg_vel.valid());
414  ASSERT_TRUE(g_msg_imu.valid());
415  EXPECT_STREQ(g_msg_imu.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
416  q.setRPY((double)packet.roll * 1e-6, (double)packet.pitch * 1e-6, M_PI_2 - (double)packet.heading * 1e-6);
417  EXPECT_EQ(g_msg_imu.get().orientation.w, q.w());
418  EXPECT_EQ(g_msg_imu.get().orientation.x, q.x());
419  EXPECT_EQ(g_msg_imu.get().orientation.y, q.y());
420  EXPECT_EQ(g_msg_imu.get().orientation.z, q.z());
421  EXPECT_EQ(g_msg_imu.get().linear_acceleration.x, (double)packet.accel_x * 1e-4);
422  EXPECT_EQ(g_msg_imu.get().linear_acceleration.y, (double)packet.accel_y * 1e-4);
423  EXPECT_EQ(g_msg_imu.get().linear_acceleration.z, (double)packet.accel_z * -1e-4);
424  EXPECT_EQ(g_msg_imu.get().angular_velocity.x, (double)packet.gyro_x * 1e-5);
425  EXPECT_EQ(g_msg_imu.get().angular_velocity.y, (double)packet.gyro_y * 1e-5);
426  EXPECT_EQ(g_msg_imu.get().angular_velocity.z, (double)packet.gyro_z * -1e-5);
427  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[0], 0.0); // x
428  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[4], 0.0); // y
429  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[8], 0.0); // z
430  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[0], 0.0); // x
431  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[4], 0.0); // y
432  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[8], 0.0); // z
433  EXPECT_EQ(g_msg_imu.get().orientation_covariance[0], 0.0); // x
434  EXPECT_EQ(g_msg_imu.get().orientation_covariance[4], 0.0); // y
435  EXPECT_EQ(g_msg_imu.get().orientation_covariance[8], 0.0); // z
436 
437  // Set orientation covariance on channel 5
438  memset(&packet.chan, 0x00, sizeof(packet.chan));
439  packet.channel = 5;
440  packet.chan.chan5.age = 10;
441  packet.chan.chan5.acc_heading = 123;
442  packet.chan.chan5.acc_pitch = 1234;
443  packet.chan.chan5.acc_roll = 12345;
444  fixPacket(packet);
445  ASSERT_TRUE(sendPacket(packet));
446  EXPECT_TRUE(waitForMsgs(DURATION));
447  ASSERT_TRUE(g_msg_fix.valid());
448  ASSERT_TRUE(g_msg_vel.valid());
449  ASSERT_TRUE(g_msg_imu.valid());
450  EXPECT_STREQ(g_msg_imu.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
451  q.setRPY((double)packet.roll * 1e-6, (double)packet.pitch * 1e-6, M_PI_2 - (double)packet.heading * 1e-6);
452  EXPECT_EQ(g_msg_imu.get().orientation.w, q.w());
453  EXPECT_EQ(g_msg_imu.get().orientation.x, q.x());
454  EXPECT_EQ(g_msg_imu.get().orientation.y, q.y());
455  EXPECT_EQ(g_msg_imu.get().orientation.z, q.z());
456  EXPECT_EQ(g_msg_imu.get().linear_acceleration.x, (double)packet.accel_x * 1e-4);
457  EXPECT_EQ(g_msg_imu.get().linear_acceleration.y, (double)packet.accel_y * 1e-4);
458  EXPECT_EQ(g_msg_imu.get().linear_acceleration.z, (double)packet.accel_z * -1e-4);
459  EXPECT_EQ(g_msg_imu.get().angular_velocity.x, (double)packet.gyro_x * 1e-5);
460  EXPECT_EQ(g_msg_imu.get().angular_velocity.y, (double)packet.gyro_y * 1e-5);
461  EXPECT_EQ(g_msg_imu.get().angular_velocity.z, (double)packet.gyro_z * -1e-5);
462  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[0], 0.0); // x
463  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[4], 0.0); // y
464  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[8], 0.0); // z
465  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[0], 0.0); // x
466  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[4], 0.0); // y
467  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[8], 0.0); // z
468  EXPECT_EQ(g_msg_imu.get().orientation_covariance[0], SQUARE(packet.chan.chan5.acc_roll * 1e-5)); // x
469  EXPECT_EQ(g_msg_imu.get().orientation_covariance[4], SQUARE(packet.chan.chan5.acc_pitch * 1e-5)); // y
470  EXPECT_EQ(g_msg_imu.get().orientation_covariance[8], SQUARE(packet.chan.chan5.acc_heading * 1e-5)); // z
471 
472  // Set orientation covariance on channel 5 with old age
473  memset(&packet.chan, 0x00, sizeof(packet.chan));
474  packet.channel = 5;
475  packet.chan.chan5.age = 190;
476  packet.chan.chan5.acc_heading = 123;
477  packet.chan.chan5.acc_pitch = 1234;
478  packet.chan.chan5.acc_roll = 12345;
479  fixPacket(packet);
480  ASSERT_TRUE(sendPacket(packet));
481  EXPECT_TRUE(waitForMsgs(DURATION));
482  ASSERT_TRUE(g_msg_fix.valid());
483  ASSERT_TRUE(g_msg_vel.valid());
484  ASSERT_TRUE(g_msg_imu.valid());
485  EXPECT_STREQ(g_msg_imu.get().header.frame_id.c_str(), g_frame_id_gps.c_str());
486  q.setRPY((double)packet.roll * 1e-6, (double)packet.pitch * 1e-6, M_PI_2 - (double)packet.heading * 1e-6);
487  EXPECT_EQ(g_msg_imu.get().orientation.w, q.w());
488  EXPECT_EQ(g_msg_imu.get().orientation.x, q.x());
489  EXPECT_EQ(g_msg_imu.get().orientation.y, q.y());
490  EXPECT_EQ(g_msg_imu.get().orientation.z, q.z());
491  EXPECT_EQ(g_msg_imu.get().linear_acceleration.x, (double)packet.accel_x * 1e-4);
492  EXPECT_EQ(g_msg_imu.get().linear_acceleration.y, (double)packet.accel_y * 1e-4);
493  EXPECT_EQ(g_msg_imu.get().linear_acceleration.z, (double)packet.accel_z * -1e-4);
494  EXPECT_EQ(g_msg_imu.get().angular_velocity.x, (double)packet.gyro_x * 1e-5);
495  EXPECT_EQ(g_msg_imu.get().angular_velocity.y, (double)packet.gyro_y * 1e-5);
496  EXPECT_EQ(g_msg_imu.get().angular_velocity.z, (double)packet.gyro_z * -1e-5);
497  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[0], 0.0); // x
498  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[4], 0.0); // y
499  EXPECT_EQ(g_msg_imu.get().angular_velocity_covariance[8], 0.0); // z
500  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[0], 0.0); // x
501  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[4], 0.0); // y
502  EXPECT_EQ(g_msg_imu.get().linear_acceleration_covariance[8], 0.0); // z
503  EXPECT_EQ(g_msg_imu.get().orientation_covariance[0], 0.0); // x
504  EXPECT_EQ(g_msg_imu.get().orientation_covariance[4], 0.0); // x
505  EXPECT_EQ(g_msg_imu.get().orientation_covariance[8], 0.0); // x
506 }
507 
508 // Run all the tests that were declared with TEST()
509 int main(int argc, char **argv)
510 {
511  testing::InitGoogleTest(&argc, argv);
512  ros::init(argc, argv, "test");
513 
514  // Parameters
515  ros::NodeHandle priv_nh("~");
516  priv_nh.getParam("ip_address", g_ip_address);
517  priv_nh.getParam("port", g_port);
518  priv_nh.getParam("frame_id_gps", g_frame_id_gps);
519  priv_nh.getParam("frame_id_vel", g_frame_id_vel);
520  priv_nh.getParam("frame_id_odom", g_frame_id_odom);
521  if (g_ip_address.empty()) {
522  g_ip_address = "127.0.0.1";
523  }
524 
525  // Subscribers
526  ros::NodeHandle nh;
527  g_sub_fix = nh.subscribe("gps/fix", 2, recvGpsFix);
528  g_sub_vel = nh.subscribe("gps/vel", 2, recvGpsVel);
529  g_sub_imu = nh.subscribe("imu/data", 2, recvImuData);
530 
531  return RUN_ALL_TESTS();
532 }
533 
recvClear
void recvClear()
Definition: system.cpp:42
g_frame_id_gps
std::string g_frame_id_gps
Definition: system.cpp:29
Channel::acc_roll
uint16_t acc_roll
Definition: dispatch.h:64
Packet::accel_x
int32_t accel_x
Definition: dispatch.h:92
Channel::acc_position_down
uint16_t acc_position_down
Definition: dispatch.h:52
ros::NodeHandle
MsgRx::valid
bool valid() const
Definition: MsgRx.h:14
Channel::acc_position_north
uint16_t acc_position_north
Definition: dispatch.h:50
MsgRx::set
void set(const MsgT &msg)
Definition: MsgRx.h:12
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
Packet::vel_down
int32_t vel_down
Definition: dispatch.h:105
waitForMsgs
static bool waitForMsgs(ros::WallDuration dur)
Definition: system.cpp:110
Channel::age
uint8_t age
Definition: dispatch.h:53
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
g_sub_imu
ros::Subscriber g_sub_imu
Definition: system.cpp:36
Channel::chan5
struct Channel::@4 chan5
g_frame_id_odom
std::string g_frame_id_odom
Definition: system.cpp:31
g_sub_fix
ros::Subscriber g_sub_fix
Definition: system.cpp:34
Packet::channel
uint8_t channel
Definition: dispatch.h:110
Channel::acc_velocity_east
uint16_t acc_velocity_east
Definition: dispatch.h:57
Packet::gyro_y
int32_t gyro_y
Definition: dispatch.h:96
Packet::chksum2
uint8_t chksum2
Definition: dispatch.h:109
Packet::vel_east
int32_t vel_east
Definition: dispatch.h:104
Packet::latitude
double latitude
Definition: dispatch.h:100
Channel::chan4
struct Channel::@3 chan4
Packet::nav_status
uint8_t nav_status
Definition: dispatch.h:98
Channel::acc_position_east
uint16_t acc_position_east
Definition: dispatch.h:51
g_ip_address
std::string g_ip_address
Definition: system.cpp:27
ros::WallDuration::sleep
bool sleep() const
waitForTopics
static bool waitForTopics(ros::WallDuration dur)
Definition: system.cpp:95
tf::Quaternion::setRPY
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Packet::longitude
double longitude
Definition: dispatch.h:101
SQUARE
static double SQUARE(double x)
Definition: system.cpp:94
g_msg_vel
MsgRx< geometry_msgs::TwistWithCovarianceStamped > g_msg_vel
Definition: system.cpp:40
ros::Subscriber::getNumPublishers
uint32_t getNumPublishers() const
MsgRx
Definition: MsgRx.h:7
main
int main(int argc, char **argv)
Definition: system.cpp:509
recvGpsVel
void recvGpsVel(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg)
Definition: system.cpp:50
Channel::acc_heading
uint16_t acc_heading
Definition: dispatch.h:62
g_msg_imu
MsgRx< sensor_msgs::Imu > g_msg_imu
Definition: system.cpp:41
g_frame_id_vel
std::string g_frame_id_vel
Definition: system.cpp:30
Channel::acc_velocity_down
uint16_t acc_velocity_down
Definition: dispatch.h:58
recvGpsFix
void recvGpsFix(const sensor_msgs::NavSatFix::ConstPtr &msg)
Definition: system.cpp:49
g_port
int g_port
Definition: system.cpp:28
Channel::acc_velocity_north
uint16_t acc_velocity_north
Definition: dispatch.h:56
TEST
TEST(Main, topics)
Definition: system.cpp:125
Packet::heading
int32_t heading
Definition: dispatch.h:106
Channel::chan0
struct Channel::@1 chan0
g_msg_fix
MsgRx< sensor_msgs::NavSatFix > g_msg_fix
Definition: system.cpp:39
Channel::position_mode
uint8_t position_mode
Definition: dispatch.h:45
ros::WallTime::now
static WallTime now()
Channel::acc_pitch
uint16_t acc_pitch
Definition: dispatch.h:63
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
Packet::accel_y
int32_t accel_y
Definition: dispatch.h:93
Packet::accel_z
int32_t accel_z
Definition: dispatch.h:94
Packet::sync
uint8_t sync
Definition: dispatch.h:90
Packet::gyro_z
int32_t gyro_z
Definition: dispatch.h:97
Channel::chan3
struct Channel::@2 chan3
ros::Time::now
static Time now()
MsgRx::clear
void clear()
Definition: MsgRx.h:13
Packet::pitch
int32_t pitch
Definition: dispatch.h:107
MsgRx::get
const MsgT & get() const
Definition: MsgRx.h:11
Packet::chksum3
uint8_t chksum3
Definition: dispatch.h:112
Packet::roll
int32_t roll
Definition: dispatch.h:108
fixPacket
static void fixPacket(Packet &packet)
Definition: system.cpp:54
Packet::chksum1
uint8_t chksum1
Definition: dispatch.h:99
g_sub_vel
ros::Subscriber g_sub_vel
Definition: system.cpp:35
Packet::vel_north
int32_t vel_north
Definition: dispatch.h:103
ros::spinOnce
ROSCPP_DECL void spinOnce()
Packet::gyro_x
int32_t gyro_x
Definition: dispatch.h:95
recvImuData
void recvImuData(const sensor_msgs::Imu::ConstPtr &msg)
Definition: system.cpp:51
Packet::altitude
float altitude
Definition: dispatch.h:102
Packet::chan
Channel chan
Definition: dispatch.h:111
sendPacket
static bool sendPacket(const Packet &packet)
Definition: system.cpp:73

oxford_gps_eth
Author(s): Kevin Hallenbeck
autogenerated on Sun Aug 9 2020 04:05:34