00001 #include <vigir_footstep_planning_default_plugins/world_model/foot_grid_map_model.h> 00002 00003 00004 00005 namespace vigir_footstep_planning 00006 { 00007 FootGridMapModel::FootGridMapModel(const std::string& name) 00008 : GridMapModel(name) 00009 { 00010 } 00011 00012 bool FootGridMapModel::initialize(const vigir_generic_params::ParameterSet& params) 00013 { 00014 if (!GridMapModel::initialize(params)) 00015 return false; 00016 00017 // get foot dimensions 00018 getFootSize(nh_, foot_size); 00019 00020 return true; 00021 } 00022 00023 bool FootGridMapModel::isAccessible(const State& s) const 00024 { 00025 boost::shared_lock<boost::shared_mutex> lock(grid_map_shared_mutex_); 00026 00027 if (!occupancy_grid_map_) 00028 { 00029 ROS_ERROR_THROTTLE(10, "[FootGridMapModel] No ground level grid map available yet."); 00030 return true; 00031 } 00032 00033 double x = s.getX(); 00034 double y = s.getY(); 00035 00036 double theta = s.getYaw(); 00037 00038 // collision check for the foot center 00039 return !collision_check(x, y, cos(theta), sin(theta), foot_size.x, foot_size.y); 00040 } 00041 } 00042 00043 #include <pluginlib/class_list_macros.h> 00044 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::FootGridMapModel, vigir_footstep_planning::CollisionCheckPlugin)