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

ubiquity_motor
Author(s):
autogenerated on Mon Jun 10 2019 15:37:24