FootstepNavigation.cpp
Go to the documentation of this file.
00001 /*
00002  * A footstep planner for humanoid robots
00003  *
00004  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00005  * http://www.ros.org/wiki/footstep_planner
00006  *
00007  *
00008  * This program is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, version 3.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 #include <footstep_planner/FootstepNavigation.h>
00022 
00023 
00024 namespace footstep_planner
00025 {
00026 FootstepNavigation::FootstepNavigation()
00027 : ivIdFootRight("/r_sole"),
00028   ivIdFootLeft("/l_sole"),
00029   ivIdMapFrame("map"),
00030   ivExecutingFootsteps(false),
00031   ivFootstepsExecution("footsteps_execution", true),
00032   ivExecutionShift(2),
00033   ivControlStepIdx(-1),
00034   ivResetStepIdx(0)
00035 {
00036   // private NodeHandle for parameters and private messages (debug / info)
00037   ros::NodeHandle nh_private("~");
00038   ros::NodeHandle nh_public;
00039 
00040   // service
00041   ivFootstepSrv =
00042     nh_public.serviceClient<humanoid_nav_msgs::StepTargetService>(
00043       "footstep_srv");
00044   ivClipFootstepSrv =
00045     nh_public.serviceClient<humanoid_nav_msgs::ClipFootstep>(
00046       "clip_footstep_srv");
00047 
00048   // subscribers
00049   ivGridMapSub =
00050     nh_public.subscribe<nav_msgs::OccupancyGrid>(
00051       "map", 1, &FootstepNavigation::mapCallback, this);
00052   ivGoalPoseSub =
00053     nh_public.subscribe<geometry_msgs::PoseStamped>(
00054       "goal", 1, &FootstepNavigation::goalPoseCallback, this);
00055 
00056   // read parameters from config file:
00057   nh_private.param("rfoot_frame_id", ivIdFootRight, ivIdFootRight);
00058   nh_private.param("lfoot_frame_id", ivIdFootLeft, ivIdFootLeft);
00059 
00060   nh_private.param("accuracy/footstep/x", ivAccuracyX, 0.01);
00061   nh_private.param("accuracy/footstep/y", ivAccuracyY, 0.01);
00062   nh_private.param("accuracy/footstep/theta", ivAccuracyTheta, 0.1);
00063 
00064   nh_private.param("accuracy/cell_size", ivCellSize, 0.005);
00065   nh_private.param("accuracy/num_angle_bins", ivNumAngleBins, 128);
00066 
00067   nh_private.param("forward_search", ivForwardSearch, false);
00068 
00069   nh_private.param("feedback_frequency", ivFeedbackFrequency, 5.0);
00070   nh_private.param("safe_execution", ivSafeExecution, true);
00071 
00072   nh_private.param("foot/max/step/x", ivMaxStepX, 0.07);
00073   nh_private.param("foot/max/step/y", ivMaxStepY, 0.15);
00074   nh_private.param("foot/max/step/theta", ivMaxStepTheta, 0.3);
00075   nh_private.param("foot/max/inverse/step/x", ivMaxInvStepX, -0.03);
00076   nh_private.param("foot/max/inverse/step/y", ivMaxInvStepY, 0.09);
00077   nh_private.param("foot/max/inverse/step/theta", ivMaxInvStepTheta, -0.01);
00078 
00079   // step range
00080   XmlRpc::XmlRpcValue step_range_x;
00081   XmlRpc::XmlRpcValue step_range_y;
00082   nh_private.getParam("step_range/x", step_range_x);
00083   nh_private.getParam("step_range/y", step_range_y);
00084   if (step_range_x.getType() != XmlRpc::XmlRpcValue::TypeArray)
00085     ROS_ERROR("Error reading footsteps/x from config file.");
00086   if (step_range_y.getType() != XmlRpc::XmlRpcValue::TypeArray)
00087     ROS_ERROR("Error reading footsteps/y from config file.");
00088   if (step_range_x.size() != step_range_y.size())
00089   {
00090     ROS_ERROR("Step range points have different size. Exit!");
00091     exit(2);
00092   }
00093   // create step range
00094   ivStepRange.clear();
00095   ivStepRange.reserve(step_range_x.size());
00096   double x, y;
00097   for (int i = 0; i < step_range_x.size(); ++i)
00098   {
00099     x = (double)step_range_x[i];
00100     y = (double)step_range_y[i];
00101     ivStepRange.push_back(std::pair<double, double>(x, y));
00102   }
00103   // insert first point again at the end!
00104   ivStepRange.push_back(ivStepRange[0]);
00105 }
00106 
00107 
00108 FootstepNavigation::~FootstepNavigation()
00109 {}
00110 
00111 
00112 bool
00113 FootstepNavigation::plan()
00114 {
00115   if (!updateStart())
00116   {
00117     ROS_ERROR("Start pose not accessible!");
00118     return false;
00119   }
00120 
00121   if (ivPlanner.plan())
00122   {
00123     startExecution();
00124     return true;
00125   }
00126   // path planning unsuccessful
00127   return false;
00128 }
00129 
00130 
00131 bool
00132 FootstepNavigation::replan()
00133 {
00134   if (!updateStart())
00135   {
00136     ROS_ERROR("Start pose not accessible!");
00137     return false;
00138   }
00139 
00140   bool path_existed = ivPlanner.pathExists();
00141 
00142   // calculate path by replanning (if no planning information exists
00143   // this call is equal to ivPlanner.plan())
00144   if (ivPlanner.replan())
00145   {
00146     startExecution();
00147     return true;
00148   }
00149   else if (path_existed)
00150   {
00151     ROS_INFO("Replanning unsuccessful. Reseting previous planning "
00152              "information.");
00153     if (ivPlanner.plan())
00154     {
00155       startExecution();
00156       return true;
00157     }
00158   }
00159   // path planning unsuccessful
00160   ivExecutingFootsteps = false;
00161   return false;
00162 }
00163 
00164 
00165 void
00166 FootstepNavigation::startExecution()
00167 {
00168   if (ivSafeExecution)
00169   {
00170     ivFootstepExecutionPtr.reset(
00171       new boost::thread(
00172         boost::bind(&FootstepNavigation::executeFootsteps, this)));
00173   }
00174   else
00175   {
00176     // ALTERNATIVE:
00177     executeFootstepsFast();
00178   }
00179 }
00180 
00181 
00182 void
00183 FootstepNavigation::executeFootsteps()
00184 {
00185   if (ivPlanner.getPathSize() <= 1)
00186     return;
00187 
00188   // lock this thread
00189   ivExecutingFootsteps = true;
00190 
00191   ROS_INFO("Start walking towards the goal.");
00192 
00193   humanoid_nav_msgs::StepTarget step;
00194   humanoid_nav_msgs::StepTargetService step_srv;
00195 
00196   tf::Transform from;
00197   std::string support_foot_id;
00198 
00199   // calculate and perform relative footsteps until goal is reached
00200   state_iter_t to_planned = ivPlanner.getPathBegin();
00201   if (to_planned == ivPlanner.getPathEnd())
00202   {
00203     ROS_ERROR("No plan available. Return.");
00204     return;
00205   }
00206 
00207   const State* from_planned = to_planned.base();
00208   to_planned++;
00209   while (to_planned != ivPlanner.getPathEnd())
00210   {
00211     try
00212     {
00213       boost::this_thread::interruption_point();
00214     }
00215     catch (const boost::thread_interrupted&)
00216     {
00217       // leave this thread
00218       return;
00219     }
00220 
00221     if (from_planned->getLeg() == RIGHT)
00222       support_foot_id = ivIdFootRight;
00223     else // support_foot = LLEG
00224       support_foot_id = ivIdFootLeft;
00225 
00226     // try to get real placement of the support foot
00227     if (getFootTransform(support_foot_id, ivIdMapFrame, ros::Time::now(),
00228                          ros::Duration(0.5), &from))
00229     {
00230       // calculate relative step and check if it can be performed
00231       if (getFootstep(from, *from_planned, *to_planned, &step))
00232       {
00233         step_srv.request.step = step;
00234         ivFootstepSrv.call(step_srv);
00235       }
00236       // ..if it cannot be performed initialize replanning
00237       else
00238       {
00239         ROS_INFO("Footstep cannot be performed. Replanning necessary.");
00240 
00241         replan();
00242         // leave the thread
00243         return;
00244       }
00245     }
00246     else
00247     {
00248       // if the support foot could not be received wait and try again
00249       ros::Duration(0.5).sleep();
00250       continue;
00251     }
00252 
00253     from_planned = to_planned.base();
00254     to_planned++;
00255   }
00256   ROS_INFO("Succeeded walking to the goal.\n");
00257 
00258   // free the lock
00259   ivExecutingFootsteps = false;
00260 }
00261 
00262 
00263 void
00264 FootstepNavigation::executeFootstepsFast()
00265 {
00266   if (ivPlanner.getPathSize() <= 1)
00267         return;
00268 
00269   // lock the planning and execution process
00270   ivExecutingFootsteps = true;
00271 
00272   // make sure the action client is connected to the action server
00273   ivFootstepsExecution.waitForServer();
00274 
00275   humanoid_nav_msgs::ExecFootstepsGoal goal;
00276   State support_leg;
00277   if (ivPlanner.getPathBegin()->getLeg() == RIGHT)
00278     support_leg = ivPlanner.getStartFootRight();
00279   else // leg == LEFT
00280     support_leg = ivPlanner.getStartFootLeft();
00281   if (getFootstepsFromPath(support_leg, 1, goal.footsteps))
00282   {
00283     goal.feedback_frequency = ivFeedbackFrequency;
00284     ivControlStepIdx = 0;
00285     ivResetStepIdx = 0;
00286 
00287     // start the execution via action; _1, _2 are place holders for
00288     // function arguments (see boost doc)
00289     ivFootstepsExecution.sendGoal(
00290       goal,
00291       boost::bind(&FootstepNavigation::doneCallback, this, _1, _2),
00292       boost::bind(&FootstepNavigation::activeCallback, this),
00293       boost::bind(&FootstepNavigation::feedbackCallback, this, _1));
00294   }
00295   else
00296   {
00297     // free the lock
00298     ivExecutingFootsteps = false;
00299 
00300     replan();
00301   }
00302 }
00303 
00304 
00305 void
00306 FootstepNavigation::activeCallback()
00307 {
00308         // lock the execution
00309         ivExecutingFootsteps = true;
00310 
00311         ROS_INFO("Start walking towards the goal.");
00312 }
00313 
00314 
00315 void
00316 FootstepNavigation::doneCallback(
00317         const actionlib::SimpleClientGoalState& state,
00318         const humanoid_nav_msgs::ExecFootstepsResultConstPtr& result)
00319 {
00320         if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00321                 ROS_INFO("Succeeded walking to the goal.");
00322         else if (state == actionlib::SimpleClientGoalState::PREEMPTED)
00323                 ROS_INFO("Preempted walking to the goal.");
00324         // TODO: distinct between further states??
00325         else
00326                 ROS_INFO("Failed walking to the goal.");
00327 
00328         // free the lock
00329         ivExecutingFootsteps = false;
00330 }
00331 
00332 
00333 void
00334 FootstepNavigation::feedbackCallback(
00335         const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr& fb)
00336 {
00337         int executed_steps_idx = fb->executed_footsteps.size() - ivExecutionShift;
00338         // make sure at least one step has been performed
00339         if (executed_steps_idx < 0)
00340     return;
00341         // if the currently executed footstep equals the currently observed one
00342         // everything is ok
00343         if (executed_steps_idx == ivControlStepIdx)
00344     return;
00345 
00346         // get planned foot placement
00347   const State& planned = *(ivPlanner.getPathBegin() + ivControlStepIdx + 1 +
00348                            ivResetStepIdx);
00349   // get executed foot placement
00350   tf::Transform executed_tf;
00351   std::string foot_id;
00352   if (planned.getLeg() == RIGHT)
00353     foot_id = ivIdFootRight;
00354   else
00355     foot_id = ivIdFootLeft;
00356 
00357   if (!getFootTransform(foot_id, ivIdMapFrame, ros::Time::now(),
00358                                     ros::Duration(0.5), &executed_tf))
00359   {
00360     State executed(executed_tf.getOrigin().x(), executed_tf.getOrigin().y(),
00361                    tf::getYaw(executed_tf.getRotation()), planned.getLeg());
00362     ivFootstepsExecution.cancelGoal();
00363     humanoid_nav_msgs::ExecFootstepsGoal goal;
00364     // try to reach the calculated path
00365     if (getFootstepsFromPath(executed, executed_steps_idx + ivResetStepIdx,
00366                              goal.footsteps))
00367     {
00368       goal.feedback_frequency = ivFeedbackFrequency;
00369       // adjust the internal counters
00370       ivResetStepIdx += ivControlStepIdx + 1;
00371       ivControlStepIdx = 0;
00372 
00373       // restart the footstep execution
00374       ivFootstepsExecution.sendGoal(
00375         goal,
00376         boost::bind(&FootstepNavigation::doneCallback, this, _1, _2),
00377         boost::bind(&FootstepNavigation::activeCallback, this),
00378         boost::bind(&FootstepNavigation::feedbackCallback, this, _1));
00379     }
00380     // the previously calculated path cannot be reached so we have plan
00381     // a new path
00382     else
00383     {
00384       replan();
00385     }
00386   }
00387 
00388   State executed(executed_tf.getOrigin().x(), executed_tf.getOrigin().y(),
00389                  tf::getYaw(executed_tf.getRotation()), planned.getLeg());
00390 
00391   // check if the currently executed footstep is no longer observed (i.e.
00392   // the robot no longer follows its calculated path)
00393   if (executed_steps_idx >= ivControlStepIdx + 2)
00394         {
00395     ivFootstepsExecution.cancelGoal();
00396 
00397     ROS_DEBUG("Footstep execution incorrect.");
00398 
00399     humanoid_nav_msgs::ExecFootstepsGoal goal;
00400     // try to reach the calculated path
00401     if (getFootstepsFromPath(executed, executed_steps_idx + ivResetStepIdx,
00402                              goal.footsteps))
00403     {
00404       ROS_INFO("Try to reach calculated path.");
00405 
00406       goal.feedback_frequency = ivFeedbackFrequency;
00407       // adjust the internal counters
00408       ivResetStepIdx += ivControlStepIdx + 1;
00409       ivControlStepIdx = 0;
00410 
00411       // restart the footstep execution
00412       ivFootstepsExecution.sendGoal(
00413         goal,
00414         boost::bind(&FootstepNavigation::doneCallback, this, _1, _2),
00415         boost::bind(&FootstepNavigation::activeCallback, this),
00416         boost::bind(&FootstepNavigation::feedbackCallback, this, _1));
00417     }
00418     // the previously calculated path cannot be reached so we have plan
00419     // a new path
00420     else
00421     {
00422       replan();
00423     }
00424 
00425     return;
00426         }
00427     // check the currently observed footstep
00428         else
00429         {
00430     ROS_DEBUG("planned (%f, %f, %f, %i) vs. executed (%f, %f, %f, %i)",
00431               planned.getX(), planned.getY(), planned.getTheta(),
00432               planned.getLeg(),
00433               executed.getX(), executed.getY(), executed.getTheta(),
00434               executed.getLeg());
00435 
00436     // adjust the internal step counters if the footstep has been
00437     // performed correctly; otherwise check in the next iteration if
00438     // the step really has been incorrect
00439     if (performanceValid(planned, executed))
00440       ivControlStepIdx++;
00441     else
00442       ROS_DEBUG("Invalid step. Wait next step update before declaring"
00443                 " step incorrect.");
00444         }
00445 }
00446 
00447 
00448 void
00449 FootstepNavigation::goalPoseCallback(
00450   const geometry_msgs::PoseStampedConstPtr& goal_pose)
00451 {
00452   // check if the execution is locked
00453   if (ivExecutingFootsteps)
00454   {
00455     ROS_INFO("Already performing a navigation task. Wait until it is "
00456              "finished.");
00457     return;
00458   }
00459 
00460   if (setGoal(goal_pose))
00461   {
00462     // this check enforces a planning from scratch if necessary (dependent on
00463     // planning direction)
00464           if (ivForwardSearch)
00465             replan();
00466           else
00467             plan();
00468   }
00469 }
00470 
00471 
00472 void
00473 FootstepNavigation::mapCallback(
00474   const nav_msgs::OccupancyGridConstPtr& occupancy_map)
00475 {
00476   // stop execution if an execution was performed
00477   if (ivExecutingFootsteps)
00478   {
00479     if (ivSafeExecution)
00480     {
00481       // interrupt the thread and wait until it has finished its execution
00482           ivFootstepExecutionPtr->interrupt();
00483       ivFootstepExecutionPtr->join();
00484     }
00485     else
00486     {
00487                 ivFootstepsExecution.cancelAllGoals();
00488     }
00489   }
00490 
00491   gridmap_2d::GridMap2DPtr map(new gridmap_2d::GridMap2D(occupancy_map));
00492   ivIdMapFrame = map->getFrameID();
00493 
00494   // updates the map and starts replanning if necessary
00495   if (ivPlanner.updateMap(map))
00496   {
00497     replan();
00498   }
00499 }
00500 
00501 
00502 bool
00503 FootstepNavigation::setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose)
00504 {
00505   return setGoal(goal_pose->pose.position.x,
00506                  goal_pose->pose.position.y,
00507                  tf::getYaw(goal_pose->pose.orientation));
00508 }
00509 
00510 
00511 bool
00512 FootstepNavigation::setGoal(float x, float y, float theta)
00513 {
00514         return ivPlanner.setGoal(x, y, theta);
00515 }
00516 
00517 
00518 bool
00519 FootstepNavigation::updateStart()
00520 {
00521   ros::Duration(0.5).sleep();
00522 
00523   tf::Transform foot_left, foot_right;
00524   {
00525     // get real placement of the feet
00526           if (!getFootTransform(ivIdFootLeft, ivIdMapFrame, ros::Time::now(),
00527                                 ros::Duration(0.5), &foot_left))
00528           {
00529             if (ivPlanner.pathExists())
00530             {
00531               ivExecutingFootsteps = false;
00532             }
00533             return false;
00534           }
00535     if (!getFootTransform(ivIdFootRight, ivIdMapFrame, ros::Time::now(),
00536                                   ros::Duration(0.5), &foot_right))
00537     {
00538       if (ivPlanner.pathExists())
00539       {
00540         ivExecutingFootsteps = false;
00541       }
00542       return false;
00543     }
00544   }
00545   State left(foot_left.getOrigin().x(), foot_left.getOrigin().y(),
00546                        tf::getYaw(foot_left.getRotation()), LEFT);
00547   State right(foot_right.getOrigin().x(), foot_right.getOrigin().y(),
00548               tf::getYaw(foot_right.getRotation()), RIGHT);
00549 
00550   ROS_INFO("Robot standing at (%f, %f, %f, %i) (%f, %f, %f, %i).",
00551                        left.getX(), left.getY(), left.getTheta(), left.getLeg(),
00552                        right.getX(), right.getY(), right.getTheta(), right.getLeg());
00553 
00554   return ivPlanner.setStart(left, right);
00555 }
00556 
00557 
00558 bool
00559 FootstepNavigation::getFootstep(const tf::Pose& from,
00560                                 const State& from_planned,
00561                                             const State& to,
00562                                             humanoid_nav_msgs::StepTarget* footstep)
00563 {
00564   // get footstep to reach 'to' from 'from'
00565   tf::Transform step = from.inverse() *
00566                        tf::Pose(tf::createQuaternionFromYaw(to.getTheta()),
00567                                 tf::Point(to.getX(), to.getY(), 0.0));
00568 
00569   // set the footstep
00570   footstep->pose.x = step.getOrigin().x();
00571   footstep->pose.y = step.getOrigin().y();
00572   footstep->pose.theta = tf::getYaw(step.getRotation());
00573   if (to.getLeg() == LEFT)
00574     footstep->leg = humanoid_nav_msgs::StepTarget::left;
00575   else // to.leg == RIGHT
00576     footstep->leg = humanoid_nav_msgs::StepTarget::right;
00577 
00578 
00579   /* check if the footstep can be performed by the NAO robot ******************/
00580 
00581   // check if the step lies within the executable range
00582   if (performable(*footstep))
00583   {
00584     return true;
00585   }
00586   else
00587   {
00588     // check if there is only a minor divergence between the current support
00589         // foot and the foot placement used during the plannig task: in such a case
00590         // perform the step that has been used during the planning
00591     float step_diff_x = fabs(from.getOrigin().x() - from_planned.getX());
00592     float step_diff_y = fabs(from.getOrigin().y() - from_planned.getY());
00593     float step_diff_theta = fabs(
00594         angles::shortest_angular_distance(
00595             tf::getYaw(from.getRotation()), from_planned.getTheta()));
00596     if (step_diff_x < ivAccuracyX && step_diff_y < ivAccuracyY &&
00597         step_diff_theta < ivAccuracyTheta)
00598     {
00599           step = tf::Pose(tf::createQuaternionFromYaw(from_planned.getTheta()),
00600                           tf::Point(from_planned.getX(), from_planned.getY(), 0.0)
00601                           ).inverse() *
00602                      tf::Pose(tf::createQuaternionFromYaw(to.getTheta()),
00603                                       tf::Point(to.getX(), to.getY(), 0.0));
00604 
00605           footstep->pose.x = step.getOrigin().x();
00606           footstep->pose.y = step.getOrigin().y();
00607           footstep->pose.theta = tf::getYaw(step.getRotation());
00608 
00609           return true;
00610     }
00611 
00612     return false;
00613   }
00614 
00615 //  // ALTERNATIVE: clip the footstep into the executable range; if nothing was
00616 //  // clipped: perform; if too much was clipped: do not perform
00617 //  humanoid_nav_msgs::ClipFootstep footstep_clip;
00618 //  footstep_clip.request.step = footstep;
00619 //  ivClipFootstepSrv.call(footstep_clip);
00620 //
00621 //  if (performanceValid(footstep_clip))
00622 //  {
00623 //      footstep.pose.x = footstep_clip.response.step.pose.x;
00624 //      footstep.pose.y = footstep_clip.response.step.pose.y;
00625 //      footstep.pose.theta = footstep_clip.response.step.pose.theta;
00626 //      return true;
00627 //  }
00628 //  else
00629 //  {
00630 //    return false;
00631 //  }
00632 }
00633 
00634 
00635 bool
00636 FootstepNavigation::getFootstepsFromPath(
00637   const State& current_support_leg, int starting_step_num,
00638   std::vector<humanoid_nav_msgs::StepTarget>& footsteps)
00639 {
00640   humanoid_nav_msgs::StepTarget footstep;
00641 
00642   state_iter_t to_planned = ivPlanner.getPathBegin() + starting_step_num - 1;
00643   tf::Pose last(tf::createQuaternionFromYaw(current_support_leg.getTheta()),
00644                 tf::Point(current_support_leg.getX(), current_support_leg.getY(),
00645                           0.0));
00646   const State* from_planned = to_planned.base();
00647   to_planned++;
00648   for (; to_planned != ivPlanner.getPathEnd(); to_planned++)
00649   {
00650     if (getFootstep(last, *from_planned, *to_planned, &footstep))
00651     {
00652       footsteps.push_back(footstep);
00653     }
00654     else
00655     {
00656       ROS_ERROR("Calculated path cannot be performed!");
00657       return false;
00658     }
00659 
00660     last = tf::Pose(tf::createQuaternionFromYaw(to_planned->getTheta()),
00661                     tf::Point(to_planned->getX(), to_planned->getY(), 0.0));
00662     from_planned = to_planned.base();
00663   }
00664 
00665   return true;
00666 }
00667 
00668 
00669 bool
00670 FootstepNavigation::getFootTransform(const std::string& foot_id,
00671                                      const std::string& world_frame_id,
00672                                      const ros::Time& time,
00673                                      const ros::Duration& waiting_time,
00674                                      tf::Transform* foot)
00675 {
00676   tf::StampedTransform stamped_foot_transform;
00677   try
00678   {
00679     ivTransformListener.waitForTransform(world_frame_id, foot_id, time,
00680                                          waiting_time);
00681     ivTransformListener.lookupTransform(world_frame_id, foot_id, time,
00682                                         stamped_foot_transform);
00683   }
00684   catch (const tf::TransformException& e)
00685   {
00686     ROS_WARN("Failed to obtain FootTransform from tf (%s)", e.what());
00687     return false;
00688   }
00689 
00690   foot->setOrigin(stamped_foot_transform.getOrigin());
00691   foot->setRotation(stamped_foot_transform.getRotation());
00692 
00693   return true;
00694 }
00695 
00696 
00697 bool
00698 FootstepNavigation::performanceValid(float a_x, float a_y, float a_theta,
00699                                      float b_x, float b_y, float b_theta)
00700 {
00701   return (fabs(a_x - b_x) < ivAccuracyX &&
00702           fabs(a_y - b_y) < ivAccuracyY &&
00703           fabs(angles::shortest_angular_distance(a_theta, b_theta)) <
00704             ivAccuracyTheta);
00705 }
00706 
00707 
00708 bool
00709 FootstepNavigation::performanceValid(
00710   const humanoid_nav_msgs::ClipFootstep& step)
00711 {
00712   return performanceValid(step.request.step.pose.x,
00713                           step.request.step.pose.y,
00714                           step.request.step.pose.theta,
00715                           step.response.step.pose.x,
00716                           step.response.step.pose.y,
00717                           step.response.step.pose.theta);
00718 }
00719 
00720 
00721 bool
00722 FootstepNavigation::performanceValid(const State& planned,
00723                                      const State& executed)
00724 {
00725   return performanceValid(
00726     planned.getX(), planned.getY(), planned.getTheta(),
00727     executed.getX(), executed.getY(), executed.getTheta());
00728 }
00729 
00730 
00731 bool
00732 FootstepNavigation::performable(const humanoid_nav_msgs::StepTarget& footstep)
00733 {
00734   float step_x = footstep.pose.x;
00735   float step_y = footstep.pose.y;
00736   float step_theta = footstep.pose.theta;
00737 
00738   if (footstep.leg == humanoid_nav_msgs::StepTarget::right)
00739   {
00740     step_y = -step_y;
00741     step_theta = -step_theta;
00742   }
00743 
00744   if (step_x + FLOAT_CMP_THR > ivMaxStepX ||
00745       step_x - FLOAT_CMP_THR < ivMaxInvStepX)
00746     return false;
00747   if (step_y + FLOAT_CMP_THR > ivMaxStepY ||
00748       step_y - FLOAT_CMP_THR < ivMaxInvStepY)
00749     return false;
00750   if (step_theta + FLOAT_CMP_THR > ivMaxStepTheta ||
00751       step_theta - FLOAT_CMP_THR < ivMaxInvStepTheta)
00752     return false;
00753 
00754   return performable(step_x, step_y);
00755 }
00756 
00757 
00758 bool
00759 FootstepNavigation::performable(float step_x, float step_y)
00760 {
00761   int cn = 0;
00762 
00763   // loop through all ivStepRange of the polygon
00764   for(unsigned int i = 0; i < ivStepRange.size() - 1; ++i)
00765   {
00766     if ((ivStepRange[i].second <= step_y &&
00767          ivStepRange[i + 1].second > step_y) ||
00768         (ivStepRange[i].second >= step_y &&
00769          ivStepRange[i + 1].second < step_y))
00770     {
00771       float vt = (float)(step_y - ivStepRange[i].second) /
00772         (ivStepRange[i + 1].second - ivStepRange[i].second);
00773       if (step_x <
00774           ivStepRange[i].first + vt *
00775             (ivStepRange[i + 1].first - ivStepRange[i].first))
00776       {
00777         ++cn;
00778       }
00779     }
00780   }
00781   return cn & 1;
00782 }
00783 }


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Sat Jun 8 2019 20:21:05