step_plan_utest.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <math.h>
00004 
00005 #include <gtest/gtest.h>
00006 
00007 #include <vigir_footstep_planning_msgs/step_plan.h>
00008 #include <vigir_footstep_planning_msgs/msgs_test_helper.h>
00009 
00010 
00011 
00012 using namespace vigir_footstep_planning;
00013 
00014 msgs::StepPlan genStepPlan(geometry_msgs::Pose start, unsigned int steps, unsigned int foot_index = msgs::Foot::LEFT, unsigned int start_step_index = 0, bool closing_step = true)
00015 {
00016   msgs::StepPlan step_plan;
00017 
00018   double yaw = tf::getYaw(start.orientation);
00019 
00020   // generate initial step
00021   msgs::Step step;
00022   step.foot.foot_index = foot_index;
00023   step.foot.pose = start;
00024   step.step_index = start_step_index;
00025   step.valid = true;
00026 
00027   step_plan.steps.push_back(step);
00028 
00029   // add extra step when closing step is set
00030   if (closing_step)
00031     steps++;
00032 
00033   for (unsigned int i = 0; i < steps; i++)
00034   {
00035     // alternate left and right foot pose
00036     if (step.foot.foot_index == msgs::Foot::RIGHT)
00037     {
00038       step.foot.foot_index = msgs::Foot::LEFT;
00039       step.foot.pose.position.x += 0.2 * -sin(yaw);
00040       step.foot.pose.position.y += 0.2 * cos(yaw);
00041     }
00042     else
00043     {
00044       step.foot.foot_index = msgs::Foot::RIGHT;
00045       step.foot.pose.position.x += -0.2 * -sin(yaw);
00046       step.foot.pose.position.y += -0.2 * cos(yaw);
00047     }
00048 
00049     // forward movement
00050     // closing step handling
00051     if (!closing_step || i < steps-1)
00052     {
00053       step.foot.pose.position.x += 0.25 * cos(yaw);
00054       step.foot.pose.position.y += 0.25 * sin(yaw);
00055     }
00056 
00057     step.step_index++;
00058 
00059     step_plan.steps.push_back(step);
00060   }
00061   return step_plan;
00062 }
00063 
00064 // Test for checking if appendStepPlan generates correct results
00065 TEST(StepPlan, appendStepPlan)
00066 {
00067   geometry_msgs::Pose start;
00068   start.position.x = 1.0;
00069   start.position.z = 1.0;
00070   start.orientation = tf::createQuaternionMsgFromYaw(0.0);
00071 
00072   // generate plan 1: 4 steps, no closing step
00073   msgs::StepPlan step_plan_1 = genStepPlan(start, 4, msgs::Foot::LEFT, 0, false);
00074 
00075   // generate plan 2: 6 steps, starting from last step of step_plan_1
00076   msgs::StepPlan step_plan_2 = genStepPlan(step_plan_1.steps.back().foot.pose, 6, msgs::Foot::LEFT);
00077   step_plan_2.steps.erase(step_plan_2.steps.begin());
00078 
00079   // append plan 1 and 2
00080   StepPlan step_plan_result(step_plan_1);
00081   step_plan_result.appendStepPlan(step_plan_2);
00082   ASSERT_EQ(12, step_plan_result.size());
00083 
00084   // generate expected step plan
00085   StepPlan step_plan_exp(genStepPlan(start, 10));
00086   ASSERT_EQ(12, step_plan_exp.size());
00087 
00088   // compare
00089   isEqualTest(step_plan_exp, step_plan_result);
00090 }
00091 
00092 // Test for checking if updateStepPlan generates correct results
00093 TEST(StepPlan, updateStepPlan)
00094 {
00095   geometry_msgs::Pose start;
00096   start.position.x = 1.0;
00097   start.position.z = 1.0;
00098   start.orientation = tf::createQuaternionMsgFromYaw(0.0);
00099 
00100   // generate plan 1: 6 steps
00101   msgs::StepPlan step_plan_1 = genStepPlan(start, 6, msgs::Foot::LEFT);
00102 
00103   // generate plan 2: 8 steps, starting from step #3 of step_plan_1
00104   msgs::StepPlan step_plan_2 = genStepPlan(step_plan_1.steps[3].foot.pose, 8, msgs::Foot::RIGHT, 3);
00105 
00106   // update plan 1 with 2
00107   StepPlan step_plan_result(step_plan_1);
00108   step_plan_result.updateStepPlan(step_plan_2);
00109   ASSERT_EQ(13, step_plan_result.size());
00110 
00111   // generate expected step plan
00112   StepPlan step_plan_exp(genStepPlan(start, 11));
00113   ASSERT_EQ(13, step_plan_exp.size());
00114 
00115   for (size_t i = 3; i < 8; i++)
00116   {
00117     msgs::Step step;
00118     EXPECT_TRUE(step_plan_exp.getStep(step, i));
00119     step_plan_exp.updateStep(step);
00120   }
00121 
00122   // compare
00123   isEqualTest(step_plan_exp, step_plan_result);
00124 }
00125 
00126 // Test for checking if stitchStepPlan generates correct results
00127 TEST(StepPlan, stitchStepPlan)
00128 {
00129   geometry_msgs::Pose start_1;
00130   start_1.position.x = 1.0;
00131   start_1.position.y = 1.1;
00132   start_1.position.z = 1.0;
00133   start_1.orientation = tf::createQuaternionMsgFromYaw(M_PI_4);
00134 
00135   // generate plan 1: 6 steps
00136   msgs::StepPlan step_plan_1 = genStepPlan(start_1, 6);
00137 
00138   geometry_msgs::Pose start_2;
00139   start_2.position.x = 2.1;
00140   start_2.position.y = 1.0;
00141   start_2.position.z = 1.0;
00142   start_2.orientation = tf::createQuaternionMsgFromYaw(1.0);
00143 
00144   // generate plan 2: 6 steps
00145   msgs::StepPlan step_plan_2 = genStepPlan(start_2, 6, msgs::Foot::RIGHT, 3);
00146 
00147   // stitch plan 1 with 2
00148   StepPlan step_plan_result(step_plan_1);
00149   step_plan_result.stitchStepPlan(step_plan_2, 3);
00150   ASSERT_EQ(11, step_plan_result.size());
00151 
00152   // generate expected step plan
00153   StepPlan step_plan_exp(genStepPlan(start_1, 9));
00154   ASSERT_EQ(11, step_plan_exp.size());
00155 
00156   for (size_t i = 3; i < 8; i++)
00157   {
00158     msgs::Step step;
00159     EXPECT_TRUE(step_plan_exp.getStep(step, i));
00160     step_plan_exp.updateStep(step);
00161   }
00162 
00163   // compare
00164   isEqualTest(step_plan_exp, step_plan_result);
00165 }


vigir_footstep_planning_msgs
Author(s): Alexander Stumpf
autogenerated on Sat Jul 8 2017 02:21:30