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


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Tue Oct 15 2013 10:06:51