00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00040 ros::NodeHandle nh_private("~");
00041 ros::NodeHandle nh_public;
00042
00043
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
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
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
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
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
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
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
00146
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
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
00180 executeFootstepsFast();
00181 }
00182 }
00183
00184
00185 void
00186 FootstepNavigation::executeFootsteps()
00187 {
00188 if (ivPlanner.getPathSize() <= 1)
00189 return;
00190
00191
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
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
00221 return;
00222 }
00223
00224 if (from_planned->getLeg() == RIGHT)
00225 support_foot_id = ivIdFootRight;
00226 else
00227 support_foot_id = ivIdFootLeft;
00228
00229
00230 if (getFootTransform(support_foot_id, ivIdMapFrame, ros::Time::now(),
00231 ros::Duration(0.5), &from))
00232 {
00233
00234 if (getFootstep(from, *from_planned, *to_planned, &step))
00235 {
00236 step_srv.request.step = step;
00237 ivFootstepSrv.call(step_srv);
00238 }
00239
00240 else
00241 {
00242 ROS_INFO("Footstep cannot be performed. Replanning necessary.");
00243
00244 replan();
00245
00246 return;
00247 }
00248 }
00249 else
00250 {
00251
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
00262 ivExecutingFootsteps = false;
00263 }
00264
00265
00266 void
00267 FootstepNavigation::executeFootstepsFast()
00268 {
00269 if (ivPlanner.getPathSize() <= 1)
00270 return;
00271
00272
00273 ivExecutingFootsteps = true;
00274
00275
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
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
00291
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
00301 ivExecutingFootsteps = false;
00302
00303 replan();
00304 }
00305 }
00306
00307
00308 void
00309 FootstepNavigation::activeCallback()
00310 {
00311
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
00328 else
00329 ROS_INFO("Failed walking to the goal.");
00330
00331
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
00342 if (executed_steps_idx < 0)
00343 return;
00344
00345
00346 if (executed_steps_idx == ivControlStepIdx)
00347 return;
00348
00349
00350 const State& planned = *(ivPlanner.getPathBegin() + ivControlStepIdx + 1 +
00351 ivResetStepIdx);
00352
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
00368 if (getFootstepsFromPath(executed, executed_steps_idx + ivResetStepIdx,
00369 goal.footsteps))
00370 {
00371 goal.feedback_frequency = ivFeedbackFrequency;
00372
00373 ivResetStepIdx += ivControlStepIdx + 1;
00374 ivControlStepIdx = 0;
00375
00376
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
00384
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
00395
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
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
00411 ivResetStepIdx += ivControlStepIdx + 1;
00412 ivControlStepIdx = 0;
00413
00414
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
00422
00423 else
00424 {
00425 replan();
00426 }
00427
00428 return;
00429 }
00430
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
00440
00441
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
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
00466
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
00480 if (ivExecutingFootsteps)
00481 {
00482 if (ivSafeExecution)
00483 {
00484
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
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
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
00568 tf::Transform step = from.inverse() *
00569 tf::Pose(tf::createQuaternionFromYaw(to.getTheta()),
00570 tf::Point(to.getX(), to.getY(), 0.0));
00571
00572
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
00579 footstep->leg = humanoid_nav_msgs::StepTarget::right;
00580
00581
00582
00583
00584
00585 if (performable(*footstep))
00586 {
00587 return true;
00588 }
00589 else
00590 {
00591
00592
00593
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
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
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
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 }