00001 #include <vigir_footstep_planning_default_plugins/heuristics/euclidean_heuristic.h> 00002 00003 00004 00005 namespace vigir_footstep_planning 00006 { 00007 EuclideanHeuristic::EuclideanHeuristic() 00008 : HeuristicPlugin("euclidean_heuristic") 00009 {} 00010 00011 double EuclideanHeuristic::getHeuristicValue(const State& from, const State& to, const State& /*start*/, const State& /*goal*/) const 00012 { 00013 if (from == to) 00014 return 0.0; 00015 00016 return euclidean_distance(from.getX(), from.getY(), from.getZ(), to.getX(), to.getY(), to.getZ()); 00017 } 00018 } 00019 00020 #include <pluginlib/class_list_macros.h> 00021 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::EuclideanHeuristic, vigir_footstep_planning::HeuristicPlugin)