motor_serial_test.cc
Go to the documentation of this file.
1 
31 #include <gtest/gtest.h>
32 
33 #include <ros/ros.h>
34 #include <ubiquity_motor/motor_message.h>
35 #include <ubiquity_motor/motor_serial.h>
36 
37 #include <string>
38 
39 #if defined(__linux__)
40 #include <pty.h>
41 #else
42 #include <util.h>
43 #endif
44 
45 class MotorSerialTests : public ::testing::Test {
46 protected:
47  virtual void SetUp() {
48  if (openpty(&master_fd, &slave_fd, name, NULL, NULL) == -1) {
49  perror("openpty");
50  exit(127);
51  }
52 
53  ASSERT_TRUE(master_fd > 0);
54  ASSERT_TRUE(slave_fd > 0);
55  ASSERT_TRUE(std::string(name).length() > 0);
56 
57  ros::Time::init();
58  motors = new MotorSerial(std::string(name), 9600);
59  }
60 
61  virtual void TearDown() { delete motors; }
62 
63  MotorSerial *motors;
64  int master_fd;
65  int slave_fd;
66  char name[100];
67 };
68 
69 TEST(MotorSerialNoFixtureTests, badPortnameException) {
70  ASSERT_THROW(MotorSerial motors(std::string("foo"), 9600),
71  serial::IOException);
72 }
73 
74 TEST_F(MotorSerialTests, serialClosedOnInterupt) {
75  ASSERT_EQ(true, motors->motors.isOpen());
76  motors->serial_thread.interrupt();
77  sleep(1);
78  ASSERT_EQ(false, motors->motors.isOpen());
79 }
80 
81 TEST_F(MotorSerialTests, goodReadWorks) {
82  uint8_t test[] = {0x7E, 0x3B, 0x2A, 0x00, 0x00, 0x01, 0x2C, 0x6D};
83  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
84  ASSERT_NE(-1, write(master_fd, test, 8));
85 
86  while (!motors->commandAvailable()) {
87  }
88 
89  MotorMessage mm;
90  mm = motors->receiveCommand();
91  ASSERT_EQ(300, mm.getData());
92  ASSERT_EQ(MotorMessage::TYPE_WRITE, mm.getType());
93  ASSERT_EQ(MotorMessage::REG_BOTH_SPEED_SET, mm.getRegister());
94 }
95 
96 TEST_F(MotorSerialTests, misalignedOneGoodReadWorks) {
97  uint8_t test[] = {0x00, 0x7E, 0x3B, 0x2A, 0x00, 0x00, 0x01, 0x2C, 0x6D};
98  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
99  ASSERT_NE(-1, write(master_fd, test, 9));
100 
101  while (!motors->commandAvailable()) {
102  }
103 
104  MotorMessage mm;
105  mm = motors->receiveCommand();
106  ASSERT_EQ(300, mm.getData());
107  ASSERT_EQ(MotorMessage::TYPE_WRITE, mm.getType());
108  ASSERT_EQ(MotorMessage::REG_BOTH_SPEED_SET, mm.getRegister());
109 }
110 
111 TEST_F(MotorSerialTests, misalignedManyGoodReadWorks) {
112  uint8_t test[] = {0x01, 0x2C, 0x0E, 0x7E, 0x3B, 0x2A,
113  0x00, 0x00, 0x01, 0x2C, 0x6D};
114  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
115  ASSERT_NE(-1, write(master_fd, test, 11));
116 
117  while (!motors->commandAvailable()) {
118  }
119 
120  MotorMessage mm;
121  mm = motors->receiveCommand();
122  ASSERT_EQ(300, mm.getData());
123  ASSERT_EQ(MotorMessage::TYPE_WRITE, mm.getType());
124  ASSERT_EQ(MotorMessage::REG_BOTH_SPEED_SET, mm.getRegister());
125 }
126 
127 TEST_F(MotorSerialTests, errorReadWorks) {
128  uint8_t test[] = {0x7E, 0x3D, 0x2A, 0x00, 0x00, 0x00, 0x00, 0x98};
129  // uint8_t test[]= {0x7E, 0x02, 0xBB, 0x07, 0x00, 0x00, 0x01, 0x2C, 0x0E};
130  ASSERT_NE(-1, write(master_fd, test, 8));
131 
132  while (!motors->commandAvailable()) {
133  }
134 
135  MotorMessage mm;
136  mm = motors->receiveCommand();
137  ASSERT_EQ(MotorMessage::TYPE_ERROR, mm.getType());
138 }
139 
140 TEST_F(MotorSerialTests, badReadFails) {
141  uint8_t test[] = {0xdd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
142  // uint8_t test[]= {0x7E, 0x02, 0xBB, 0x07, 0x00, 0x00, 0x01, 0x2C, 0x0E};
143  ASSERT_NE(-1, write(master_fd, test, 9));
144 
145  ros::Rate loop(100);
146  int times = 0;
147  while (!motors->commandAvailable()) {
148  loop.sleep();
149  times++;
150  if (times >= 20) {
151  break;
152  }
153  }
154 
155  if (times >= 20) {
156  SUCCEED();
157  } else {
158  FAIL();
159  }
160 }
161 
162 TEST_F(MotorSerialTests, misalignedOneBadReadFails) {
163  uint8_t test[] = {0x00, 0x7d, 0x3B, 0x2A, 0x00, 0x00, 0x01, 0x2C, 0x6D};
164  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
165  ASSERT_NE(-1, write(master_fd, test, 9));
166 
167  ros::Rate loop(100);
168  int times = 0;
169  while (!motors->commandAvailable()) {
170  loop.sleep();
171  times++;
172  if (times >= 20) {
173  break;
174  }
175  }
176 
177  if (times >= 20) {
178  SUCCEED();
179  } else {
180  FAIL();
181  }
182 }
183 
184 TEST_F(MotorSerialTests, incompleteReadFails) {
185  uint8_t test[] = {0x7E, 0x3B, 0x00};
186  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
187  ASSERT_NE(-1, write(master_fd, test, 3));
188 
189  ros::Rate loop(100);
190  int times = 0;
191  while (!motors->commandAvailable()) {
192  loop.sleep();
193  times++;
194  if (times >= 20) {
195  break;
196  }
197  }
198 
199  if (times >= 20) {
200  SUCCEED();
201  } else {
202  FAIL();
203  }
204 }
205 
206 TEST_F(MotorSerialTests, incompleteMisalignedReadFails) {
207  uint8_t test[] = {0x0f, 0x7E, 0x3B, 0x00};
208  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
209  ASSERT_NE(-1, write(master_fd, test, 4));
210 
211  ros::Rate loop(100);
212  int times = 0;
213  while (!motors->commandAvailable()) {
214  loop.sleep();
215  times++;
216  if (times >= 20) {
217  break;
218  }
219  }
220 
221  if (times >= 20) {
222  SUCCEED();
223  } else {
224  FAIL();
225  }
226 }
227 
228 TEST_F(MotorSerialTests, badProtocolReadFails) {
229  uint8_t test[] = {0x7E, 0xFB, 0x07, 0x00, 0x00, 0x00, 0x00, 0xFB};
230  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
231  ASSERT_NE(-1, write(master_fd, test, 8));
232 
233  ros::Rate loop(100);
234  int times = 0;
235  while (!motors->commandAvailable()) {
236  loop.sleep();
237  times++;
238  if (times >= 20) {
239  break;
240  }
241  }
242 
243  if (times >= 20) {
244  SUCCEED();
245  } else {
246  FAIL();
247  }
248 }
249 
250 TEST_F(MotorSerialTests, badTypeReadFails) {
251  uint8_t test[] = {0x7E, 0x2E, 0x07, 0x00, 0x00, 0x00, 0x00, 0xCA};
252  // char test[]= {0x0E, 0x2C, 0x01, 0x00, 0x00, 0x07, 0xBB, 0x02, 0x7E};
253  ASSERT_NE(-1, write(master_fd, test, 8));
254 
255  ros::Rate loop(100);
256  int times = 0;
257  while (!motors->commandAvailable()) {
258  loop.sleep();
259  times++;
260  if (times >= 20) {
261  break;
262  }
263  }
264 
265  if (times >= 20) {
266  SUCCEED();
267  } else {
268  FAIL();
269  }
270 }
271 
272 TEST_F(MotorSerialTests, writeOutputs) {
273  MotorMessage version;
274  version.setRegister(MotorMessage::REG_FIRMWARE_VERSION);
275  version.setType(MotorMessage::TYPE_READ);
276  version.setData(0);
277  motors->transmitCommand(version);
278 
279  int count = 0;
280  while (count < 8) {
281  ASSERT_NE(-1, ioctl(master_fd, TIOCINQ, &count));
282  }
283  EXPECT_EQ(8, count);
284 
285  RawMotorMessage input;
286  EXPECT_EQ(input.size(), read(master_fd, input.c_array(), input.size()));
287 
288  ASSERT_EQ(input, version.serialize());
289 }
290 
291 TEST_F(MotorSerialTests, writeMultipleOutputs) {
292  std::vector<MotorMessage> commands;
293 
294  MotorMessage left_odom;
295  left_odom.setRegister(MotorMessage::REG_BOTH_ODOM);
296  left_odom.setType(MotorMessage::TYPE_READ);
297  left_odom.setData(0);
298  commands.push_back(left_odom);
299 
300  MotorMessage right_odom;
301  right_odom.setRegister(MotorMessage::REG_BOTH_ODOM);
302  right_odom.setType(MotorMessage::TYPE_READ);
303  right_odom.setData(0);
304  commands.push_back(right_odom);
305 
306  MotorMessage left_vel;
307  left_vel.setRegister(MotorMessage::REG_MAX_SPEED_FWD);
308  left_vel.setType(MotorMessage::TYPE_READ);
309  left_vel.setData(0);
310  commands.push_back(left_vel);
311 
312  MotorMessage right_vel;
313  right_vel.setRegister(MotorMessage::REG_MAX_SPEED_REV);
314  right_vel.setType(MotorMessage::TYPE_READ);
315  right_vel.setData(0);
316  commands.push_back(right_vel);
317 
318  motors->transmitCommands(commands);
319 
320  fd_set fds;
321  FD_ZERO(&fds);
322  FD_SET(master_fd, &fds);
323  struct timeval timeout = { 10, 0 }; /* 10 seconds */
324 
325  uint8_t arr[32];
326  int count = 0;
327 
328  while (count < 31) {
329  // Wait for data
330  int ret = select(master_fd+1, &fds, NULL, NULL, &timeout);
331  EXPECT_EQ(1, ret);
332 
333  // Get number of bytes availible
334  int avail = 0;
335  ASSERT_NE(-1, ioctl(master_fd, TIOCINQ, &avail));
336 
337  // Read bytes into the buffer
338  EXPECT_EQ(avail, read(master_fd, arr+count, avail));
339  count += avail;
340  }
341 
342  ASSERT_EQ(32, count);
343 
344  std::vector<uint8_t> input(arr, arr + sizeof(arr) / sizeof(uint8_t));
345 
346  std::vector<uint8_t> expected(0);
347  for (std::vector<MotorMessage>::iterator i = commands.begin();
348  i != commands.end(); ++i) {
349  RawMotorMessage serialized = i->serialize();
350  expected.insert(expected.end(), serialized.begin(), serialized.end());
351  }
352 
353  ASSERT_EQ(expected, input);
354 }
355 
356 int main(int argc, char **argv) {
357  testing::InitGoogleTest(&argc, argv);
358  return RUN_ALL_TESTS();
359 }
MotorSerialTests::TearDown
virtual void TearDown()
Definition: motor_serial_test.cc:61
TIOCINQ
#define TIOCINQ
Definition: unix.cc:41
upgrade_firmware.version
version
Definition: upgrade_firmware.py:67
MotorMessage::REG_BOTH_ODOM
@ REG_BOTH_ODOM
Definition: motor_message.h:147
MotorSerialTests::motors
MotorSerial * motors
Definition: motor_serial_test.cc:63
ros.h
MotorMessage::getType
MotorMessage::MessageTypes getType() const
Definition: motor_message.cc:112
MotorSerial
Definition: motor_serial.h:43
MotorMessage::REG_FIRMWARE_VERSION
@ REG_FIRMWARE_VERSION
Definition: motor_message.h:131
MotorMessage::REG_BOTH_SPEED_SET
@ REG_BOTH_SPEED_SET
Definition: motor_message.h:142
main
int main(int argc, char **argv)
Definition: motor_serial_test.cc:356
MotorSerialTests::name
char name[100]
Definition: motor_serial_test.cc:66
MotorMessage::TYPE_ERROR
@ TYPE_ERROR
Definition: motor_message.h:78
MotorMessage::TYPE_READ
@ TYPE_READ
Definition: motor_message.h:75
test
MotorMessage::setData
void setData(int32_t data)
Definition: motor_message.cc:126
serial::IOException
Definition: serial.h:688
MotorSerialTests::SetUp
virtual void SetUp()
Definition: motor_serial_test.cc:47
motor_serial.h
TEST
TEST(MotorSerialNoFixtureTests, badPortnameException)
Definition: motor_serial_test.cc:69
python_serial_test.timeout
timeout
Definition: python_serial_test.py:10
MotorMessage::REG_MAX_SPEED_FWD
@ REG_MAX_SPEED_FWD
Definition: motor_message.h:154
MotorSerialTests
Definition: motor_serial_test.cc:45
MotorSerialTests::master_fd
int master_fd
Definition: motor_serial_test.cc:64
ros::Rate::sleep
bool sleep()
MotorMessage::TYPE_WRITE
@ TYPE_WRITE
Definition: motor_message.h:76
MotorMessage::getData
int32_t getData() const
Definition: motor_message.cc:135
ros::Time::init
static void init()
motor_message.h
MotorMessage::getRegister
MotorMessage::Registers getRegister() const
Definition: motor_message.cc:122
MotorSerialTests::slave_fd
int slave_fd
Definition: motor_serial_test.cc:65
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
ros::Rate
RawMotorMessage
boost::array< uint8_t, 8 > RawMotorMessage
Definition: motor_message.h:38
MotorMessage::setRegister
void setRegister(MotorMessage::Registers reg)
Definition: motor_message.cc:116
MotorMessage::setType
void setType(MotorMessage::MessageTypes type)
Definition: motor_message.cc:106
MotorMessage::REG_MAX_SPEED_REV
@ REG_MAX_SPEED_REV
Definition: motor_message.h:155
MotorMessage
Definition: motor_message.h:67
TEST_F
TEST_F(MotorSerialTests, serialClosedOnInterupt)
Definition: motor_serial_test.cc:74

ubiquity_motor
Author(s):
autogenerated on Thu Nov 16 2023 03:30:55