Browse files

[halfsteps_pattern_generator] Add test, fix footprint read/write.

  • Loading branch information...
1 parent 304bb93 commit 7c87fbaef1de040118dbcdf7f0d384916eee4b2b @thomas-moulard thomas-moulard committed Mar 15, 2012
View
5 halfsteps_pattern_generator/CMakeLists.txt
@@ -38,3 +38,8 @@ rosbuild_add_executable(generator
target_link_libraries(generator ${PROJECT_NAME})
rosbuild_add_boost_directories()
+
+# Tests.
+rosbuild_add_gtest(pattern-generator test/pattern-generator.cpp)
+target_link_libraries(pattern-generator ${PROJECT_NAME})
+rosbuild_link_boost(pattern-generator filesystem date_time system)
View
23 halfsteps_pattern_generator/include/yaml.hh
@@ -9,8 +9,15 @@ namespace walk
inline YAML::Emitter&
operator<< (YAML::Emitter& out, const footprint_t& footprint)
{
- out << (const walk::Footprint2d&) (footprint)
- << YAML::BeginMap
+ out << YAML::BeginMap
+ << YAML::Key << "beginTime"
+ << YAML::Value
+ << to_iso_string (footprint.beginTime)
+ << YAML::Key << "duration"
+ << YAML::Value
+ << (0. + footprint.duration.total_nanoseconds () / 1e9)
+ << YAML::Key << "position"
+ << YAML::Value << footprint.position
<< YAML::Key << "slide-up"
<< YAML::Value
<< footprint.slideUp
@@ -43,7 +50,17 @@ namespace walk
{
checkYamlType (node, YAML::NodeType::Map, "footprint");
- node >> (walk::Footprint2d&) (footprint);
+ using namespace boost::posix_time;
+ using namespace boost::gregorian;
+
+ std::string beginTimeStr;
+ node["beginTime"] >> beginTimeStr;
+ footprint.beginTime = walk::Time (from_iso_string (beginTimeStr));
+ double d = 0;
+ node["duration"] >> d;
+ footprint.duration = milliseconds (d * 1e3);
+ node["position"] >> footprint.position;
+
node["slide-up"] >> footprint.slideUp;
node["slide-down"] >> footprint.slideDown;
node["horizontal-distance"] >> footprint.horizontalDistance;
View
6 ...teps_pattern_generator/src/libhalfsteps_pattern_generator/halfsteps_pattern_generator.cpp
@@ -101,6 +101,12 @@ HalfStepsPatternGenerator::computeTrajectories()
walk_msgs::Footprint2d footprint;
walk_msgs::convertHomogeneousMatrix3dToFootprint2d
(footprint, footprint_M_newfootprint);
+
+ if (footprint.x != footprint.x
+ || footprint.y != footprint.y
+ || footprint.theta != footprint.theta)
+ throw std::runtime_error("Nan detected in footprints");
+
stepData.push_back(this->footprints()[i].slideUp);
stepData.push_back(-this->footprints()[i].horizontalDistance);
stepData.push_back(this->footprints()[i].stepHeight);
View
45 halfsteps_pattern_generator/test/pattern-generator.cpp
@@ -0,0 +1,45 @@
+#include <iostream>
+#include <sstream>
+#include <gtest/gtest.h>
+
+#include "halfsteps_pattern_generator.hh"
+#include "halfsteps_pattern_generator/Footprint.h"
+#include "yaml.hh"
+
+
+TEST(TestStampedPosition, empty)
+{
+ HalfStepsPatternGenerator pg(0.95, 1.05, 0.005);
+ HalfStepsPatternGenerator::footprints_t footprints;
+
+ HalfStepsPatternGenerator::footprint_t footprint;
+
+ using namespace boost::posix_time;
+ using namespace boost::gregorian;
+ walk::TimeDuration d (milliseconds (5));
+ footprint.duration = d;
+ footprint.beginTime = walk::Time (date(1970,1,1), seconds(1));
+ footprints.push_back (footprint);
+
+ pg.setFootprints(footprints, true);
+
+ const walk::Trajectory3d& lf = pg.leftFootTrajectory();
+
+ walk::TimeDuration lengthLf = lf.computeLength();
+
+ walk::YamlWriter<HalfStepsPatternGenerator> writer (pg);
+ writer.write("/tmp/test.yaml");
+
+ // walk::YamlReader<halfStepsPgParent_t> reader ("/tmp/test.yaml");
+
+ // walk::YamlWriter<walk::YamlReader<halfStepsPgParent_t> > writer2 (reader);
+ // writer2.write("/tmp/test2.yaml");
+
+ // walk::YamlReader<halfStepsPgParent_t> reader2 ("/tmp/test.yaml");
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}

0 comments on commit 7c87fba

Please sign in to comment.