motor_hardware_test.cc
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include <ros/ros.h>
4 #include <ubiquity_motor/motor_hardware.h>
5 #include <ubiquity_motor/motor_message.h>
6 #include <limits>
7 
8 #if defined(__linux__)
9 #include <pty.h>
10 #else
11 #include <util.h>
12 #endif
13 
14 class MotorHardwareTests : public ::testing::Test {
15 protected:
16  virtual void SetUp() {
17  if (openpty(&master_fd, &slave_fd, name, NULL, NULL) == -1) {
18  perror("openpty");
19  exit(127);
20  }
21 
22  ASSERT_TRUE(master_fd > 0);
23  ASSERT_TRUE(slave_fd > 0);
24  ASSERT_TRUE(std::string(name).length() > 0);
25 
26  CommsParams cp(nh);
27  cp.serial_port = std::string(name);
28  FirmwareParams fp(nh);
29 
30  robot = new MotorHardware(nh, cp, fp);
31  }
32 
33  virtual void TearDown() { delete robot; }
34 
35  void wait_for_read(){
36  unsigned int count = 0;
37 
38  fd_set fds;
39  FD_ZERO(&fds);
40  FD_SET(master_fd, &fds);
41  struct timeval timeout = { 10, 0 }; /* 10 seconds */
42  int ret = select(master_fd+1, &fds, NULL, NULL, &timeout);
43  EXPECT_EQ(1, ret);
44 
45  ASSERT_NE(-1, ioctl(master_fd, TIOCINQ, &count));
46  EXPECT_EQ(8, count);
47  usleep(1000);
48  }
49 
50  void wait_for_write(){
51  unsigned int count_out = 8;
52  while (count_out > 0) {
53  ASSERT_NE(-1, ioctl(master_fd, TIOCOUTQ, &count_out));
54  }
55  EXPECT_EQ(0, count_out);
56  usleep(10000);
57  }
58 
59  MotorHardware *robot;
60  ros::NodeHandle nh;
61  int master_fd;
62  int slave_fd;
63  char name[100];
64 };
65 
66 
67 TEST_F(MotorHardwareTests, writeSpeedsOutputs) {
68  robot->writeSpeeds();
69 
70  int aval;
71  RawMotorMessage out;
72 
73  wait_for_read();
74 
75  // Make sure that we get exactly 1 message out on port
76  ASSERT_NE(-1, ioctl(master_fd, FIONREAD, &aval));
77  ASSERT_EQ(out.size(), aval);
78  ASSERT_EQ(out.size(), read(master_fd, out.c_array(), out.size()));
79 
80  MotorMessage mm;
81  ASSERT_EQ(0, mm.deserialize(out));
82  ASSERT_EQ(MotorMessage::REG_BOTH_SPEED_SET, mm.getRegister());
83  ASSERT_EQ(0, mm.getData());
84 }
85 
86 TEST_F(MotorHardwareTests, nonZeroWriteSpeedsOutputs) {
87  robot->joints_[0].velocity_command = 5;
88  robot->joints_[1].velocity_command = -5;
89  robot->writeSpeeds();
90 
91  int aval;
92  RawMotorMessage out;
93 
94  wait_for_read();
95 
96  // Make sure that we get exactly 1 message out on port
97  ASSERT_NE(-1, ioctl(master_fd, FIONREAD, &aval));
98  ASSERT_EQ(out.size(), aval);
99  ASSERT_EQ(out.size(), read(master_fd, out.c_array(), out.size()));
100 
101  MotorMessage mm;
102  ASSERT_EQ(0, mm.deserialize(out));
103  ASSERT_EQ(MotorMessage::REG_BOTH_SPEED_SET, mm.getRegister());
104  int16_t left = (mm.getData() >> 16) & 0xffff;
105  int16_t right = mm.getData() & 0xffff;
106 
107  // Left is 5 rad/s so it should be positive
108  ASSERT_LT(0, left);
109  // Right is -5 rad/s so it should be positive
110  ASSERT_GT(0, right);
111 }
112 
113 TEST_F(MotorHardwareTests, odomUpdatesPosition) {
114  MotorMessage mm;
115  mm.setType(MotorMessage::TYPE_RESPONSE);
116  mm.setRegister(MotorMessage::REG_BOTH_ODOM);
117  mm.setData((50 << 16) | (-50 & 0x0000ffff));
118 
119  RawMotorMessage out = mm.serialize();
120  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
121  wait_for_write();
122  robot->readInputs();
123 
124  double left = robot->joints_[0].position;
125  double right = robot->joints_[1].position;
126 
127  // Left is 5 rad/s so it should be positive
128  ASSERT_LT(0, left);
129  // Right is -5 rad/s so it should be negative
130  ASSERT_GT(0, right);
131 
132  // Send zero and re-read
133  mm.setData((0 << 16) | (0 & 0x0000ffff));
134  out = mm.serialize();
135  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
136  wait_for_write();
137  robot->readInputs();
138 
139  // Make sure that the value stays same
140  ASSERT_EQ(left, robot->joints_[0].position);
141  ASSERT_EQ(right, robot->joints_[1].position);
142 
143  // Send original message again and re-read
144  mm.setData((50 << 16) | (-50 & 0x0000ffff));
145  out = mm.serialize();
146  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
147  wait_for_write();
148  robot->readInputs();
149 
150  // Make sure that the value accumulates
151  ASSERT_DOUBLE_EQ(left * 2, robot->joints_[0].position);
152  ASSERT_DOUBLE_EQ(right * 2, robot->joints_[1].position);
153 
154  // Invert the odom message and re-send/read
155  mm.setData((-50 << 16) | (50 & 0x0000ffff));
156  out = mm.serialize();
157  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
158  wait_for_write();
159  robot->readInputs();
160 
161  // Values should be back the the first reading
162  ASSERT_DOUBLE_EQ(left, robot->joints_[0].position);
163  ASSERT_DOUBLE_EQ(right, robot->joints_[1].position);
164 }
165 
166 TEST_F(MotorHardwareTests, odomUpdatesPositionMax) {
167  MotorMessage mm;
168  mm.setType(MotorMessage::TYPE_RESPONSE);
169  mm.setRegister(MotorMessage::REG_BOTH_ODOM);
170  mm.setData((std::numeric_limits<int16_t>::max() << 16) |
171  (std::numeric_limits<int16_t>::min() & 0x0000ffff));
172 
173  RawMotorMessage out = mm.serialize();
174  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
175  wait_for_write();
176  robot->readInputs();
177 
178  double left = robot->joints_[0].position;
179  double right = robot->joints_[1].position;
180 
181  // Left is + rad/s so it should be positive
182  ASSERT_LT(0, left);
183  // Right is - rad/s so it should be negative
184  ASSERT_GT(0, right);
185 
186  // Send zero and re-read
187  mm.setData((0 << 16) | (0 & 0x0000ffff));
188  out = mm.serialize();
189  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
190  wait_for_write();
191  robot->readInputs();
192 
193  // Make sure that the value stays same
194  ASSERT_EQ(left, robot->joints_[0].position);
195  ASSERT_EQ(right, robot->joints_[1].position);
196 
197  // Send original message again and re-read
198  mm.setData((std::numeric_limits<int16_t>::max() << 16) |
199  (std::numeric_limits<int16_t>::min() & 0x0000ffff));
200  out = mm.serialize();
201  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
202  wait_for_write();
203  robot->readInputs();
204 
205  // Make sure that the value accumulates
206  ASSERT_DOUBLE_EQ(left * 2, robot->joints_[0].position);
207  ASSERT_DOUBLE_EQ(right * 2, robot->joints_[1].position);
208 
209  // Invert the odom message and re-send/read
210  mm.setData((std::numeric_limits<int16_t>::min() << 16) |
211  (std::numeric_limits<int16_t>::max() & 0x0000ffff));
212  out = mm.serialize();
213  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
214  wait_for_write();
215  robot->readInputs();
216 
217  // Values should be back the the first reading
218  // Need to use NEAR due to high precision loss
219  ASSERT_NEAR(left, robot->joints_[0].position, 0.1);
220  ASSERT_NEAR(right, robot->joints_[1].position, 0.1);
221 }
222 
223 TEST_F(MotorHardwareTests, requestVersionOutputs) {
224  robot->requestVersion();
225 
226  wait_for_read();
227 
228  int aval;
229  RawMotorMessage out;
230  // Make sure that we get exactly 1 message out on port
231  ASSERT_NE(-1, ioctl(master_fd, FIONREAD, &aval));
232  ASSERT_EQ(out.size(), aval);
233  ASSERT_EQ(out.size(), read(master_fd, out.c_array(), out.size()));
234 
235  MotorMessage mm;
236  ASSERT_EQ(0, mm.deserialize(out));
237  ASSERT_EQ(MotorMessage::REG_FIRMWARE_VERSION, mm.getRegister());
238  ASSERT_EQ(0, mm.getData());
239 }
240 
241 TEST_F(MotorHardwareTests, oldFirmwareThrows) {
242  MotorMessage mm;
243  mm.setType(MotorMessage::TYPE_RESPONSE);
244  mm.setRegister(MotorMessage::REG_FIRMWARE_VERSION);
245  mm.setData(10);
246 
247  RawMotorMessage out = mm.serialize();
248  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
249 
250  wait_for_write();
251  ASSERT_THROW(robot->readInputs(), std::runtime_error);
252 }
253 
254 TEST_F(MotorHardwareTests, setDeadmanTimerOutputs) {
255  robot->setDeadmanTimer(1000);
256  wait_for_read();
257 
258  int aval;
259  RawMotorMessage out;
260  // Make sure that we get exactly 1 message out on port
261  ASSERT_NE(-1, ioctl(master_fd, FIONREAD, &aval));
262  ASSERT_EQ(out.size(), aval);
263  ASSERT_EQ(out.size(), read(master_fd, out.c_array(), out.size()));
264 
265  MotorMessage mm;
266  ASSERT_EQ(0, mm.deserialize(out));
267  ASSERT_EQ(MotorMessage::REG_DEADMAN, mm.getRegister());
268  ASSERT_EQ(1000, mm.getData());
269 }
270 
271 TEST_F(MotorHardwareTests, setParamsSendParams) {
272  FirmwareParams fp;
273  fp.pid_proportional = 12;
274  fp.pid_integral = 12;
275  fp.pid_derivative = 12;
276  fp.pid_denominator = 12;
277  fp.pid_moving_buffer_size = 12;
278 
279  robot->setParams(fp);
280 
281  int aval;
282  RawMotorMessage out;
283 
284  for (int i; i < 5; ++i) {
285  robot->sendParams();
286  wait_for_read();
287  // Make sure that we get exactly 1 message out on port each time
288  ASSERT_NE(-1, ioctl(master_fd, FIONREAD, &aval));
289  ASSERT_EQ(out.size(), aval);
290  ASSERT_EQ(out.size(), read(master_fd, out.c_array(), out.size()));
291  }
292 }
293 
294 static bool called;
295 
296 void callbackU(const std_msgs::UInt32 &data) {
297  ASSERT_EQ(10, data.data);
298  called = true;
299 }
300 
301 void callbackS(const std_msgs::Int32 &data) {
302  ASSERT_EQ(-10, data.data);
303  called = true;
304 }
305 
306 /*
307 TEST_F(MotorHardwareTests, debugRegisterUnsignedPublishes) {
308  called = false;
309  ros::Subscriber sub = nh.subscribe("u50", 1, callbackU);
310 
311  MotorMessage mm;
312  mm.setType(MotorMessage::TYPE_RESPONSE);
313  mm.setRegister(static_cast<MotorMessage::Registers>(0x50));
314  mm.setData(10);
315 
316  RawMotorMessage out = mm.serialize();
317  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
318 
319  wait_for_write();
320  robot->readInputs();
321  usleep(5000);
322  ros::spinOnce();
323  ASSERT_TRUE(called);
324 
325  called = false;
326 }
327 
328 TEST_F(MotorHardwareTests, debugRegisterSignedPublishes) {
329  called = false;
330  ros::Subscriber sub = nh.subscribe("s50", 1, callbackS);
331 
332  MotorMessage mm;
333  mm.setType(MotorMessage::TYPE_RESPONSE);
334  mm.setRegister(static_cast<MotorMessage::Registers>(0x50));
335  mm.setData(-10);
336 
337  RawMotorMessage out = mm.serialize();
338  ASSERT_EQ(out.size(), write(master_fd, out.c_array(), out.size()));
339 
340  wait_for_write();
341  robot->readInputs();
342  usleep(5000);
343  ros::spinOnce();
344  ASSERT_TRUE(called);
345 
346  called = false;
347 }
348 */
349 
350 int main(int argc, char **argv) {
351  testing::InitGoogleTest(&argc, argv);
352  ros::init(argc, argv, "param_test");
353  return RUN_ALL_TESTS();
354 }
called
static bool called
Definition: motor_hardware_test.cc:294
FirmwareParams::pid_proportional
int32_t pid_proportional
Definition: motor_parmeters.h:47
ros::NodeHandle
FirmwareParams::pid_denominator
int32_t pid_denominator
Definition: motor_parmeters.h:51
MotorMessage::setData
void setData(int32_t data)
Definition: motor_message.cc:123
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
RawMotorMessage
boost::array< uint8_t, 8 > RawMotorMessage
Definition: motor_message.h:38
motor_message.h
callbackS
void callbackS(const std_msgs::Int32 &data)
Definition: motor_hardware_test.cc:301
FirmwareParams::pid_moving_buffer_size
int32_t pid_moving_buffer_size
Definition: motor_parmeters.h:52
MotorHardware::joints_
struct MotorHardware::Joint joints_[2]
FirmwareParams::pid_integral
int32_t pid_integral
Definition: motor_parmeters.h:48
MotorHardware::setDeadmanTimer
void setDeadmanTimer(int32_t deadman)
Definition: motor_hardware.cc:416
FirmwareParams::pid_derivative
int32_t pid_derivative
Definition: motor_parmeters.h:49
MotorHardware::setParams
void setParams(FirmwareParams firmware_params)
Definition: motor_hardware.cc:434
MotorMessage::deserialize
MotorMessage::ErrorCodes deserialize(const RawMotorMessage &serialized)
Definition: motor_message.cc:148
MotorMessage::getData
int32_t getData() const
Definition: motor_message.cc:132
MotorMessage::setRegister
void setRegister(MotorMessage::Registers reg)
Definition: motor_message.cc:113
main
int main(int argc, char **argv)
Definition: motor_hardware_test.cc:350
TEST_F
TEST_F(MotorHardwareTests, writeSpeedsOutputs)
Definition: motor_hardware_test.cc:67
MotorMessage::serialize
RawMotorMessage serialize() const
Definition: motor_message.cc:138
MotorMessage::setType
void setType(MotorMessage::MessageTypes type)
Definition: motor_message.cc:103
callbackU
void callbackU(const std_msgs::UInt32 &data)
Definition: motor_hardware_test.cc:296
motor_hardware.h
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
CommsParams::serial_port
std::string serial_port
Definition: motor_parmeters.h:141
MotorMessage::getRegister
MotorMessage::Registers getRegister() const
Definition: motor_message.cc:119

ubiquity_motor
Author(s):
autogenerated on Mon Feb 28 2022 23:57:06