vigir_footstep_planning_plugins: post_process

Go to the documentation of this file.
00001 #include <vigir_footstep_planning_plugins/plugins/post_process_plugin.h>
00002 
00003 
00004 
00005 namespace vigir_footstep_planning
00006 {
00007 PostProcessPlugin::PostProcessPlugin(const std::string& name)
00008   : Plugin(name)
00009 {
00010 }
00011 
00012 bool PostProcessPlugin::isUnique() const
00013 {
00014   return false;
00015 }
00016 
00017 void PostProcessPlugin::postProcessStepForward(const State& /*left_foot*/, const State& /*right_foot*/, State& /*swing_foot*/) const
00018 {
00019 }
00020 
00021 void PostProcessPlugin::postProcessStepBackward(const State& /*left_foot*/, const State& /*right_foot*/, State& /*swing_foot*/) const
00022 {
00023 }
00024 
00025 void PostProcessPlugin::postProcessStep(const msgs::Step& /*left_foot*/, const msgs::Step& /*right_foot*/, msgs::Step& /*swing_foot*/, msgs::StepPlan& /*step_plan*/) const
00026 {
00027 }
00028 
00029 void PostProcessPlugin::postProcess(msgs::StepPlan step_plan) const
00030 {
00031   if (step_plan.steps.empty())
00032     return;
00033 
00034   std::vector<msgs::Step>::iterator itr = step_plan.steps.begin();
00035 
00036   // determine start configuration
00037   msgs::Step left_foot, right_foot = *itr;
00038 
00039   if (left_foot.foot.foot_index != msgs::Foot::LEFT)
00040     left_foot.foot = step_plan.start.left;
00041 
00042   if (right_foot.foot.foot_index != msgs::Foot::RIGHT)
00043     right_foot.foot = step_plan.start.right;
00044 
00045   // iterate step plan
00046   for (; itr != step_plan.steps.end(); itr++)
00047   {
00048     msgs::Step& swing_foot = *itr;
00049 
00050     postProcessStep(left_foot, right_foot, swing_foot, step_plan);
00051 
00052     if (swing_foot.foot.foot_index == msgs::Foot::LEFT)
00053       left_foot = swing_foot;
00054     else
00055       right_foot = swing_foot;
00056   }
00057 }
00058 }