segwayrmp_tests.cc
Go to the documentation of this file.
00001 #include "gtest/gtest.h"
00002 
00003 // OMG this is so nasty...
00004 #define private public
00005 #define protected public
00006 #include "segwayrmp/segwayrmp.h"
00007 #include "segwayrmp/impl/rmp_io.h"
00008 
00009 using namespace segwayrmp;
00010 
00011 namespace {
00012 
00013 class PacketTests : public ::testing::Test {
00014 protected:
00015     virtual void SetUp() {
00016         segway_rmp = new SegwayRMP(no_interface);
00017         ss = SegwayStatus::Ptr(new SegwayStatus);
00018         
00019         pck.channel = 0xAA;
00020         pck.id = 0x0400;
00021         pck.data[0] = 0x00;
00022         pck.data[1] = 0x00;
00023         pck.data[2] = 0x00;
00024         pck.data[3] = 0x00;
00025         pck.data[4] = 0x00;
00026         pck.data[5] = 0x00;
00027         pck.data[6] = 0x00;
00028         pck.data[7] = 0x00;
00029     }
00030     
00031     SegwayRMP *segway_rmp;
00032     Packet pck;
00033     SegwayStatus::Ptr ss;
00034 };
00035 
00036 TEST_F(PacketTests, IgnoresChannelB) {
00037     pck.channel = 0xBB;
00038     pck.id = 0x0401;
00039     segway_rmp->ParsePacket_(pck, ss);
00040     
00041     ASSERT_FALSE(ss->touched);
00042 }
00043 
00044 TEST_F(PacketTests, IgnoresCommandRequest) {
00045     segway_rmp->ParsePacket_(pck,ss);
00046     
00047     ASSERT_FALSE(ss->touched);
00048 }
00049 
00050 TEST_F(PacketTests, ParsesPitchRollCorrectly) {
00051     pck.id = 0x0401;
00052     pck.data[0] = 0xFF;
00053     pck.data[1] = 0xF9;
00054     pck.data[2] = 0x00;
00055     pck.data[3] = 0x0F;
00056     pck.data[4] = 0xFF;
00057     pck.data[5] = 0xFF;
00058     pck.data[6] = 0x00;
00059     pck.data[7] = 0x09;
00060     
00061     segway_rmp->ParsePacket_(pck,ss);
00062     EXPECT_TRUE(ss->touched);
00063     EXPECT_NEAR(-0.897,ss->pitch,0.128);
00064     EXPECT_NEAR(1.923,ss->pitch_rate,0.128);
00065     EXPECT_NEAR(-0.128,ss->roll,0.128);
00066     EXPECT_NEAR(1.154,ss->roll_rate,0.128);
00067 }
00068 
00069 TEST_F(PacketTests, ParsesWheelSpeedsYawRatesServoFramesCorrectly) {
00070     pck.id = 0x0402;
00071     pck.data[0] = 0x00;
00072     pck.data[1] = 0x0C;
00073     pck.data[2] = 0xFF;
00074     pck.data[3] = 0xF2;
00075     pck.data[4] = 0x00;
00076     pck.data[5] = 0x4B;
00077     pck.data[6] = 0x18;
00078     pck.data[7] = 0xD4;
00079     
00080     segway_rmp->ParsePacket_(pck,ss);
00081     EXPECT_TRUE(ss->touched);
00082     EXPECT_NEAR(0.0361,ss->left_wheel_speed,0.003);
00083     EXPECT_NEAR(-0.0422,ss->right_wheel_speed,0.003);
00084     EXPECT_NEAR(9.6154,ss->yaw_rate,0.128);
00085     EXPECT_NEAR(63.56,ss->servo_frames,0.01);
00086 }
00087 
00088 TEST_F(PacketTests, ParsesIntegratedWheelsCorrectly) {
00089     pck.id = 0x0403;
00090     pck.data[0] = 0xFE;
00091     pck.data[1] = 0x06;
00092     pck.data[2] = 0xFF;
00093     pck.data[3] = 0xFF;
00094     pck.data[4] = 0x00;
00095     pck.data[5] = 0x4F;
00096     pck.data[6] = 0x00;
00097     pck.data[7] = 0x00;
00098     
00099     segway_rmp->ParsePacket_(pck,ss);
00100     EXPECT_TRUE(ss->touched);
00101     EXPECT_NEAR(-0.0152341,ss->integrated_left_wheel_position,0.00003);
00102     EXPECT_NEAR(0.0023784,ss->integrated_right_wheel_position,0.00003);
00103 }
00104 
00105 TEST_F(PacketTests, ParsesIntegratedDistanceTurnCorrectly) {
00106     pck.id = 0x0404;
00107     pck.channel = 0xAA;
00108     // Packet id: 404, Packet Channel: AA,
00109     // Packet Data: 0xFC 0x19 0xFF 0xFF
00110     //              0xFF 0xAC 0xFF 0xFF
00111     pck.data[0] = 0xFC;
00112     pck.data[1] = 0x19;
00113     pck.data[2] = 0xFF;
00114     pck.data[3] = 0xFF;
00115     pck.data[4] = 0xFF;
00116     pck.data[5] = 0xAC;
00117     pck.data[6] = 0xFF;
00118     pck.data[7] = 0xFF;
00119     
00120     segway_rmp->ParsePacket_(pck,ss);
00121     EXPECT_TRUE(ss->touched);
00122     EXPECT_NEAR(-0.0300768,ss->integrated_forward_position,0.00003);
00123     EXPECT_NEAR(-0.268452,ss->integrated_turn_position,0.00001);
00124 }
00125 
00126 TEST_F(PacketTests, ParsesMotorTorqueCorrectly) {
00127     pck.id = 0x0405;
00128     // Packet id: 405, Packet Channel: AA, Packet Data: 0xFF 0xF9 0x00 0xA7 0x00 0x80 0x00 0x00
00129     // 1094
00130     pck.data[0] = 0xFF;
00131     pck.data[1] = 0xF9;
00132     pck.data[2] = 0x00;
00133     pck.data[3] = 0xA7;
00134     pck.data[4] = 0x00;
00135     pck.data[5] = 0x80;
00136     pck.data[6] = 0x00;
00137     pck.data[7] = 0x00;
00138     
00139     segway_rmp->ParsePacket_(pck,ss);
00140     EXPECT_TRUE(ss->touched);
00141     EXPECT_NEAR(-0.006399,ss->left_motor_torque,0.0009);
00142     EXPECT_NEAR(0.152651,ss->right_motor_torque,0.0009);
00143 }
00144 
00145 TEST_F(PacketTests, ParsesStatusesCorrectly) {
00146     pck.id = 0x0406;
00147     // Packet id: 406, Packet Channel: AA, Packet Data: 0x00 0x01 0x00 0x00 0x02 0x7A 0x01 0x38
00148     pck.data[0] = 0x00;
00149     pck.data[1] = 0x01;
00150     pck.data[2] = 0x00;
00151     pck.data[3] = 0x00;
00152     pck.data[4] = 0x02;
00153     pck.data[5] = 0x7A;
00154     pck.data[6] = 0x01;
00155     pck.data[7] = 0x38;
00156     
00157     segway_rmp->ParsePacket_(pck,ss);
00158     EXPECT_TRUE(ss->touched);
00159     // EXPECT_EQ(1,(int)ss.operational_mode);
00160     // EXPECT_EQ(0,(int)ss.controller_gain_schedule);
00161     EXPECT_NEAR(9.325,ss->ui_battery_voltage,0.25);
00162     EXPECT_NEAR(78.0,ss->powerbase_battery_voltage,0.25);
00163 }
00164 
00165 // TODO: Add tests for motor enabled/disabled and commanded velocity and yaw rate
00166 
00167 }  // namespace
00168 
00169 int main(int argc, char **argv) {
00170     ::testing::InitGoogleTest(&argc, argv);
00171     return RUN_ALL_TESTS();
00172 }


libsegwayrmp
Author(s): William Woodall
autogenerated on Fri Jan 3 2014 11:28:21