grid_map_model.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_default_plugins/world_model/grid_map_model.h>
00002 
00003 
00004 
00005 namespace vigir_footstep_planning
00006 {
00007 GridMapModel::GridMapModel(const std::string& name)
00008   : CollisionCheckGridMapPlugin(name)
00009 {
00010 }
00011 
00012 bool GridMapModel::loadParams(const vigir_generic_params::ParameterSet& params)
00013 {
00014   if (!CollisionCheckGridMapPlugin::loadParams(params))
00015     return false;
00016 
00017   params.getParam("collision_check/collision_check_accuracy", (int&)collision_check_accuracy);
00018   return true;
00019 }
00020 
00021 void GridMapModel::mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_grid_map)
00022 {
00023   CollisionCheckGridMapPlugin::mapCallback(occupancy_grid_map);
00024 
00025   boost::unique_lock<boost::shared_mutex> lock(grid_map_shared_mutex_);
00026   distance_map.setMap(occupancy_grid_map);
00027 }
00028 
00029 bool GridMapModel::collision_check(double x, double y, double cos_theta, double sin_theta, double height, double width) const
00030 {
00031   double d = distance_map.distanceMapAt(x, y);
00032 
00033   bool out_of_bounds = d < 0.0; // if out of bounds => assume no collision
00034 
00035   if (out_of_bounds && collision_check_accuracy != 2)
00036     return false;
00037 
00038   d = std::max(d-distance_map.getResolution(), 0.0);
00039 
00040   double r_o = width*width + height*height;
00041 
00042   if (!out_of_bounds)
00043   {
00044     if (d*d >= 0.25 * r_o)
00045       return false;
00046     else if (collision_check_accuracy == 0)
00047       return false;
00048   }
00049 
00050   double h_half = 0.5 * height;
00051   double w_half = 0.5 * width;
00052   double r_i = std::min(w_half, h_half);
00053 
00054   if (!out_of_bounds)
00055   {
00056     if (d <= r_i)
00057       return true;
00058     else if (collision_check_accuracy == 1)
00059       return true;
00060   }
00061   else if (r_i < distance_map.getResolution())
00062   {
00063     return false;
00064   }
00065 
00066   double h_new;
00067   double w_new;
00068   double delta_x;
00069   double delta_y;
00070   if (width < height)
00071   {
00072     double h_clear = out_of_bounds ? 0.0 : sqrt(d*d - w_half*w_half);
00073     h_new = h_half - h_clear;
00074     w_new = width;
00075     delta_x = h_clear + 0.5 * h_new;
00076     delta_y = 0.0;
00077   }
00078   else // footWidth >= footHeight
00079   {
00080     double w_clear = out_of_bounds ? 0.0 : sqrt(d*d - h_half*h_half);
00081     h_new = height;
00082     w_new = w_half - w_clear;
00083     delta_x = 0.0;
00084     delta_y = w_clear + 0.5 * w_new;
00085   }
00086   double x_shift = cos_theta*delta_x - sin_theta*delta_y;
00087   double y_shift = sin_theta*delta_x + cos_theta*delta_y;
00088 
00089   return (collision_check(x+x_shift, y+y_shift, cos_theta, sin_theta, h_new, w_new) ||
00090           collision_check(x-x_shift, y-y_shift, cos_theta, sin_theta, h_new, w_new));
00091 }
00092 }


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