Go to the documentation of this file.
52 global_frame_(global_frame),
53 rolling_window_(rolling_window),
65 circumscribed_radius_(1.0),
66 inscribed_radius_(0.1)
83 double origin_y,
bool size_locked)
91 (*plugin)->matchSize();
118 if(!(*plugin)->isEnabled())
120 double prev_minx =
minx_;
121 double prev_miny =
miny_;
122 double prev_maxx =
maxx_;
123 double prev_maxy =
maxy_;
127 ROS_WARN_THROTTLE(1.0,
"Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
128 "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
129 prev_minx, prev_miny, prev_maxx , prev_maxy,
131 (*plugin)->getName().c_str());
139 x0 = std::max(0, x0);
141 y0 = std::max(0, y0);
144 ROS_DEBUG(
"Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
146 if (xn < x0 || yn < y0)
153 if((*plugin)->isEnabled())
154 (*plugin)->updateCosts(
costmap_, x0, y0, xn, yn);
171 if((*plugin)->isEnabled())
185 (*plugin)->onFootprintChanged();
double getSizeInMetersX() const
Accessor for the x size of the costmap in meters.
double getSizeInMetersY() const
Accessor for the y size of the costmap in meters.
#define ROS_WARN_THROTTLE(period,...)
void setFootprint(const std::vector< geometry_msgs::Point > &footprint_spec)
Updates the stored footprint, updates the circumscribed and inscribed radii, and calls onFootprintCha...
static const unsigned char NO_INFORMATION
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
void calculateMinAndMaxDistances(const std::vector< geometry_msgs::Point > &footprint, double &min_dist, double &max_dist)
Calculate the extreme distances for the footprint.
std::vector< geometry_msgs::Point > footprint_
double circumscribed_radius_
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
Constructor for a costmap.
std::vector< boost::shared_ptr< Layer > > plugins_
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
void worldToMapEnforceBounds(double wx, double wy, int &mx, int &my) const
Convert from world coordinates to map coordinates, constraining results to legal bounds.
void updateMap(double robot_x, double robot_y, double robot_yaw)
Update the underlying costmap with new data. If you want to update the map outside of the update loop...
~LayeredCostmap()
Destructor.
void setDefaultValue(unsigned char c)
virtual void updateOrigin(double new_origin_x, double new_origin_y)
Move the origin of the costmap to a new location.... keeping data when it can.
static const unsigned char FREE_SPACE
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:17