step_plan.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_msgs/step_plan.h>
00002 
00003 
00004 
00005 namespace vigir_footstep_planning
00006 {
00007 StepPlan::StepPlan(const msgs::StepPlan& step_plan)
00008 {
00009   fromMsg(step_plan);
00010 }
00011 
00012 StepPlan::StepPlan()
00013   : mode(0)
00014 {
00015 }
00016 
00017 StepPlan& StepPlan::operator=(const msgs::StepPlan& step_plan)
00018 {
00019   fromMsg(step_plan);
00020   return *this;
00021 }
00022 
00023 StepPlan& StepPlan::operator+(const msgs::StepPlan& step_plan)
00024 {
00025   appendStepPlan(step_plan);
00026   return *this;
00027 }
00028 
00029 StepPlan& StepPlan::operator|(const msgs::StepPlan& step_plan)
00030 {
00031   updateStepPlan(step_plan);
00032   return *this;
00033 }
00034 
00035 StepPlan& StepPlan::operator+(const msgs::Step& step)
00036 {
00037   insertStep(step);
00038   return *this;
00039 }
00040 
00041 StepPlan& StepPlan::operator|(const msgs::Step& step)
00042 {
00043   updateStep(step);
00044   return *this;
00045 }
00046 
00047 StepPlan& StepPlan::operator-(const msgs::Step& step)
00048 {
00049   removeStep(step.step_index);
00050   return *this;
00051 }
00052 
00053 int StepPlan::getFirstStepIndex() const
00054 {
00055   boost::shared_lock<boost::shared_mutex> lock(step_plan_mutex);
00056 
00057   if (steps.empty())
00058     return -1;
00059   else
00060     return steps.begin()->second.step_index;
00061 }
00062 
00063 int StepPlan::getLastStepIndex() const
00064 {
00065   boost::shared_lock<boost::shared_mutex> lock(step_plan_mutex);
00066 
00067   if (steps.empty())
00068     return -1;
00069   else
00070     return steps.rbegin()->second.step_index;
00071 }
00072 
00073 msgs::ErrorStatus StepPlan::insertStep(const msgs::Step& step)
00074 {
00075   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00076   return _insertStep(step);
00077 }
00078 
00079 msgs::ErrorStatus StepPlan::updateStep(const msgs::Step& step)
00080 {
00081   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00082   return _updateStep(step);
00083 }
00084 
00085 bool StepPlan::getStep(msgs::Step& step, const msgs::StepPlan& step_plan, unsigned int step_index)
00086 {
00087   for (std::vector<msgs::Step>::const_iterator itr = step_plan.steps.begin(); itr != step_plan.steps.end(); itr++)
00088   {
00089     const msgs::Step& s = *itr;
00090     if (s.step_index == step_index)
00091     {
00092       step = s;
00093       return true;
00094     }
00095   }
00096   return false;
00097 }
00098 
00099 bool StepPlan::getStepAt(msgs::Step& step, unsigned int position) const
00100 {
00101   boost::shared_lock<boost::shared_mutex> lock(step_plan_mutex);
00102 
00103   if (steps.empty())
00104     return false;
00105   else
00106   {
00107     std::map<unsigned int, msgs::Step>::const_iterator itr = steps.begin();
00108     std::advance(itr, position);
00109     step = itr->second;
00110     return true;
00111   }
00112 }
00113 
00114 bool StepPlan::getfirstStep(msgs::Step& step) const
00115 {
00116   boost::shared_lock<boost::shared_mutex> lock(step_plan_mutex);
00117 
00118   if (steps.empty())
00119     return false;
00120   else
00121   {
00122     step = steps.begin()->second;
00123     return true;
00124   }
00125 }
00126 
00127 bool StepPlan::getLastStep(msgs::Step& step) const
00128 {
00129   boost::shared_lock<boost::shared_mutex> lock(step_plan_mutex);
00130 
00131   if (steps.empty())
00132     return false;
00133   else
00134   {
00135     step = steps.rbegin()->second;
00136     return true;
00137   }
00138 }
00139 
00140 bool StepPlan::popStep(msgs::Step& step)
00141 {
00142   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00143 
00144   if (steps.empty())
00145     return false;
00146   else
00147   {
00148     step = steps.begin()->second;
00149     steps.erase(steps.begin());
00150     return true;
00151   }
00152 }
00153 
00154 bool StepPlan::popStep()
00155 {
00156   msgs::Step step;
00157   popStep(step);
00158 }
00159 
00160 void StepPlan::removeStep(unsigned int step_index)
00161 {
00162   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00163   steps.erase(step_index);
00164 }
00165 
00166 void StepPlan::removeStepAt(unsigned int position)
00167 {
00168   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00169   steps.erase(steps.find(position));
00170 }
00171 
00172 void StepPlan::removeSteps(unsigned int from_step_index, int to_step_index)
00173 {
00174   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00175 
00176   if (steps.empty())
00177     return;
00178 
00179   from_step_index = std::max(from_step_index, steps.begin()->first);
00180   if (from_step_index > to_step_index)
00181     return;
00182 
00183   std::map<unsigned int, msgs::Step>::iterator start_itr = steps.find(from_step_index);
00184   std::map<unsigned int, msgs::Step>::iterator end_itr = to_step_index >= 0 ? steps.find(to_step_index+1) : steps.end();
00185   steps.erase(start_itr, end_itr);
00186 }
00187 
00188 msgs::ErrorStatus StepPlan::appendStepPlan(const msgs::StepPlan& step_plan)
00189 {
00190   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00191 
00192   // check for errors
00193   msgs::ErrorStatus status = isConsistent(step_plan);
00194   if (status.error != msgs::ErrorStatus::NO_ERROR)
00195     return status;
00196   else if (step_plan.steps.empty())
00197     return ErrorStatusWarning(msgs::ErrorStatus::WARN_INVALID_STEP_PLAN, "appendStepPlan", "Got empty plan!");
00198 
00199   if (!_empty())
00200   {
00201     if (step_plan.header.frame_id != header.frame_id)
00202       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "appendStepPlan", "Frame id mismatch of plans: " + header.frame_id + " vs. " + step_plan.header.frame_id);
00203     else if (mode != step_plan.mode)
00204       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "appendStepPlan", "Plans have different modes!");
00205   }
00206   // append plan
00207   return _appendStepPlan(step_plan);
00208 }
00209 
00210 msgs::ErrorStatus StepPlan::updateStepPlan(const msgs::StepPlan& step_plan)
00211 {
00212   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00213 
00214   // check for errors
00215   if (step_plan.steps.empty())
00216     return ErrorStatusWarning(msgs::ErrorStatus::WARN_INVALID_STEP_PLAN, "updateStepPlan", "Got empty plan!");
00217 
00218   if (!_empty())
00219   {
00220     if (step_plan.header.frame_id != header.frame_id)
00221       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "updateStepPlan", "Frame id mismatch of plans: " + header.frame_id + " vs. " + step_plan.header.frame_id);
00222     else if (mode != step_plan.mode)
00223       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "updateStepPlan", "Plans have different modes!");
00224   }
00225 
00226   // update plan
00227   return _updateStepPlan(step_plan);
00228 }
00229 
00230 msgs::ErrorStatus StepPlan::stitchStepPlan(const msgs::StepPlan& step_plan, int step_index)
00231 {
00232   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00233 
00234   // check for errors
00235   msgs::ErrorStatus status = isConsistent(step_plan);
00236   if (status.error != msgs::ErrorStatus::NO_ERROR)
00237     return status;
00238   else if (step_plan.steps.empty())
00239     return ErrorStatusWarning(msgs::ErrorStatus::WARN_INVALID_STEP_PLAN, "stitchStepPlan", "Got empty plan!");
00240 
00241   if (!_empty())
00242   {
00243     if (step_plan.header.frame_id != header.frame_id)
00244       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "stitchStepPlan", "Frame id mismatch of plans: " + header.frame_id + " vs. " + step_plan.header.frame_id);
00245     else if (mode != step_plan.mode)
00246       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "stitchStepPlan", "Plans have different modes!");
00247     else if (step_index > 0 && (steps.empty() || step_index > steps.rbegin()->second.step_index))
00248       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "stitchStepPlan", "Can't stitch as requested step index is not in current step plan!");
00250   }
00251 
00252   // stitch plan
00253   return _stitchStepPlan(step_plan, step_index);
00254 }
00255 
00256 tf::Transform StepPlan::getTransform(const geometry_msgs::Pose& current, const geometry_msgs::Pose& target)
00257 {
00258   tf::Pose ref_current;
00259   tf::poseMsgToTF(current, ref_current);
00260 
00261   tf::Pose ref_target;
00262   tf::poseMsgToTF(target, ref_target);
00263   //ref_target.setRotation(tf::createQuaternionFromYaw(tf::getYaw(ref_target.getRotation()))); /// HACK to clamp everything to flat ground
00264 
00265   // determine transformation
00266   return ref_target * ref_current.inverse();
00267 }
00268 
00269 void StepPlan::transformStepPlan(msgs::StepPlan& step_plan, const tf::Transform& transform)
00270 {
00271   tf::Pose pose;
00272 
00273   // start pose
00274   tf::poseMsgToTF(step_plan.start.left.pose, pose);
00275   pose = transform * pose;
00276   tf::poseTFToMsg(pose, step_plan.start.left.pose);
00277 
00278   tf::poseMsgToTF(step_plan.start.right.pose, pose);
00279   pose = transform * pose;
00280   tf::poseTFToMsg(pose, step_plan.start.right.pose);
00281 
00282   // goal pose
00283   tf::poseMsgToTF(step_plan.goal.left.pose, pose);
00284   pose = transform * pose;
00285   tf::poseTFToMsg(pose, step_plan.goal.left.pose);
00286 
00287   tf::poseMsgToTF(step_plan.goal.right.pose, pose);
00288   pose = transform * pose;
00289   tf::poseTFToMsg(pose, step_plan.goal.right.pose);
00290 
00291   // entire plan
00292   for (std::vector<msgs::Step>::iterator itr = step_plan.steps.begin(); itr != step_plan.steps.end(); itr++)
00293   {
00294     msgs::Step& step = *itr;
00295 
00296     tf::poseMsgToTF(step.foot.pose, pose);
00297     pose = transform * pose;
00298     tf::poseTFToMsg(pose, step.foot.pose);
00299   }
00300 }
00301 
00302 void StepPlan::_clear()
00303 {
00304   header = std_msgs::Header();
00305   start = msgs::Feet();
00306   goal = msgs::Feet();
00307   steps.clear();
00308   mode = 0;
00309   data.clear();
00310 }
00311 
00312 bool StepPlan::_empty() const
00313 {
00314   return steps.empty();
00315 }
00316 
00317 size_t StepPlan::_size() const
00318 {
00319   return steps.size();
00320 }
00321 
00322 bool StepPlan::_hasStep(unsigned int step_index) const
00323 {
00324   return steps.find(step_index) != steps.end();
00325 }
00326 
00327 bool StepPlan::_getStep(msgs::Step& step, unsigned int step_index) const
00328 {
00329   std::map<unsigned int, msgs::Step>::const_iterator itr = steps.find(step_index);
00330   if (itr != steps.end())
00331   {
00332     step = itr->second;
00333     return true;
00334   }
00335   return false;
00336 }
00337 
00338 msgs::ErrorStatus StepPlan::_insertStep(const msgs::Step& step)
00339 {
00340   // check for errors
00341   if (steps.empty())
00342     header = step.header;
00343   else if (step.header.frame_id != header.frame_id)
00344     return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "insertStep", "Frame id mismatch! Plan: " + header.frame_id + " vs. step: " + step.header.frame_id);
00345 
00346   // insert step
00347   steps[step.step_index] = step;
00348 
00349   return msgs::ErrorStatus();
00350 }
00351 
00352 msgs::ErrorStatus StepPlan::_updateStep(const msgs::Step& step)
00353 {
00354   // check for errors
00355   if (steps.empty())
00356     header = step.header;
00357   else if (step.header.frame_id != header.frame_id)
00358     return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "updateStep", "Frame id mismatch! Plan: " + header.frame_id + " vs. step: " + step.header.frame_id);
00359 
00360   // check if step has already existed
00361   bool modified(_hasStep(step.step_index));
00362 
00363   // insert/update step
00364   steps[step.step_index] = step;
00365   steps[step.step_index].modified = modified;
00366 
00367   return msgs::ErrorStatus();
00368 }
00369 
00370 msgs::ErrorStatus StepPlan::_appendStepPlan(const msgs::StepPlan& step_plan)
00371 {
00372   int step_index = 0;
00373 
00374   // init header
00375   if (steps.empty())
00376   {
00377     header = step_plan.header;
00378     mode = step_plan.mode;
00379     data = step_plan.data;
00380   }
00381   else
00382     step_index = steps.rbegin()->second.step_index + 1;
00383 
00384   // append plan
00385   msgs::ErrorStatus status;
00386   for (msgs::Step step : step_plan.steps)
00387   {
00388     step.step_index = step_index++;
00389     status += _insertStep(step);
00390 
00391     if (status.error != msgs::ErrorStatus::NO_ERROR)
00392       break;
00393   }
00394 
00395   return status;
00396 }
00397 
00398 msgs::ErrorStatus StepPlan::_updateStepPlan(const msgs::StepPlan& step_plan)
00399 {
00400   // init header
00401   if (steps.empty())
00402   {
00403     header = step_plan.header;
00404     mode = step_plan.mode;
00405     data = step_plan.data;
00406   }
00407 
00408   // update plan
00409   msgs::ErrorStatus status;
00410   for (std::vector<msgs::Step>::const_iterator itr = step_plan.steps.begin(); itr != step_plan.steps.end(); itr++)
00411   {
00412     status += _updateStep(*itr);
00413 
00414     if (status.error != msgs::ErrorStatus::NO_ERROR)
00415       break;
00416   }
00417 
00418   return status;
00419 }
00420 
00421 msgs::ErrorStatus StepPlan::_stitchStepPlan(const msgs::StepPlan& step_plan, int step_index)
00422 {
00423   // simple case: current step plan is still empty
00424   if (steps.empty())
00425     return _updateStepPlan(step_plan);
00426 
00427   msgs::Step last_current_step;
00428   msgs::Step first_new_step;
00429 
00430   if (step_index == 0)
00431   {
00432     last_current_step = steps.rbegin()->second;
00433     first_new_step = step_plan.steps.front();
00434   }
00435   else
00436   {
00437     if (!_getStep(last_current_step, step_index))
00438       return ErrorStatusError(msgs::ErrorStatus::ERR_INVALID_STEP, "stitchStepPlan", "Current step plan doesn't contain step index " + boost::lexical_cast<std::string>(step_index) + "!");
00439 
00440     if (!getStep(first_new_step, step_plan, step_index))
00441       return ErrorStatusError(msgs::ErrorStatus::ERR_INVALID_STEP, "stitchStepPlan", "Input step plan doesn't contain step index " + boost::lexical_cast<std::string>(step_index) + "!");
00442   }
00443 
00444   // ensure overlapping first step
00445   if (last_current_step.foot.foot_index != first_new_step.foot.foot_index)
00446     return ErrorStatusError(msgs::ErrorStatus::ERR_INCONSISTENT_STEP_PLAN, "stitchStepPlan", std::string("First foot ") +
00447                             (first_new_step.foot.foot_index == msgs::Foot::RIGHT ? std::string("(RIGHT)") : std::string("(LEFT)")) + std::string(" of input step plan doesn't match ") +
00448                             (last_current_step.foot.foot_index == msgs::Foot::RIGHT ? std::string("(RIGHT)") : std::string("(LEFT)")) + std::string(" last foot of current step plan."));
00449 
00450   // transform input plan to be relative to current plan's reference foot pose
00451   msgs::StepPlan step_plan_transformed = step_plan;
00452   step_plan_transformed.steps.clear();
00453   
00454   for (msgs::Step step : step_plan.steps)
00455   {
00456     if (step.step_index >= step_index)
00457       step_plan_transformed.steps.push_back(step);
00458   }
00459   
00460   // determine transformation 'input plan first step' -> 'current plan last step'
00461   tf::Transform transform = getTransform(first_new_step.foot.pose, last_current_step.foot.pose);
00462   transformStepPlan(step_plan_transformed, transform);
00463   
00464   // remove remaining tail of old step plan
00465   unsigned int max_step_index = step_plan_transformed.steps.back().step_index;
00466   for (std::map<unsigned int, msgs::Step>::iterator itr = steps.begin(); itr != steps.end();)
00467   {
00468     if (itr->second.step_index > max_step_index)
00469       steps.erase(itr++);
00470     else
00471       itr++;
00472   }
00473 
00474   // update plan using the transformed step plan
00475   return _updateStepPlan(step_plan_transformed);
00476 }
00477 
00478 msgs::ErrorStatus StepPlan::fromMsg(const msgs::StepPlan& step_plan)
00479 {
00480   clear();
00481   msgs::ErrorStatus status = isConsistent(step_plan);
00482 
00483   // check for errors
00484   if (status.error != msgs::ErrorStatus::NO_ERROR)
00485     return status;
00486 
00487   boost::unique_lock<boost::shared_mutex> lock(step_plan_mutex);
00488 
00489   // convert step plan
00490   header = step_plan.header;
00491 
00492   start = step_plan.start;
00493   goal = step_plan.goal;
00494 
00495   mode = step_plan.mode;
00496   data = step_plan.data;
00497 
00498   steps.clear();
00499 
00500   for (unsigned int i = 0u; i < step_plan.steps.size(); i++)
00501   {
00502     status += _insertStep(step_plan.steps[i]);
00503 
00504     if (status.error != msgs::ErrorStatus::NO_ERROR)
00505       break;
00506   }
00507 
00508   return status;
00509 }
00510 
00511 msgs::ErrorStatus StepPlan::toMsg(msgs::StepPlan& step_plan) const
00512 {
00513   boost::shared_lock<boost::shared_mutex> lock(step_plan_mutex);
00514 
00515   msgs::ErrorStatus status;
00516 
00517   // convert step plan
00518   step_plan.header = header;
00519 
00520   step_plan.start = start;
00521   step_plan.goal = goal;
00522 
00523   step_plan.steps.clear();
00524   for (std::map<unsigned int, msgs::Step>::const_iterator itr = steps.begin(); itr != steps.end(); itr++)
00525     step_plan.steps.push_back(itr->second);
00526 
00527   step_plan.mode = mode;
00528   step_plan.data = data;
00529 
00530   status += isConsistent(step_plan);
00531 
00532   return status;
00533 }
00534 }


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