Go to the documentation of this file.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 #ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
00039 #define COSTMAP_2D_LAYERED_COSTMAP_H_
00040
00041 #include <costmap_2d/cost_values.h>
00042 #include <costmap_2d/layer.h>
00043 #include <costmap_2d/costmap_2d.h>
00044 #include <vector>
00045 #include <string>
00046
00047 namespace costmap_2d
00048 {
00049 class Layer;
00050
00055 class LayeredCostmap
00056 {
00057 public:
00061 LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
00062
00066 ~LayeredCostmap();
00067
00072 void updateMap(double robot_x, double robot_y, double robot_yaw);
00073
00074 std::string getGlobalFrameID() const
00075 {
00076 return global_frame_;
00077 }
00078
00079 void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
00080 bool size_locked = false);
00081
00082 void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy)
00083 {
00084 minx = minx_;
00085 miny = miny_;
00086 maxx = maxx_;
00087 maxy = maxy_;
00088 }
00089
00090 bool isCurrent();
00091
00092 Costmap2D* getCostmap()
00093 {
00094 return &costmap_;
00095 }
00096
00097 bool isRolling()
00098 {
00099 return rolling_window_;
00100 }
00101
00102 bool isTrackingUnknown()
00103 {
00104 return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
00105 }
00106
00107 std::vector<boost::shared_ptr<Layer> >* getPlugins()
00108 {
00109 return &plugins_;
00110 }
00111
00112 void addPlugin(boost::shared_ptr<Layer> plugin)
00113 {
00114 plugins_.push_back(plugin);
00115 }
00116
00117 bool isSizeLocked()
00118 {
00119 return size_locked_;
00120 }
00121
00122 void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn)
00123 {
00124 *x0 = bx0_;
00125 *xn = bxn_;
00126 *y0 = by0_;
00127 *yn = byn_;
00128 }
00129
00130 bool isInitialized()
00131 {
00132 return initialized_;
00133 }
00134
00138 void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
00139
00141 const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
00142
00148 double getCircumscribedRadius() { return circumscribed_radius_; }
00149
00155 double getInscribedRadius() { return inscribed_radius_; }
00156
00157 private:
00158 Costmap2D costmap_;
00159 std::string global_frame_;
00160
00161 bool rolling_window_;
00162
00163 bool current_;
00164 double minx_, miny_, maxx_, maxy_;
00165 unsigned int bx0_, bxn_, by0_, byn_;
00166
00167 std::vector<boost::shared_ptr<Layer> > plugins_;
00168
00169 bool initialized_;
00170 bool size_locked_;
00171 double circumscribed_radius_, inscribed_radius_;
00172 std::vector<geometry_msgs::Point> footprint_;
00173 };
00174
00175 }
00176
00177 #endif // COSTMAP_2D_LAYERED_COSTMAP_H_
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15