00001 #include <iostream> 00002 #include <sstream> 00003 #include <gtest/gtest.h> 00004 00005 #include "walk_interfaces/pattern-generator.hh" 00006 #include "walk_interfaces/binary.hh" 00007 #include "walk_interfaces/yaml.hh" 00008 00009 class MyPatternGenerator : public walk::DiscretizedPatternGenerator2d 00010 { 00011 public: 00012 MyPatternGenerator() 00013 : walk::DiscretizedPatternGenerator2d() 00014 {} 00015 00016 void computeTrajectories() 00017 { 00018 walk::TimeDuration l = 00019 walk::computeFootprintSequenceLength(footprints()); 00020 00021 Trajectory3d& lf = getLeftFootTrajectory(); 00022 Trajectory3d& rf = getRightFootTrajectory(); 00023 TrajectoryV3d& com = getCenterOfMassTrajectory(); 00024 TrajectoryV2d& zmp = getZmpTrajectory(); 00025 00026 lf.data().resize(1); 00027 rf.data().resize(1); 00028 com.data().resize(1); 00029 zmp.data().resize(1); 00030 00031 lf.data()[0].duration = l; 00032 rf.data()[0].duration = l; 00033 com.data()[0].duration = l; 00034 zmp.data()[0].duration = l; 00035 00036 lf.data()[0].position = initialLeftFootPosition(); 00037 rf.data()[0].position = initialRightFootPosition(); 00038 com.data()[0].position = initialCenterOfMassPosition(); 00039 walk::Vector2d initialZmp; 00040 initialZmp[0] = initialZmp[1] = 0.; 00041 zmp.data()[0].position = initialZmp; 00042 } 00043 }; 00044 00045 TEST(TestStampedPosition, empty) 00046 { 00047 MyPatternGenerator pg; 00048 walk::StampedFootprint2dSequence footprints; 00049 00050 walk::StampedFootprint2d footprint; 00051 00052 using namespace boost::posix_time; 00053 using namespace boost::gregorian; 00054 walk::TimeDuration d (milliseconds (5)); 00055 footprint.duration = d; 00056 footprint.beginTime = walk::Time (date(1970,1,1), seconds(1)); 00057 footprints.push_back (footprint); 00058 00059 pg.setFootprints(footprints, true); 00060 00061 const MyPatternGenerator::Trajectory3d& lf = pg.leftFootTrajectory(); 00062 00063 walk::TimeDuration lengthLf = lf.computeLength(); 00064 00065 EXPECT_EQ(d, lengthLf); 00066 00067 walk::YamlWriter<MyPatternGenerator> writer (pg); 00068 writer.write("/tmp/test.yaml"); 00069 00070 walk::BinaryWriter<MyPatternGenerator> binaryWriter (pg); 00071 binaryWriter.write("/tmp/test.bin"); 00072 00073 walk::YamlReader<MyPatternGenerator> reader ("/tmp/test.yaml"); 00074 00075 walk::BinaryReader<MyPatternGenerator> binaryReader ("/tmp/test.bin"); 00076 00077 walk::YamlWriter<walk::YamlReader<MyPatternGenerator> > writer2 (reader); 00078 writer2.write("/tmp/test2.yaml"); 00079 00080 walk::YamlReader<MyPatternGenerator> reader2 ("/tmp/test.yaml"); 00081 } 00082 00083 int main(int argc, char **argv) 00084 { 00085 testing::InitGoogleTest(&argc, argv); 00086 return RUN_ALL_TESTS(); 00087 }