52 StaticLayer::StaticLayer() : dsrv_(NULL) {}
67 std::string map_topic;
68 nh.param(
"map_topic", map_topic, std::string(
"map"));
75 int temp_lethal_threshold, temp_unknown_cost_value;
76 nh.param(
"lethal_cost_threshold", temp_lethal_threshold,
int(100));
77 nh.param(
"unknown_cost_value", temp_unknown_cost_value,
int(-1));
118 dsrv_ =
new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
119 dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
121 dsrv_->setCallback(cb);
166 unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
168 ROS_DEBUG(
"Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
176 master->
getOriginX() != new_map->info.origin.position.x ||
177 master->
getOriginY() != new_map->info.origin.position.y))
180 ROS_INFO(
"Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
182 new_map->info.origin.position.y,
187 origin_x_ != new_map->info.origin.position.x ||
188 origin_y_ != new_map->info.origin.position.y)
191 ROS_INFO(
"Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
192 resizeMap(size_x, size_y, new_map->info.resolution,
193 new_map->info.origin.position.x, new_map->info.origin.position.y);
196 unsigned int index = 0;
199 for (
unsigned int i = 0; i < size_y; ++i)
201 for (
unsigned int j = 0; j < size_x; ++j)
203 unsigned char value = new_map->data[index];
220 ROS_INFO(
"Shutting down the map subscriber. first_map_only flag is on");
228 for (
unsigned int y = 0;
y < update->height ;
y++)
230 unsigned int index_base = (update->y +
y) *
size_x_;
231 for (
unsigned int x = 0;
x < update->width ;
x++)
233 unsigned int index = index_base +
x + update->x;
269 double* max_x,
double* max_y)
282 *min_x = std::min(wx, *min_x);
283 *min_y = std::min(wy, *min_y);
286 *max_x = std::max(wx, *max_x);
287 *max_y = std::max(wy, *max_y);
325 for (
unsigned int i = min_i; i < max_i; ++i)
327 for (
unsigned int j = min_j; j < max_j; ++j)
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to up...
LayeredCostmap * layered_costmap_
virtual void activate()
Restart publishers if they've been stopped.
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization...
virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds().
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
virtual void deactivate()
Stop publishers.
std::string getGlobalFrameID() const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
dynamic_reconfigure::Server< costmap_2d::GenericPluginConfig > * dsrv_
double getOriginX() const
Accessor for the x origin of the costmap.
unsigned char lethal_threshold_
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
TFSIMD_FORCE_INLINE const tfScalar & y() const
static const unsigned char FREE_SPACE
bool first_map_only_
Store the first static map and reuse it on reinitializing.
void useExtraBounds(double *min_x, double *min_y, double *max_x, double *max_y)
bool subscribe_to_updates_
frame that map is located in
tf::TransformListener * tf_
TFSIMD_FORCE_INLINE const tfScalar & x() const
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
std::string global_frame_
The global frame for the costmap.
void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
std::string getTopic() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
void updateWithTrueOverwrite(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
TFSIMD_FORCE_INLINE const tfScalar & x() const
double getOriginY() const
Accessor for the y origin of the costmap.
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
void incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map)
Callback to update the costmap's map from the map_server.
unsigned char interpretValue(unsigned char value)
bool enabled_
Currently this var is managed by subclasses. TODO: make this managed by this class and/or container c...
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr &update)
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
void updateWithMax(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
A 2D costmap provides a mapping between points in the world and their associated "costs".
double getResolution() const
Accessor for the resolution of the costmap.
bool track_unknown_space_
ROSCPP_DECL void spinOnce()
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Convert from world coordinates to map coordinates.
ros::Subscriber map_update_sub_
unsigned char unknown_cost_value_
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.