00001 #include <vigir_footstep_planning_default_plugins/step_cost_estimators/map_step_cost_estimator.h>
00002
00003
00004
00005 namespace vigir_footstep_planning
00006 {
00007 MapStepCostEstimator::MapStepCostEstimator()
00008 : StepCostEstimatorPlugin("map_step_cost_estimator")
00009 {
00010 }
00011
00012 bool MapStepCostEstimator::loadParams(const vigir_generic_params::ParameterSet& params)
00013 {
00014 if (!StepCostEstimatorPlugin::loadParams(params))
00015 return false;
00016
00017 std::string filename;
00018 params.getParam("map_step_cost_estimator/file", filename);
00019
00020 loadFromFile(filename);
00021
00022 return true;
00023 }
00024
00025 bool MapStepCostEstimator::getCost(const State& left_foot, const State& right_foot, const State& swing_foot, double& cost, double& cost_multiplier, double& risk, double& risk_multiplier) const
00026 {
00027 cost = 0.0;
00028 cost_multiplier = 1.0;
00029 risk = 0.0;
00030 risk_multiplier = 1.0;
00031
00032 const State& stand_foot = swing_foot.getLeg() == LEFT ? right_foot : left_foot;
00033 double foot_dist = euclidean_distance(stand_foot.getX(), stand_foot.getY(), swing_foot.getX(), swing_foot.getY());
00034
00035 const State& swing_foot_before = swing_foot.getLeg() == LEFT ? left_foot : right_foot;
00036 double dist = 0.5*euclidean_distance(swing_foot_before.getX(), swing_foot_before.getY(), swing_foot.getX(), swing_foot.getY());
00037
00038 if (dist > 0.3 || foot_dist > 0.45)
00039 {
00040 risk = 1.0;
00041 }
00042 else
00043 {
00044 StepCostKey key(left_foot, right_foot, swing_foot, cell_size, angle_bin_size);
00045 boost::unordered_map<StepCostKey, std::pair<double, double> >::const_iterator iter = cost_map.find(key);
00046
00047 if (iter == cost_map.end())
00048 {
00049 risk = 1.0;
00050 }
00051 else
00052 {
00053 risk = iter->second.second <= 0.25 ? abs(iter->second.first) : 1.0;
00054 }
00055 }
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 cost = dist + 2*risk*risk;
00066 return true;
00067 }
00068
00069 void MapStepCostEstimator::insert(const std::vector<double> &key, const std::pair<double, double> &entry)
00070 {
00071 cost_map[StepCostKey(key, cell_size, angle_bin_size)] = entry;
00072 }
00073
00074 void MapStepCostEstimator::loadFromFile(const std::string &filename)
00075 {
00076
00077 std::ifstream file;
00078 file.open(filename.c_str(), std::ios::in | std::ios::binary);
00079
00080 if (!file.is_open())
00081 {
00082 ROS_ERROR("Could not read from file %s", filename.c_str());
00083 return;
00084 }
00085
00086
00087
00088
00089 read<float>(file, min_x);
00090 read<float>(file, max_x);
00091 read<float>(file, min_y);
00092 read<float>(file, max_y);
00093 read<float>(file, min_yaw);
00094 read<float>(file, max_yaw);
00095
00096
00097 float temp;
00098 read<float>(file, temp);
00099 cell_size = temp;
00100 read<uint32_t>(file, num_angle_bin);
00101 angle_bin_size = 2.0*M_PI/(double)num_angle_bin;
00102
00103
00104 uint32_t dim_size, num_elements;
00105 read<uint32_t>(file, num_elements);
00106 read<uint32_t>(file, dim_size);
00107 dim_size -= 2;
00108
00109
00110 std::vector<double> key;
00111 key.resize(dim_size);
00112 std::pair<double, double> entry;
00113
00114 cost_map.clear();
00115 for (unsigned int i = 0; i < num_elements; i++)
00116 {
00117
00118 for (unsigned int d = 0; d < dim_size; d++)
00119 {
00120 float k;
00121 read<float>(file, k);
00122 key[d] = k;
00123 }
00124
00125
00126 float c, s;
00127 read<float>(file, c);
00128 read<float>(file, s);
00129 entry.first = c;
00130 entry.second = s;
00131
00132
00133 insert(key, entry);
00134 }
00135
00136 ROS_INFO("Boundaries (x/y/yaw): %f %f %f %f %f %f", min_x, max_x, min_y, max_y, min_yaw, max_yaw);
00137 ROS_INFO("Resolution: long: %f rot: %u %f", cell_size, num_angle_bin, angle_bin_size);
00138 ROS_INFO("Loaded step cost map: %u dimensions and %lu entries", dim_size, cost_map.size());
00139
00140
00141 file.close();
00142 }
00143 }
00144
00145 #include <pluginlib/class_list_macros.h>
00146 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::MapStepCostEstimator, vigir_footstep_planning::StepCostEstimatorPlugin)