step_cost_heuristic.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_default_plugins/heuristics/step_cost_heuristic.h>
00002 
00003 
00004 
00005 namespace vigir_footstep_planning
00006 {
00007 StepCostHeuristic::StepCostHeuristic()
00008   : HeuristicPlugin("step_cost_heuristic")
00009 {
00010 }
00011 
00012 bool StepCostHeuristic::loadParams(const vigir_generic_params::ParameterSet& params)
00013 {
00014   if (!HeuristicPlugin::loadParams(params))
00015     return false;
00016 
00017   params.getParam("const_step_cost_estimator/step_cost", step_cost_, 0.1);
00018   params.getParam("diff_angle_cost", diff_angle_cost_);
00019   params.getParam("max_step_dist/x", max_step_dist_x_inv_);
00020   max_step_dist_x_inv_ = 1.0/max_step_dist_x_inv_;
00021   params.getParam("max_step_dist/y", max_step_dist_y_inv_);
00022   max_step_dist_y_inv_ = 1.0/max_step_dist_y_inv_;
00023   return true;
00024 }
00025 
00026 double StepCostHeuristic::getHeuristicValue(const State& from, const State& to, const State& /*start*/, const State& /*goal*/) const
00027 {
00028   if (from == to)
00029     return 0.0;
00030 
00031   // expected steps
00032   tf::Transform dstep;
00033   getDeltaStep(from.getPose(), to.getPose(), dstep);
00034   double expected_steps_x = std::abs(dstep.getOrigin().x()) * max_step_dist_x_inv_;
00035   double expected_steps_y = std::abs(dstep.getOrigin().y()) * max_step_dist_y_inv_;
00036   double expected_steps = std::ceil(std::max(expected_steps_x, expected_steps_y));
00037 
00038   double diff_angle = 0.0;
00039   if (diff_angle_cost_ > 0.0)
00040     diff_angle = std::abs(angles::shortest_angular_distance(to.getYaw(), from.getYaw()));
00041 
00042 //  ROS_WARN("-------------------------------");
00043 //  ROS_INFO("x: %f %f %f", step.getOrigin().x(), ivMaxStepDistX, std::abs(step.getOrigin().x()) / ivMaxStepDistX);
00044 //  ROS_INFO("y: %f %f %f", step.getOrigin().y(), ivMaxStepDistY, std::abs(step.getOrigin().y()) / ivMaxStepDistY);
00045 //  ROS_INFO("steps: %f, dist: %f, cost: %f", expected_steps, dist, (dist + expected_steps * ivStepCost + diff_angle * ivDiffAngleCost));
00046 
00047   return expected_steps * step_cost_ + diff_angle * diff_angle_cost_;
00048 }
00049 }
00050 
00051 #include <pluginlib/class_list_macros.h>
00052 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::StepCostHeuristic, vigir_footstep_planning::HeuristicPlugin)


vigir_footstep_planning_default_plugins
Author(s): Alexander Stumpf
autogenerated on Fri Apr 7 2017 02:59:40