00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <costmap_2d/layered_costmap.h>
00039 #include <costmap_2d/footprint.h>
00040 #include <cstdio>
00041 #include <string>
00042 #include <algorithm>
00043 #include <vector>
00044
00045 using std::vector;
00046
00047 namespace costmap_2d
00048 {
00049
00050 LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) :
00051 costmap_(),
00052 global_frame_(global_frame),
00053 rolling_window_(rolling_window),
00054 current_(false),
00055 minx_(0.0),
00056 miny_(0.0),
00057 maxx_(0.0),
00058 maxy_(0.0),
00059 bx0_(0),
00060 bxn_(0),
00061 by0_(0),
00062 byn_(0),
00063 initialized_(false),
00064 size_locked_(false),
00065 circumscribed_radius_(1.0),
00066 inscribed_radius_(0.1)
00067 {
00068 if (track_unknown)
00069 costmap_.setDefaultValue(255);
00070 else
00071 costmap_.setDefaultValue(0);
00072 }
00073
00074 LayeredCostmap::~LayeredCostmap()
00075 {
00076 while (plugins_.size() > 0)
00077 {
00078 plugins_.pop_back();
00079 }
00080 }
00081
00082 void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
00083 double origin_y, bool size_locked)
00084 {
00085 boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
00086 size_locked_ = size_locked;
00087 costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
00088 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
00089 ++plugin)
00090 {
00091 (*plugin)->matchSize();
00092 }
00093 }
00094
00095 void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
00096 {
00097
00098
00099 boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
00100
00101
00102 if (rolling_window_)
00103 {
00104 double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
00105 double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
00106 costmap_.updateOrigin(new_origin_x, new_origin_y);
00107 }
00108
00109 if (plugins_.size() == 0)
00110 return;
00111
00112 minx_ = miny_ = 1e30;
00113 maxx_ = maxy_ = -1e30;
00114
00115 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
00116 ++plugin)
00117 {
00118 double prev_minx = minx_;
00119 double prev_miny = miny_;
00120 double prev_maxx = maxx_;
00121 double prev_maxy = maxy_;
00122 (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
00123 if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
00124 {
00125 ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
00126 "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
00127 prev_minx, prev_miny, prev_maxx , prev_maxy,
00128 minx_, miny_, maxx_ , maxy_,
00129 (*plugin)->getName().c_str());
00130 }
00131 }
00132
00133 int x0, xn, y0, yn;
00134 costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
00135 costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
00136
00137 x0 = std::max(0, x0);
00138 xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
00139 y0 = std::max(0, y0);
00140 yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
00141
00142 ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
00143
00144 if (xn < x0 || yn < y0)
00145 return;
00146
00147 costmap_.resetMap(x0, y0, xn, yn);
00148 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
00149 ++plugin)
00150 {
00151 (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
00152 }
00153
00154 bx0_ = x0;
00155 bxn_ = xn;
00156 by0_ = y0;
00157 byn_ = yn;
00158
00159 initialized_ = true;
00160 }
00161
00162 bool LayeredCostmap::isCurrent()
00163 {
00164 current_ = true;
00165 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
00166 ++plugin)
00167 {
00168 current_ = current_ && (*plugin)->isCurrent();
00169 }
00170 return current_;
00171 }
00172
00173 void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec)
00174 {
00175 footprint_ = footprint_spec;
00176 costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
00177
00178 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
00179 ++plugin)
00180 {
00181 (*plugin)->onFootprintChanged();
00182 }
00183 }
00184
00185 }