segwayrmp_tests.cc
Go to the documentation of this file.
1 #include "gtest/gtest.h"
2 
3 // OMG this is so nasty...
4 #define private public
5 #define protected public
6 #include "segwayrmp/segwayrmp.h"
7 #include "segwayrmp/impl/rmp_io.h"
8 
9 using namespace segwayrmp;
10 
11 namespace {
12 
13 class PacketTests : public ::testing::Test {
14 protected:
15  virtual void SetUp() {
16  segway_rmp = new SegwayRMP(no_interface);
17  ss = SegwayStatus::Ptr(new SegwayStatus);
18 
19  pck.channel = 0xAA;
20  pck.id = 0x0400;
21  pck.data[0] = 0x00;
22  pck.data[1] = 0x00;
23  pck.data[2] = 0x00;
24  pck.data[3] = 0x00;
25  pck.data[4] = 0x00;
26  pck.data[5] = 0x00;
27  pck.data[6] = 0x00;
28  pck.data[7] = 0x00;
29  }
30 
31  SegwayRMP *segway_rmp;
32  Packet pck;
33  SegwayStatus::Ptr ss;
34 };
35 
36 TEST_F(PacketTests, IgnoresChannelB) {
37  pck.channel = 0xBB;
38  pck.id = 0x0401;
39  segway_rmp->ParsePacket_(pck, ss);
40 
41  ASSERT_FALSE(ss->touched);
42 }
43 
44 TEST_F(PacketTests, IgnoresCommandRequest) {
45  segway_rmp->ParsePacket_(pck,ss);
46 
47  ASSERT_FALSE(ss->touched);
48 }
49 
50 TEST_F(PacketTests, ParsesPitchRollCorrectly) {
51  pck.id = 0x0401;
52  pck.data[0] = 0xFF;
53  pck.data[1] = 0xF9;
54  pck.data[2] = 0x00;
55  pck.data[3] = 0x0F;
56  pck.data[4] = 0xFF;
57  pck.data[5] = 0xFF;
58  pck.data[6] = 0x00;
59  pck.data[7] = 0x09;
60 
61  segway_rmp->ParsePacket_(pck,ss);
62  EXPECT_TRUE(ss->touched);
63  EXPECT_NEAR(-0.897,ss->pitch,0.128);
64  EXPECT_NEAR(1.923,ss->pitch_rate,0.128);
65  EXPECT_NEAR(-0.128,ss->roll,0.128);
66  EXPECT_NEAR(1.154,ss->roll_rate,0.128);
67 }
68 
69 TEST_F(PacketTests, ParsesWheelSpeedsYawRatesServoFramesCorrectly) {
70  pck.id = 0x0402;
71  pck.data[0] = 0x00;
72  pck.data[1] = 0x0C;
73  pck.data[2] = 0xFF;
74  pck.data[3] = 0xF2;
75  pck.data[4] = 0x00;
76  pck.data[5] = 0x4B;
77  pck.data[6] = 0x18;
78  pck.data[7] = 0xD4;
79 
80  segway_rmp->ParsePacket_(pck,ss);
81  EXPECT_TRUE(ss->touched);
82  EXPECT_NEAR(0.0361,ss->left_wheel_speed,0.003);
83  EXPECT_NEAR(-0.0422,ss->right_wheel_speed,0.003);
84  EXPECT_NEAR(9.6154,ss->yaw_rate,0.128);
85  EXPECT_NEAR(63.56,ss->servo_frames,0.01);
86 }
87 
88 TEST_F(PacketTests, ParsesIntegratedWheelsCorrectly) {
89  pck.id = 0x0403;
90  pck.data[0] = 0xFE;
91  pck.data[1] = 0x06;
92  pck.data[2] = 0xFF;
93  pck.data[3] = 0xFF;
94  pck.data[4] = 0x00;
95  pck.data[5] = 0x4F;
96  pck.data[6] = 0x00;
97  pck.data[7] = 0x00;
98 
99  segway_rmp->ParsePacket_(pck,ss);
100  EXPECT_TRUE(ss->touched);
101  EXPECT_NEAR(-0.0152341,ss->integrated_left_wheel_position,0.00003);
102  EXPECT_NEAR(0.0023784,ss->integrated_right_wheel_position,0.00003);
103 }
104 
105 TEST_F(PacketTests, ParsesIntegratedDistanceTurnCorrectly) {
106  pck.id = 0x0404;
107  pck.channel = 0xAA;
108  // Packet id: 404, Packet Channel: AA,
109  // Packet Data: 0xFC 0x19 0xFF 0xFF
110  // 0xFF 0xAC 0xFF 0xFF
111  pck.data[0] = 0xFC;
112  pck.data[1] = 0x19;
113  pck.data[2] = 0xFF;
114  pck.data[3] = 0xFF;
115  pck.data[4] = 0xFF;
116  pck.data[5] = 0xAC;
117  pck.data[6] = 0xFF;
118  pck.data[7] = 0xFF;
119 
120  segway_rmp->ParsePacket_(pck,ss);
121  EXPECT_TRUE(ss->touched);
122  EXPECT_NEAR(-0.0300768,ss->integrated_forward_position,0.00003);
123  EXPECT_NEAR(-0.268452,ss->integrated_turn_position,0.00001);
124 }
125 
126 TEST_F(PacketTests, ParsesMotorTorqueCorrectly) {
127  pck.id = 0x0405;
128  // Packet id: 405, Packet Channel: AA, Packet Data: 0xFF 0xF9 0x00 0xA7 0x00 0x80 0x00 0x00
129  // 1094
130  pck.data[0] = 0xFF;
131  pck.data[1] = 0xF9;
132  pck.data[2] = 0x00;
133  pck.data[3] = 0xA7;
134  pck.data[4] = 0x00;
135  pck.data[5] = 0x80;
136  pck.data[6] = 0x00;
137  pck.data[7] = 0x00;
138 
139  segway_rmp->ParsePacket_(pck,ss);
140  EXPECT_TRUE(ss->touched);
141  EXPECT_NEAR(-0.006399,ss->left_motor_torque,0.0009);
142  EXPECT_NEAR(0.152651,ss->right_motor_torque,0.0009);
143 }
144 
145 TEST_F(PacketTests, ParsesStatusesCorrectly) {
146  pck.id = 0x0406;
147  // Packet id: 406, Packet Channel: AA, Packet Data: 0x00 0x01 0x00 0x00 0x02 0x7A 0x01 0x38
148  pck.data[0] = 0x00;
149  pck.data[1] = 0x01;
150  pck.data[2] = 0x00;
151  pck.data[3] = 0x00;
152  pck.data[4] = 0x02;
153  pck.data[5] = 0x7A;
154  pck.data[6] = 0x01;
155  pck.data[7] = 0x38;
156 
157  segway_rmp->ParsePacket_(pck,ss);
158  EXPECT_TRUE(ss->touched);
159  // EXPECT_EQ(1,(int)ss.operational_mode);
160  // EXPECT_EQ(0,(int)ss.controller_gain_schedule);
161  EXPECT_NEAR(9.325,ss->ui_battery_voltage,0.25);
162  EXPECT_NEAR(78.0,ss->powerbase_battery_voltage,0.25);
163 }
164 
165 // TODO: Add tests for motor enabled/disabled and commanded velocity and yaw rate
166 
167 } // namespace
168 
169 int main(int argc, char **argv) {
170  ::testing::InitGoogleTest(&argc, argv);
171  return RUN_ALL_TESTS();
172 }
rmp_io.h
segwayrmp::SegwayStatus::Ptr
boost::shared_ptr< SegwayStatus > Ptr
Definition: segwayrmp.h:325
segwayrmp::Packet::channel
unsigned char channel
Definition: rmp_io.h:57
main
int main(int argc, char **argv)
Definition: segwayrmp_tests.cc:169

libsegwayrmp
Author(s): William Woodall
autogenerated on Mon Jun 10 2019 13:46:50