test_ulc_node.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <ros/ros.h>
3 
4 #include <std_msgs/Bool.h>
5 #include <geometry_msgs/TwistStamped.h>
6 #include <dataspeed_ulc_msgs/UlcCmd.h>
7 #include <dataspeed_ulc_msgs/UlcReport.h>
8 #include <dataspeed_ulc_can/dispatch.h>
9 #include <can_msgs/Frame.h>
10 #include "MsgRx.h"
11 
12 using namespace dataspeed_ulc_can;
13 
14 ros::NodeHandle *n, *pn;
15 
16 MsgRx<MsgUlcCmd> g_msg_ulc_cmd(ros::WallDuration(0.05));
17 MsgRx<MsgUlcCfg> g_msg_ulc_cfg(ros::WallDuration(0.05));
18 MsgRx<dataspeed_ulc_msgs::UlcReport> g_msg_ulc_report(ros::WallDuration(0.05));
19 
20 dataspeed_ulc_msgs::UlcCmd g_ulc_cmd;
21 double g_cfg_freq;
22 
23 // Command message scale factors
24 const double LIN_VEL_SCALE_FACTOR = 0.0025;
25 const double ACCEL_CMD_SCALE_FACTOR = 0.0005;
26 const double YAW_RATE_SCALE_FACTOR = 0.00025;
27 const double CURVATURE_SCALE_FACTOR = 0.0000061;
28 
29 // Config message scale factors
30 const double LINEAR_ACCEL_SCALE_FACTOR = 0.025;
31 const double LINEAR_DECEL_SCALE_FACTOR = 0.025;
32 const double LATERAL_ACCEL_SCALE_FACTOR = 0.05;
33 const double ANGULAR_ACCEL_SCALE_FACTOR = 0.02;
34 const double JERK_LIMIT_THROTTLE_SCALE_FACTOR = 0.1;
35 const double JERK_LIMIT_BRAKE_SCALE_FACTOR = 0.1;
36 
37 // Report message scale factors
38 const double SPEED_REPORT_SCALE_FACTOR = 0.02;
39 const double ACCEL_REPORT_SCALE_FACTOR = 0.05;
40 const double MAX_ANGLE_SCALE_FACTOR = 5.0;
41 const double MAX_RATE_SCALE_FACTOR = 8.0;
42 
43 ros::Publisher g_pub_ulc_cmd;
44 ros::Publisher g_pub_enable;
45 ros::Publisher g_pub_twist;
46 ros::Publisher g_pub_twist_stamped;
47 ros::Publisher g_pub_can;
48 ros::Subscriber g_sub_can;
49 ros::Subscriber g_sub_report;
50 
51 void recvCan(const can_msgs::FrameConstPtr& msg)
52 {
53  if (!msg->is_rtr && !msg->is_error && !msg->is_extended) {
54  switch (msg->id) {
55  case ID_ULC_CMD:
56  g_msg_ulc_cmd.set(*((MsgUlcCmd*)msg->data.elems));
57  break;
58  case ID_ULC_CONFIG:
59  g_msg_ulc_cfg.set(*((MsgUlcCfg*)msg->data.elems));
60  break;
61  }
62  }
63 }
64 
65 void recvReport(const dataspeed_ulc_msgs::UlcReportConstPtr& msg)
66 {
67  g_msg_ulc_report.set(*msg);
68 }
69 
70 template <class T>
71 static bool waitForMsg(ros::WallDuration dur, const MsgRx<T>& msg_rx)
72 {
73  const ros::WallTime start = ros::WallTime::now();
74  while (true) {
75  if (msg_rx.fresh()) {
76  return true;
77  }
78  if ((ros::WallTime::now() - start) > dur) {
79  return false;
80  }
81  ros::WallDuration(0.001).sleep();
82  }
83 }
84 
85 static bool waitForTopics(ros::WallDuration dur) {
86  const ros::WallTime start = ros::WallTime::now();
87  while (true) {
88  if ((g_sub_can.getNumPublishers() == 1)
89  && (g_sub_report.getNumPublishers() == 1)
90  && (g_pub_ulc_cmd.getNumSubscribers() == 1)
91  && (g_pub_enable.getNumSubscribers() == 1)
92  && (g_pub_twist.getNumSubscribers() == 1)
93  && (g_pub_twist_stamped.getNumSubscribers() == 1)
94  && (g_pub_can.getNumSubscribers() == 1)) {
95  return true;
96  }
97  if ((ros::WallTime::now() - start) > dur) {
98  return false;
99  }
100  ros::WallDuration(0.001).sleep();
101  }
102 }
103 
104 static void checkImmediateCfg()
105 {
106  ros::WallTime stamp = ros::WallTime::now();
107  g_msg_ulc_cfg.clear();
108  g_pub_ulc_cmd.publish(g_ulc_cmd);
109  EXPECT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cfg));
110  EXPECT_NEAR(g_msg_ulc_cfg.stamp().toSec(), stamp.toSec(), 0.01);
111 }
112 
113 TEST(ULCNode, topics)
114 {
115  // Wait for all topics to connect before running other tests
116  ASSERT_TRUE(waitForTopics(ros::WallDuration(2.0)));
117  ros::WallDuration(1.0).sleep();
118 
119  // Call unused function to complete coverage testing
120  dispatchAssertSizes();
121 }
122 
123 TEST(ULCNode, cfgTiming)
124 {
125  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
126  g_ulc_cmd.linear_accel = 1.0;
127  g_ulc_cmd.linear_decel = 1.0;
128  g_ulc_cmd.jerk_limit_throttle = 10.0;
129  g_ulc_cmd.jerk_limit_brake = 10.0;
130  g_ulc_cmd.lateral_accel = 1.0;
131  g_ulc_cmd.angular_accel = 1.0;
132 
133  // Publish command messages with the same acceleration limits and make sure
134  // config CAN messages are sent at the nominal rate
135  size_t count = 0;
136  ros::WallTime stamp_old;
137  ros::WallTime t_end = ros::WallTime::now() + ros::WallDuration(1.0);
138  g_msg_ulc_cfg.clear();
139  while (t_end > ros::WallTime::now()) {
140  g_pub_ulc_cmd.publish(g_ulc_cmd);
141  ros::WallDuration(0.02).sleep();
142  ros::WallTime stamp_new = g_msg_ulc_cfg.stamp();
143  if (!stamp_new.isZero()) {
144  if (!stamp_old.isZero() && (stamp_old != stamp_new)) {
145  EXPECT_NEAR(stamp_old.toSec() + (1.0 / g_cfg_freq), stamp_new.toSec(), 0.01);
146  count++;
147  }
148  stamp_old = stamp_new;
149  }
150  }
151  EXPECT_GE(count, 1);
152 
153  // Change accel/jerk limits and make sure config CAN messages are sent immediately
154  g_ulc_cmd.linear_accel = 2.0;
155  checkImmediateCfg();
156  g_ulc_cmd.linear_decel = 2.0;
157  checkImmediateCfg();
158  g_ulc_cmd.jerk_limit_throttle = 5.0;
159  checkImmediateCfg();
160  g_ulc_cmd.jerk_limit_brake = 5.0;
161  checkImmediateCfg();
162  g_ulc_cmd.lateral_accel = 2.0;
163  checkImmediateCfg();
164  g_ulc_cmd.angular_accel = 2.0;
165  checkImmediateCfg();
166 }
167 
168 TEST(ULCNode, cmdRangeSaturation)
169 {
170  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
171 
172  /*** Underflow tests ******************************************************/
173  g_ulc_cmd.enable_pedals = true;
174  g_ulc_cmd.enable_steering = true;
175  g_ulc_cmd.linear_velocity = (INT16_MIN * LIN_VEL_SCALE_FACTOR) - 1.0;
176  g_ulc_cmd.accel_cmd = 0.0;
177  g_ulc_cmd.pedals_mode = dataspeed_ulc_msgs::UlcCmd::SPEED_MODE;
178  g_ulc_cmd.coast_decel = 0;
179  g_ulc_cmd.linear_accel = -1.0;
180  g_ulc_cmd.linear_decel = -1.0;
181  g_ulc_cmd.jerk_limit_throttle = -1.0;
182  g_ulc_cmd.jerk_limit_brake = -1.0;
183  g_ulc_cmd.lateral_accel = -1.0;
184  g_ulc_cmd.angular_accel = -1.0;
185 
186  // Yaw rate steering
187  g_ulc_cmd.yaw_command = (INT16_MIN * YAW_RATE_SCALE_FACTOR) - 0.5;
188  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::YAW_RATE_MODE;
189  g_msg_ulc_cmd.clear();
190  g_msg_ulc_cfg.clear();
191  g_pub_ulc_cmd.publish(g_ulc_cmd);
192  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
193  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cfg));
194  EXPECT_EQ(INT16_MIN, g_msg_ulc_cmd.get().lon_command);
195  EXPECT_EQ(INT16_MIN, g_msg_ulc_cmd.get().yaw_command);
196  EXPECT_EQ(0, g_msg_ulc_cfg.get().linear_accel);
197  EXPECT_EQ(0, g_msg_ulc_cfg.get().linear_decel);
198  EXPECT_EQ(0, g_msg_ulc_cfg.get().jerk_limit_throttle);
199  EXPECT_EQ(0, g_msg_ulc_cfg.get().jerk_limit_brake);
200  EXPECT_EQ(0, g_msg_ulc_cfg.get().lateral_accel);
201  EXPECT_EQ(0, g_msg_ulc_cfg.get().angular_accel);
202 
203  // Curvature steering
204  g_ulc_cmd.yaw_command = (INT16_MIN * CURVATURE_SCALE_FACTOR) - 0.05;
205  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::CURVATURE_MODE;
206  g_msg_ulc_cmd.clear();
207  g_msg_ulc_cfg.clear();
208  g_pub_ulc_cmd.publish(g_ulc_cmd);
209  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
210  EXPECT_EQ(INT16_MIN, g_msg_ulc_cmd.get().yaw_command);
211  /**************************************************************************/
212 
213  /*** Overflow tests *******************************************************/
214  g_ulc_cmd.enable_pedals = true;
215  g_ulc_cmd.enable_steering = true;
216  g_ulc_cmd.linear_velocity = (INT16_MAX * LIN_VEL_SCALE_FACTOR) + 1.0;
217  g_ulc_cmd.pedals_mode = dataspeed_ulc_msgs::UlcCmd::SPEED_MODE;
218  g_ulc_cmd.coast_decel = 0;
219  g_ulc_cmd.linear_accel = 100.0;
220  g_ulc_cmd.linear_decel = 100.0;
221  g_ulc_cmd.jerk_limit_throttle = 100.0;
222  g_ulc_cmd.jerk_limit_brake = 100.0;
223  g_ulc_cmd.lateral_accel = 100.0;
224  g_ulc_cmd.angular_accel = 100.0;
225 
226  // Yaw rate steering
227  g_ulc_cmd.yaw_command = (INT16_MAX * YAW_RATE_SCALE_FACTOR) + 0.5;
228  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::YAW_RATE_MODE;
229  g_msg_ulc_cmd.clear();
230  g_msg_ulc_cfg.clear();
231  g_pub_ulc_cmd.publish(g_ulc_cmd);
232  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
233  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cfg));
234  EXPECT_EQ(INT16_MAX, g_msg_ulc_cmd.get().lon_command);
235  EXPECT_EQ(INT16_MAX, g_msg_ulc_cmd.get().yaw_command);
236  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().linear_accel);
237  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().linear_decel);
238  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().jerk_limit_throttle);
239  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().jerk_limit_brake);
240  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().lateral_accel);
241  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().angular_accel);
242 
243  // Curvature steering
244  g_ulc_cmd.yaw_command = (INT16_MAX * CURVATURE_SCALE_FACTOR) + 0.05;
245  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::CURVATURE_MODE;
246  g_msg_ulc_cmd.clear();
247  g_msg_ulc_cfg.clear();
248  g_pub_ulc_cmd.publish(g_ulc_cmd);
249  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
250  EXPECT_EQ(INT16_MAX, g_msg_ulc_cmd.get().yaw_command);
251  /**************************************************************************/
252 
253  /*** +Inf tests ***********************************************************/
254  g_ulc_cmd.enable_pedals = true;
255  g_ulc_cmd.enable_steering = true;
256  g_ulc_cmd.linear_velocity = INFINITY;
257  g_ulc_cmd.accel_cmd = INFINITY;
258  g_ulc_cmd.linear_accel = INFINITY;
259  g_ulc_cmd.linear_decel = INFINITY;
260  g_ulc_cmd.jerk_limit_throttle = INFINITY;
261  g_ulc_cmd.jerk_limit_brake = INFINITY;
262  g_ulc_cmd.lateral_accel = INFINITY;
263  g_ulc_cmd.angular_accel = INFINITY;
264 
265  // Yaw rate steering
266  g_ulc_cmd.yaw_command = INFINITY;
267  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::YAW_RATE_MODE;
268  g_msg_ulc_cmd.clear();
269  g_msg_ulc_cfg.clear();
270  g_pub_ulc_cmd.publish(g_ulc_cmd);
271  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
272  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cfg));
273  EXPECT_EQ(INT16_MAX, g_msg_ulc_cmd.get().lon_command);
274  EXPECT_EQ(INT16_MAX, g_msg_ulc_cmd.get().yaw_command);
275  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().linear_accel);
276  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().linear_decel);
277  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().jerk_limit_throttle);
278  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().jerk_limit_brake);
279  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().lateral_accel);
280  EXPECT_EQ(UINT8_MAX, g_msg_ulc_cfg.get().angular_accel);
281 
282  // Curvature steering
283  g_ulc_cmd.yaw_command = INFINITY;
284  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::CURVATURE_MODE;
285  g_msg_ulc_cmd.clear();
286  g_msg_ulc_cfg.clear();
287  g_pub_ulc_cmd.publish(g_ulc_cmd);
288  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
289  EXPECT_EQ(INT16_MAX, g_msg_ulc_cmd.get().yaw_command);
290  /**************************************************************************/
291 
292  /*** -Inf tests ***********************************************************/
293  g_ulc_cmd.enable_pedals = true;
294  g_ulc_cmd.enable_steering = true;
295  g_ulc_cmd.linear_accel = -INFINITY;
296  g_ulc_cmd.linear_velocity = -INFINITY;
297  g_ulc_cmd.accel_cmd = 0.0;
298  g_ulc_cmd.linear_accel = -INFINITY;
299  g_ulc_cmd.linear_decel = -INFINITY;
300  g_ulc_cmd.pedals_mode = dataspeed_ulc_msgs::UlcCmd::SPEED_MODE;
301  g_ulc_cmd.jerk_limit_throttle = -INFINITY;
302  g_ulc_cmd.jerk_limit_brake = -INFINITY;
303  g_ulc_cmd.lateral_accel = -INFINITY;
304  g_ulc_cmd.angular_accel = -INFINITY;
305 
306  // Yaw rate steering
307  g_ulc_cmd.yaw_command = -INFINITY;
308  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::YAW_RATE_MODE;
309  g_msg_ulc_cmd.clear();
310  g_msg_ulc_cfg.clear();
311  g_pub_ulc_cmd.publish(g_ulc_cmd);
312  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
313  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cfg));
314  EXPECT_EQ(INT16_MIN, g_msg_ulc_cmd.get().lon_command);
315  EXPECT_EQ(INT16_MIN, g_msg_ulc_cmd.get().yaw_command);
316  EXPECT_EQ(0, g_msg_ulc_cfg.get().linear_accel);
317  EXPECT_EQ(0, g_msg_ulc_cfg.get().linear_decel);
318  EXPECT_EQ(0, g_msg_ulc_cfg.get().jerk_limit_throttle);
319  EXPECT_EQ(0, g_msg_ulc_cfg.get().jerk_limit_brake);
320  EXPECT_EQ(0, g_msg_ulc_cfg.get().lateral_accel);
321  EXPECT_EQ(0, g_msg_ulc_cfg.get().angular_accel);
322 
323  // Curvature steering
324  g_ulc_cmd.yaw_command = -INFINITY;
325  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::CURVATURE_MODE;
326  g_msg_ulc_cmd.clear();
327  g_msg_ulc_cfg.clear();
328  g_pub_ulc_cmd.publish(g_ulc_cmd);
329  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
330  EXPECT_EQ(INT16_MIN, g_msg_ulc_cmd.get().yaw_command);
331  /**************************************************************************/
332 }
333 
334 TEST(ULCNode, outOfBoundsInputs)
335 {
336  g_msg_ulc_cfg.clear();
337 
338  // NaN in longitudinal command fields
339  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
340  g_ulc_cmd.linear_velocity = NAN;
341  g_ulc_cmd.accel_cmd = 0.0;
342  g_ulc_cmd.pedals_mode = dataspeed_ulc_msgs::UlcCmd::SPEED_MODE;
343  g_msg_ulc_cmd.clear();
344  g_pub_ulc_cmd.publish(g_ulc_cmd);
345  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
346 
347  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
348  g_ulc_cmd.linear_velocity = 0.0;
349  g_ulc_cmd.accel_cmd = NAN;
350  g_ulc_cmd.pedals_mode = dataspeed_ulc_msgs::UlcCmd::ACCEL_MODE;
351  g_msg_ulc_cmd.clear();
352  g_pub_ulc_cmd.publish(g_ulc_cmd);
353  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
354 
355  // NaN in yaw command field
356  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
357  g_ulc_cmd.yaw_command = NAN;
358  g_msg_ulc_cmd.clear();
359  g_pub_ulc_cmd.publish(g_ulc_cmd);
360  ASSERT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
361 
362  // NaN in linear accel field
363  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
364  g_ulc_cmd.linear_accel = NAN;
365  g_msg_ulc_cmd.clear();
366  g_pub_ulc_cmd.publish(g_ulc_cmd);
367  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
368 
369  // NaN in linear decel field
370  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
371  g_ulc_cmd.linear_decel = NAN;
372  g_msg_ulc_cmd.clear();
373  g_pub_ulc_cmd.publish(g_ulc_cmd);
374  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
375 
376  // NaN in throttle jerk limit field
377  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
378  g_ulc_cmd.jerk_limit_throttle = NAN;
379  g_msg_ulc_cmd.clear();
380  g_pub_ulc_cmd.publish(g_ulc_cmd);
381  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
382 
383  // NaN in brake jerk limit field
384  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
385  g_ulc_cmd.jerk_limit_brake = NAN;
386  g_msg_ulc_cmd.clear();
387  g_pub_ulc_cmd.publish(g_ulc_cmd);
388  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
389 
390  // NaN in lateral accel field
391  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
392  g_ulc_cmd.lateral_accel = NAN;
393  g_msg_ulc_cmd.clear();
394  g_pub_ulc_cmd.publish(g_ulc_cmd);
395  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
396 
397  // NaN in angular accel field
398  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
399  g_ulc_cmd.angular_accel = NAN;
400  g_msg_ulc_cmd.clear();
401  g_pub_ulc_cmd.publish(g_ulc_cmd);
402  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
403 
404  // Invalid steering mode
405  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
406  g_ulc_cmd.steering_mode = 3;
407  g_msg_ulc_cmd.clear();
408  g_pub_ulc_cmd.publish(g_ulc_cmd);
409  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
410 
411  // Invalid pedals mode
412  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
413  g_ulc_cmd.pedals_mode = 3;
414  g_msg_ulc_cmd.clear();
415  g_pub_ulc_cmd.publish(g_ulc_cmd);
416  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
417 
418  // Make sure no config messages were sent during this process
419  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cfg));
420 }
421 
422 TEST(ULCNode, scaleFactors)
423 {
424  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
425  g_ulc_cmd.linear_accel = 1.23;
426  g_ulc_cmd.linear_decel = 3.45;
427  g_ulc_cmd.jerk_limit_throttle = 7.89;
428  g_ulc_cmd.jerk_limit_brake = 10.10;
429  g_ulc_cmd.lateral_accel = 5.43;
430  g_ulc_cmd.angular_accel = 3.21;
431 
432  // Speed mode
433  g_ulc_cmd.linear_velocity = 22.3;
434  g_ulc_cmd.accel_cmd = 0.0;
435  g_ulc_cmd.pedals_mode = dataspeed_ulc_msgs::UlcCmd::SPEED_MODE;
436  g_msg_ulc_cmd.clear();
437  g_msg_ulc_cfg.clear();
438  g_pub_ulc_cmd.publish(g_ulc_cmd);
439  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
440  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cfg));
441  EXPECT_EQ((int16_t)(g_ulc_cmd.linear_velocity / LIN_VEL_SCALE_FACTOR), g_msg_ulc_cmd.get().lon_command);
442  EXPECT_EQ((int16_t)(g_ulc_cmd.linear_accel / LINEAR_ACCEL_SCALE_FACTOR), g_msg_ulc_cfg.get().linear_accel);
443  EXPECT_EQ((int16_t)(g_ulc_cmd.linear_decel / LINEAR_DECEL_SCALE_FACTOR), g_msg_ulc_cfg.get().linear_decel);
444  EXPECT_EQ((int16_t)(g_ulc_cmd.jerk_limit_throttle / JERK_LIMIT_THROTTLE_SCALE_FACTOR), g_msg_ulc_cfg.get().jerk_limit_throttle);
445  EXPECT_EQ((int16_t)(g_ulc_cmd.jerk_limit_brake / JERK_LIMIT_BRAKE_SCALE_FACTOR), g_msg_ulc_cfg.get().jerk_limit_brake);
446  EXPECT_EQ((int16_t)(g_ulc_cmd.lateral_accel / LATERAL_ACCEL_SCALE_FACTOR), g_msg_ulc_cfg.get().lateral_accel);
447  EXPECT_EQ((int16_t)(g_ulc_cmd.angular_accel / ANGULAR_ACCEL_SCALE_FACTOR), g_msg_ulc_cfg.get().angular_accel);
448 
449  // Accel mode
450  g_ulc_cmd.linear_velocity = 0.0;
451  g_ulc_cmd.accel_cmd = 2.123;
452  g_ulc_cmd.pedals_mode = dataspeed_ulc_msgs::UlcCmd::ACCEL_MODE;
453  g_msg_ulc_cmd.clear();
454  g_pub_ulc_cmd.publish(g_ulc_cmd);
455  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
456  EXPECT_EQ((int16_t)(g_ulc_cmd.accel_cmd / ACCEL_CMD_SCALE_FACTOR), g_msg_ulc_cmd.get().lon_command);
457 
458  // Yaw rate steering
459  g_ulc_cmd.yaw_command = 0.567;
460  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::YAW_RATE_MODE;
461  g_msg_ulc_cmd.clear();
462  g_pub_ulc_cmd.publish(g_ulc_cmd);
463  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
464  EXPECT_EQ((int16_t)(g_ulc_cmd.yaw_command / YAW_RATE_SCALE_FACTOR), g_msg_ulc_cmd.get().yaw_command);
465 
466  // Curvature steering
467  g_ulc_cmd.yaw_command = 0.0789;
468  g_ulc_cmd.steering_mode = dataspeed_ulc_msgs::UlcCmd::CURVATURE_MODE;
469  g_msg_ulc_cmd.clear();
470  g_pub_ulc_cmd.publish(g_ulc_cmd);
471  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
472  EXPECT_EQ((int16_t)(g_ulc_cmd.yaw_command / CURVATURE_SCALE_FACTOR), g_msg_ulc_cmd.get().yaw_command);
473 }
474 
475 TEST(ULCNode, dbwEnable)
476 {
477  std_msgs::Bool dbw_enabled_msg;
478 
479  g_ulc_cmd = dataspeed_ulc_msgs::UlcCmd();
480  g_ulc_cmd.enable_pedals = true;
481  g_ulc_cmd.enable_steering = true;
482  g_ulc_cmd.enable_shifting = true;
483  g_ulc_cmd.shift_from_park = true;
484 
485  // Make sure CAN enable signals are false because dbw_enabled was not sent yet
486  g_msg_ulc_cmd.clear();
487  g_pub_ulc_cmd.publish(g_ulc_cmd);
488  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
489  EXPECT_FALSE(g_msg_ulc_cmd.get().enable_pedals);
490  EXPECT_FALSE(g_msg_ulc_cmd.get().enable_steering);
491  EXPECT_FALSE(g_msg_ulc_cmd.get().enable_shifting);
492  EXPECT_FALSE(g_msg_ulc_cmd.get().shift_from_park);
493 
494  // Publish dbw_enabled as true
495  dbw_enabled_msg.data = true;
496  g_pub_enable.publish(dbw_enabled_msg);
497  ros::WallDuration(0.001).sleep();
498 
499  // Send command again and make sure CAN enable signals are true
500  g_msg_ulc_cmd.clear();
501  g_pub_ulc_cmd.publish(g_ulc_cmd);
502  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
503  EXPECT_TRUE(g_msg_ulc_cmd.get().enable_pedals);
504  EXPECT_TRUE(g_msg_ulc_cmd.get().enable_steering);
505  EXPECT_TRUE(g_msg_ulc_cmd.get().enable_shifting);
506  EXPECT_TRUE(g_msg_ulc_cmd.get().shift_from_park);
507 
508  // Publish dbw_enabled as false and make sure CAN enable signals are false
509  dbw_enabled_msg.data = false;
510  g_pub_enable.publish(dbw_enabled_msg);
511  ros::WallDuration(0.05).sleep();
512  g_msg_ulc_cmd.clear();
513  g_pub_ulc_cmd.publish(g_ulc_cmd);
514  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
515  ASSERT_FALSE(g_msg_ulc_cmd.get().enable_pedals);
516  ASSERT_FALSE(g_msg_ulc_cmd.get().enable_steering);
517  ASSERT_FALSE(g_msg_ulc_cmd.get().enable_shifting);
518  ASSERT_FALSE(g_msg_ulc_cmd.get().shift_from_park);
519 }
520 
521 TEST(ULCNode, twistInputs)
522 {
523  geometry_msgs::Twist twist_cmd;
524  twist_cmd.linear.x = 22.0;
525  twist_cmd.angular.z = 0.2;
526  ros::WallDuration(1.0).sleep();
527 
528  g_msg_ulc_cmd.clear();
529  g_msg_ulc_cfg.clear();
530  g_pub_twist.publish(twist_cmd);
531  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
532  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.5), g_msg_ulc_cfg));
533  EXPECT_EQ((int16_t)(twist_cmd.linear.x / LIN_VEL_SCALE_FACTOR), g_msg_ulc_cmd.get().lon_command);
534  EXPECT_EQ((int16_t)(twist_cmd.angular.z / YAW_RATE_SCALE_FACTOR), g_msg_ulc_cmd.get().yaw_command);
535  EXPECT_EQ(0, g_msg_ulc_cmd.get().steering_mode);
536 
537  geometry_msgs::TwistStamped twist_stamped_cmd;
538  twist_stamped_cmd.twist = twist_cmd;
539  g_msg_ulc_cmd.clear();
540  g_msg_ulc_cfg.clear();
541  g_pub_twist_stamped.publish(twist_stamped_cmd);
542  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_cmd));
543  EXPECT_FALSE(waitForMsg(ros::WallDuration(0.5), g_msg_ulc_cfg));
544  EXPECT_EQ((int16_t)(twist_cmd.linear.x / LIN_VEL_SCALE_FACTOR), g_msg_ulc_cmd.get().lon_command);
545  EXPECT_EQ((int16_t)(twist_cmd.angular.z / YAW_RATE_SCALE_FACTOR), g_msg_ulc_cmd.get().yaw_command);
546  EXPECT_EQ(0, g_msg_ulc_cmd.get().steering_mode);
547 }
548 
549 TEST(ULCNode, reportParsing)
550 {
551  can_msgs::Frame report_out;
552  report_out.id = ID_ULC_REPORT;
553  report_out.is_extended = false;
554  report_out.dlc = sizeof(MsgUlcReport);
555  MsgUlcReport* msg_report_ptr = (MsgUlcReport*)report_out.data.elems;
556  memset(msg_report_ptr, 0x00, sizeof(MsgUlcReport));
557  msg_report_ptr->timeout = false;
558  msg_report_ptr->pedals_mode = 0;
559  msg_report_ptr->coasting = 0;
560  msg_report_ptr->steering_mode = 1;
561  msg_report_ptr->steering_enabled = false;
562  msg_report_ptr->pedals_enabled = true;
563  msg_report_ptr->speed_ref = 22.2f / SPEED_REPORT_SCALE_FACTOR;
564  msg_report_ptr->accel_ref = 1.1f / ACCEL_REPORT_SCALE_FACTOR;
565  msg_report_ptr->speed_meas = 21.1f / SPEED_REPORT_SCALE_FACTOR;
566  msg_report_ptr->accel_meas = 0.99f / ACCEL_REPORT_SCALE_FACTOR;
567  msg_report_ptr->max_steering_vel = 16.0f / MAX_RATE_SCALE_FACTOR;
568  msg_report_ptr->max_steering_angle = 55.0f / MAX_ANGLE_SCALE_FACTOR;
569  msg_report_ptr->speed_preempted = true;
570  msg_report_ptr->steering_preempted = false;
571  msg_report_ptr->override = true;
572 
573  g_pub_can.publish(report_out);
574  ASSERT_TRUE(waitForMsg(ros::WallDuration(0.1), g_msg_ulc_report));
575  ASSERT_FALSE(g_msg_ulc_report.get().timeout);
576  ASSERT_EQ(0, g_msg_ulc_report.get().pedals_mode);
577  ASSERT_EQ(0, g_msg_ulc_report.get().coasting);
578  ASSERT_EQ(1, g_msg_ulc_report.get().steering_mode);
579  ASSERT_FALSE(g_msg_ulc_report.get().steering_enabled);
580  ASSERT_TRUE(g_msg_ulc_report.get().pedals_enabled);
581  ASSERT_FLOAT_EQ(22.2f, g_msg_ulc_report.get().speed_ref);
582  ASSERT_FLOAT_EQ(1.1f, g_msg_ulc_report.get().accel_ref);
583  ASSERT_FLOAT_EQ(21.1f, g_msg_ulc_report.get().speed_meas);
584  ASSERT_FLOAT_EQ(0.95f, g_msg_ulc_report.get().accel_meas);
585  ASSERT_FLOAT_EQ(16.0f, g_msg_ulc_report.get().max_steering_vel);
586  ASSERT_FLOAT_EQ(55.0f, g_msg_ulc_report.get().max_steering_angle);
587  ASSERT_TRUE(g_msg_ulc_report.get().speed_preempted);
588  ASSERT_FALSE(g_msg_ulc_report.get().steering_preempted);
589 
590 }
591 
592 int main(int argc, char **argv) {
593  testing::InitGoogleTest(&argc, argv);
594  ros::init(argc, argv, "ulc_node_test");
595  n = new ros::NodeHandle();
596  pn = new ros::NodeHandle("~");
597 
598  const ros::TransportHints NODELAY = ros::TransportHints().tcpNoDelay();
599  g_sub_can = n->subscribe("can_tx", 100, recvCan, NODELAY);
600  g_sub_report = n->subscribe("ulc_report", 10, recvReport, NODELAY);
601  g_pub_ulc_cmd = n->advertise<dataspeed_ulc_msgs::UlcCmd>("ulc_cmd", 2);
602  g_pub_enable = n->advertise<std_msgs::Bool>("dbw_enabled", 2);
603  g_pub_twist = n->advertise<geometry_msgs::Twist>("cmd_vel", 2);
604  g_pub_twist_stamped = n->advertise<geometry_msgs::TwistStamped>("cmd_vel_stamped", 2);
605  g_pub_can = n->advertise<can_msgs::Frame>("can_rx", 100);
606  pn->param("config_frequency", g_cfg_freq, 5.0);
607 
608  // Setup Spinner
609  ros::AsyncSpinner spinner(3);
610  spinner.start();
611 
612  // Run all the tests that were declared with TEST()
613  int result = RUN_ALL_TESTS();
614 
615  // Cleanup
616  spinner.stop();
617  n->shutdown();
618  pn->shutdown();
619 
620  // Return test result
621  return result;
622 }
dataspeed_ulc_can::MsgUlcReport::speed_ref
int16_t speed_ref
Definition: dispatch.h:74
g_cfg_freq
double g_cfg_freq
Definition: test_ulc_node.cpp:21
ros::NodeHandle
g_pub_enable
ros::Publisher g_pub_enable
Definition: test_ulc_node.cpp:44
start
ROSCPP_DECL void start()
f
f
dataspeed_ulc_can::MsgUlcReport::speed_preempted
uint8_t speed_preempted
Definition: dispatch.h:88
dataspeed_ulc_can::MsgUlcReport::accel_ref
int8_t accel_ref
Definition: dispatch.h:82
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
g_sub_can
ros::Subscriber g_sub_can
Definition: test_ulc_node.cpp:48
n
ros::NodeHandle * n
Definition: test_ulc_node.cpp:14
pn
ros::NodeHandle * pn
Definition: test_ulc_node.cpp:14
ANGULAR_ACCEL_SCALE_FACTOR
const double ANGULAR_ACCEL_SCALE_FACTOR
Definition: test_ulc_node.cpp:33
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Subscriber::getNumPublishers
uint32_t getNumPublishers() const
MsgRx::fresh
bool fresh(ros::WallDuration delta) const
Definition: MsgRx.h:28
g_msg_ulc_cfg
MsgRx< MsgUlcCfg > g_msg_ulc_cfg(ros::WallDuration(0.05))
dataspeed_ulc_can
Definition: dispatch.h:39
dataspeed_ulc_can::MsgUlcReport::max_steering_vel
uint8_t max_steering_vel
Definition: dispatch.h:86
ACCEL_REPORT_SCALE_FACTOR
const double ACCEL_REPORT_SCALE_FACTOR
Definition: test_ulc_node.cpp:39
ACCEL_CMD_SCALE_FACTOR
const double ACCEL_CMD_SCALE_FACTOR
Definition: test_ulc_node.cpp:25
dataspeed_ulc_can::MsgUlcReport::steering_mode
uint16_t steering_mode
Definition: dispatch.h:81
waitForTopics
static bool waitForTopics(ros::WallDuration dur)
Definition: test_ulc_node.cpp:85
MAX_RATE_SCALE_FACTOR
const double MAX_RATE_SCALE_FACTOR
Definition: test_ulc_node.cpp:41
dataspeed_ulc_can::MsgUlcReport::steering_enabled
uint16_t steering_enabled
Definition: dispatch.h:80
ros::TransportHints::tcpNoDelay
TransportHints & tcpNoDelay(bool nodelay=true)
MAX_ANGLE_SCALE_FACTOR
const double MAX_ANGLE_SCALE_FACTOR
Definition: test_ulc_node.cpp:40
dataspeed_ulc_can::MsgUlcReport::timeout
uint16_t timeout
Definition: dispatch.h:75
main
int main(int argc, char **argv)
Definition: test_ulc_node.cpp:592
JERK_LIMIT_THROTTLE_SCALE_FACTOR
const double JERK_LIMIT_THROTTLE_SCALE_FACTOR
Definition: test_ulc_node.cpp:34
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
MsgRx
Definition: MsgRx.h:22
waitForMsg
static bool waitForMsg(ros::WallDuration dur, const MsgRx< T > &msg_rx)
Definition: test_ulc_node.cpp:71
g_pub_can
ros::Publisher g_pub_can
Definition: test_ulc_node.cpp:47
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::WallDuration::sleep
bool sleep() const
dataspeed_ulc_can::MsgUlcReport::steering_preempted
uint8_t steering_preempted
Definition: dispatch.h:87
recvReport
void recvReport(const dataspeed_ulc_msgs::UlcReportConstPtr &msg)
Definition: test_ulc_node.cpp:65
dataspeed_ulc_can::MsgUlcCmd
Definition: dispatch.h:42
LIN_VEL_SCALE_FACTOR
const double LIN_VEL_SCALE_FACTOR
Definition: test_ulc_node.cpp:24
JERK_LIMIT_BRAKE_SCALE_FACTOR
const double JERK_LIMIT_BRAKE_SCALE_FACTOR
Definition: test_ulc_node.cpp:35
checkImmediateCfg
static void checkImmediateCfg()
Definition: test_ulc_node.cpp:104
g_pub_twist
ros::Publisher g_pub_twist
Definition: test_ulc_node.cpp:45
g_sub_report
ros::Subscriber g_sub_report
Definition: test_ulc_node.cpp:49
g_msg_ulc_report
MsgRx< dataspeed_ulc_msgs::UlcReport > g_msg_ulc_report(ros::WallDuration(0.05))
dataspeed_ulc_can::MsgUlcReport::accel_meas
int8_t accel_meas
Definition: dispatch.h:83
TEST
TEST(ULCNode, topics)
Definition: test_ulc_node.cpp:113
CURVATURE_SCALE_FACTOR
const double CURVATURE_SCALE_FACTOR
Definition: test_ulc_node.cpp:27
dataspeed_ulc_can::MsgUlcReport::speed_meas
int16_t speed_meas
Definition: dispatch.h:78
ros::WallTime::now
static WallTime now()
dataspeed_ulc_can::MsgUlcReport::max_steering_angle
uint8_t max_steering_angle
Definition: dispatch.h:84
dataspeed_ulc_can::ID_ULC_CONFIG
Definition: dispatch.h:110
LATERAL_ACCEL_SCALE_FACTOR
const double LATERAL_ACCEL_SCALE_FACTOR
Definition: test_ulc_node.cpp:32
LINEAR_ACCEL_SCALE_FACTOR
const double LINEAR_ACCEL_SCALE_FACTOR
Definition: test_ulc_node.cpp:30
SPEED_REPORT_SCALE_FACTOR
const double SPEED_REPORT_SCALE_FACTOR
Definition: test_ulc_node.cpp:38
LINEAR_DECEL_SCALE_FACTOR
const double LINEAR_DECEL_SCALE_FACTOR
Definition: test_ulc_node.cpp:31
dataspeed_ulc_can::MsgUlcCfg
Definition: dispatch.h:62
dataspeed_ulc_can::MsgUlcReport::coasting
uint8_t coasting
Definition: dispatch.h:85
g_pub_twist_stamped
ros::Publisher g_pub_twist_stamped
Definition: test_ulc_node.cpp:46
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
dataspeed_ulc_can::MsgUlcReport::override
uint16_t override
Definition: dispatch.h:79
dataspeed_ulc_can::ID_ULC_CMD
Definition: dispatch.h:109
dataspeed_ulc_can::MsgUlcReport::pedals_enabled
uint16_t pedals_enabled
Definition: dispatch.h:76
dataspeed_ulc_can::ID_ULC_REPORT
Definition: dispatch.h:111
g_pub_ulc_cmd
ros::Publisher g_pub_ulc_cmd
Definition: test_ulc_node.cpp:43
g_msg_ulc_cmd
MsgRx< MsgUlcCmd > g_msg_ulc_cmd(ros::WallDuration(0.05))
g_ulc_cmd
dataspeed_ulc_msgs::UlcCmd g_ulc_cmd
Definition: test_ulc_node.cpp:20
YAW_RATE_SCALE_FACTOR
const double YAW_RATE_SCALE_FACTOR
Definition: test_ulc_node.cpp:26
dataspeed_ulc_can::dispatchAssertSizes
static void dispatchAssertSizes()
Definition: dispatch.h:100
recvCan
void recvCan(const can_msgs::FrameConstPtr &msg)
Definition: test_ulc_node.cpp:51
dataspeed_ulc_can::MsgUlcReport
Definition: dispatch.h:73
dataspeed_ulc_can::MsgUlcReport::pedals_mode
uint16_t pedals_mode
Definition: dispatch.h:77

dataspeed_ulc_can
Author(s): Micho Radovnikovich
autogenerated on Fri Dec 2 2022 03:20:37