Go to the documentation of this file.00001 #include <vigir_footstep_planning_default_plugins/world_model/upper_body_grid_map_model.h>
00002
00003
00004
00005 namespace vigir_footstep_planning
00006 {
00007 UpperBodyGridMapModel::UpperBodyGridMapModel(const std::string& name)
00008 : GridMapModel(name)
00009 {
00010 }
00011
00012 bool UpperBodyGridMapModel::initialize(const vigir_generic_params::ParameterSet& params)
00013 {
00014 if (!GridMapModel::initialize(params))
00015 return false;
00016
00017
00018 getUpperBodySize(nh_, upper_body_size);
00019 getUpperBodyOriginShift(nh_, upper_body_origin_shift);
00020
00021 return true;
00022 }
00023
00024 bool UpperBodyGridMapModel::isAccessible(const State& ) const
00025 {
00026
00027 return true;
00028 }
00029
00030 bool UpperBodyGridMapModel::isAccessible(const State& next, const State& current) const
00031 {
00032 boost::shared_lock<boost::shared_mutex> lock(grid_map_shared_mutex_);
00033
00034 if (!occupancy_grid_map_)
00035 {
00036 ROS_ERROR_THROTTLE(10, "[UpperBodyGridMapModel] No body level grid map available yet.");
00037 return true;
00038 }
00039
00040 if (next.getLeg() == current.getLeg())
00041 {
00042 ROS_ERROR_THROTTLE(10, "[UpperBodyGridMapModel] Swing foot can't equal support foot.");
00043 return false;
00044 }
00045
00046 const State& left = next.getLeg() == LEFT ? next : current;
00047 const State& right = next.getLeg() == RIGHT ? next : current;
00048
00049
00050 float x = right.getX() + 0.5 * (left.getX() - right.getX());
00051 float y = right.getY() + 0.5 * (left.getY() - right.getY());
00052
00053 float theta = right.getYaw() + 0.5 * (left.getYaw() - right.getYaw());
00054
00055
00056 float cos_theta = cos(theta);
00057 float sin_theta = sin(theta);
00058 float shift_x = cos_theta * upper_body_origin_shift.x - sin_theta * upper_body_origin_shift.y;
00059 float shift_y = sin_theta * upper_body_origin_shift.x + cos_theta * upper_body_origin_shift.y;
00060
00061
00062 x += shift_x;
00063 y += shift_y;
00064
00065 return !collision_check(x, y, cos_theta, sin_theta, upper_body_size.x, upper_body_size.y);
00066 }
00067 }
00068
00069 #include <pluginlib/class_list_macros.h>
00070 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::UpperBodyGridMapModel, vigir_footstep_planning::CollisionCheckPlugin)