40 #include "nav2_costmap_2d/static_layer.hpp"
45 #include "pluginlib/class_list_macros.hpp"
46 #include "tf2/convert.hpp"
47 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
48 #include "nav2_ros_common/validate_messages.hpp"
49 #include "nav2_util/robot_utils.hpp"
55 using nav2_costmap_2d::NO_INFORMATION;
56 using nav2_costmap_2d::LETHAL_OBSTACLE;
57 using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
58 using nav2_costmap_2d::FREE_SPACE;
59 using rcl_interfaces::msg::ParameterType;
65 : map_buffer_(nullptr)
81 if (map_subscribe_transient_local_) {
87 "Subscribing to the map topic (%s) with %s durability",
89 map_subscribe_transient_local_ ?
"transient local" :
"volatile");
91 auto node = node_.lock();
93 throw std::runtime_error{
"Failed to lock node"};
96 map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
101 if (subscribe_to_updates_) {
102 RCLCPP_INFO(logger_,
"Subscribing to updates");
103 map_update_sub_ = node->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
104 map_topic_ +
"_updates",
112 auto node = node_.lock();
114 post_set_params_handler_ = node->add_post_set_parameters_callback(
117 this, std::placeholders::_1));
118 on_set_params_handler_ = node->add_on_set_parameters_callback(
121 this, std::placeholders::_1));
127 auto node = node_.lock();
128 if (post_set_params_handler_ && node) {
129 node->remove_post_set_parameters_callback(post_set_params_handler_.get());
131 post_set_params_handler_.reset();
132 if (on_set_params_handler_ && node) {
133 node->remove_on_set_parameters_callback(on_set_params_handler_.get());
135 on_set_params_handler_.reset();
148 int temp_lethal_threshold = 0;
150 auto node = node_.lock();
152 throw std::runtime_error{
"Failed to lock node"};
155 enabled_ = node->declare_or_get_parameter(name_ +
"." +
"enabled",
true);
156 resize_master_ = node->declare_or_get_parameter(name_ +
"." +
"resize_master",
true);
157 subscribe_to_updates_ = node->declare_or_get_parameter(
158 name_ +
"." +
"subscribe_to_updates",
false);
159 footprint_clearing_enabled_ = node->declare_or_get_parameter(
160 name_ +
"." +
"footprint_clearing_enabled",
false);
161 restore_cleared_footprint_ = node->declare_or_get_parameter(
162 name_ +
"." +
"restore_cleared_footprint",
true);
163 map_topic_ = node->declare_or_get_parameter(
164 name_ +
"." +
"map_topic", std::string(
"map"));
166 map_subscribe_transient_local_ = node->declare_or_get_parameter(
167 name_ +
"." +
"map_subscribe_transient_local",
true);
169 name_ +
"." +
"transform_staleness_threshold", 0.0);
170 node->get_parameter(
"track_unknown_space", track_unknown_space_);
171 node->get_parameter(
"use_maximum", use_maximum_);
172 node->get_parameter(
"lethal_cost_threshold", temp_lethal_threshold);
173 node->get_parameter(
"inscribed_obstacle_cost_value", inscribed_obstacle_cost_value_);
174 node->get_parameter(
"unknown_cost_value", unknown_cost_value_);
175 node->get_parameter(
"trinary_costmap", trinary_costmap_);
178 lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
179 map_received_ =
false;
180 map_received_in_update_bounds_ =
false;
186 RCLCPP_DEBUG(logger_,
"StaticLayer: Process map");
188 unsigned int size_x = new_map.info.width;
189 unsigned int size_y = new_map.info.height;
193 "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y,
194 new_map.info.resolution);
208 "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
209 new_map.info.resolution);
211 double fmod_x = std::fmod(new_map.info.origin.position.x, new_map.info.resolution);
212 double fmod_y = std::fmod(new_map.info.origin.position.y, new_map.info.resolution);
214 if (std::abs(fmod_x) > EPSILON || std::abs(fmod_y) > EPSILON) {
217 "StaticLayer: Costmap origin coordinates are not perfectly aligned with the resolution. "
218 "This may cause misalignment aliasing between rolling and non-rolling costmaps.\n"
219 "Map origin: (%.f, %.f) | Resolution: %.f",
220 new_map.info.origin.position.x, new_map.info.origin.position.y,
221 new_map.info.resolution);
225 size_x, size_y, new_map.info.resolution,
226 new_map.info.origin.position.x,
227 new_map.info.origin.position.y,
229 }
else if (size_x_ != size_x || size_y_ != size_y ||
230 !
isEqual(resolution_, new_map.info.resolution, EPSILON) ||
231 !
isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) ||
232 !
isEqual(origin_y_, new_map.info.origin.position.y, EPSILON))
237 "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
238 new_map.info.resolution);
239 if (!layered_costmap_->
isRolling() && !resize_master_ && size_x_ > 0 && size_y_ > 0) {
242 origin_x_, origin_y_,
243 origin_x_ + size_x_ * resolution_, origin_y_ + size_y_ * resolution_);
246 size_x, size_y, new_map.info.resolution,
247 new_map.info.origin.position.x, new_map.info.origin.position.y);
250 unsigned int index = 0;
253 std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
256 for (
unsigned int i = 0; i < size_y; ++i) {
257 for (
unsigned int j = 0; j < size_x; ++j) {
258 unsigned char value = new_map.data[index];
264 map_frame_ = new_map.header.frame_id;
293 return !layered_costmap_->
isRolling() && resize_master_;
300 if (track_unknown_space_ && value == unknown_cost_value_) {
301 return NO_INFORMATION;
302 }
else if (!track_unknown_space_ && value == unknown_cost_value_) {
304 }
else if (value == inscribed_obstacle_cost_value_) {
305 return INSCRIBED_INFLATED_OBSTACLE;
306 }
else if (value >= lethal_threshold_) {
307 return LETHAL_OBSTACLE;
308 }
else if (trinary_costmap_) {
312 double scale =
static_cast<double>(value) / lethal_threshold_;
313 return scale * LETHAL_OBSTACLE;
319 if (!nav2::validateMsg(*new_map)) {
320 RCLCPP_ERROR(logger_,
"Received map message is malformed. Rejecting.");
323 if (!layered_costmap_->
isRolling() && !resize_master_ &&
327 RCLCPP_ERROR_THROTTLE(
328 logger_, *clock_, 10000,
329 "StaticLayer: Map in frame %s ignored: with resize_master false on a non-rolling costmap "
330 "the map must be in the costmap global frame (%s)",
334 if (!map_received_) {
336 map_received_ =
true;
339 std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
340 map_buffer_ = new_map;
347 if (!nav2::validateMsg(*update)) {
348 RCLCPP_ERROR(logger_,
"Received map update is malformed. Rejecting.");
352 std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
353 if (update->y <
static_cast<int32_t
>(y_) ||
354 y_ + height_ < update->y + update->height ||
355 update->x <
static_cast<int32_t
>(x_) ||
356 x_ + width_ < update->x + update->width)
360 "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
361 "Static layer origin: %d, %d bounds: %d X %d\n"
362 "Update origin: %d, %d bounds: %d X %d",
363 x_, y_, width_, height_, update->x, update->y, update->width,
368 if (update->header.frame_id != map_frame_) {
371 "StaticLayer: Map update ignored. Current map is in frame %s "
372 "but update was in frame %s",
373 map_frame_.c_str(), update->header.frame_id.c_str());
378 for (
unsigned int y = 0; y < update->height; y++) {
379 unsigned int index_base = (update->y + y) * size_x_;
380 for (
unsigned int x = 0; x < update->width; x++) {
381 unsigned int index = index_base + x + update->x;
392 double robot_x,
double robot_y,
double robot_yaw,
double * min_x,
397 if (!map_received_) {
398 map_received_in_update_bounds_ =
false;
401 map_received_in_update_bounds_ =
true;
403 std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
408 map_buffer_ =
nullptr;
417 useExtraBounds(min_x, min_y, max_x, max_y);
429 *min_x = std::min(robot_x - half_w, *min_x);
430 *min_y = std::min(robot_y - half_h, *min_y);
431 *max_x = std::max(robot_x + half_w, *max_x);
432 *max_y = std::max(robot_y + half_h, *max_y);
437 *min_x = std::min(wx, *min_x);
438 *min_y = std::min(wy, *min_y);
440 mapToWorld(x_ + width_, y_ + height_, wx, wy);
441 *max_x = std::max(wx, *max_x);
442 *max_y = std::max(wy, *max_y);
447 updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
452 double robot_x,
double robot_y,
double robot_yaw,
453 double * min_x,
double * min_y,
457 if (!footprint_clearing_enabled_) {
return;}
461 for (
unsigned int i = 0; i < transformed_footprint_.size(); i++) {
462 touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
469 int min_i,
int min_j,
int max_i,
int max_j)
471 std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
475 if (!map_received_in_update_bounds_) {
476 static int count = 0;
479 RCLCPP_WARN(logger_,
"Can't update static costmap layer, no map received");
486 tf2::Transform tf2_transform = tf2::Transform::getIdentity();
488 geometry_msgs::msg::TransformStamped transform;
489 if (!nav2_util::lookupTransformWithStalenessCheck(
493 RCLCPP_ERROR(logger_,
"StaticLayer: transform lookup failed or returned stale data");
498 tf2::fromMsg(transform.transform, tf2_transform);
501 std::vector<MapLocation> map_region_to_restore;
502 if (footprint_clearing_enabled_) {
504 std::vector<geometry_msgs::msg::Point> footprint_in_map_frame = transformed_footprint_;
506 for (
auto & point : footprint_in_map_frame) {
507 const tf2::Vector3 p = tf2_transform * tf2::Vector3(point.x, point.y, 0);
512 map_region_to_restore.reserve(100);
520 updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
522 updateWithMax(master_grid, min_i, min_j, max_i, max_j);
530 for (
int i = min_i; i < max_i; ++i) {
531 for (
int j = min_j; j < max_j; ++j) {
535 tf2::Vector3 p(wx, wy, 0);
536 p = tf2_transform * p;
539 const unsigned char cost =
getCost(mx, my);
541 master_grid.
setCost(i, j, cost);
542 }
else if (cost != NO_INFORMATION) {
544 const unsigned char old_cost = master_grid.
getCost(i, j);
545 if (old_cost == NO_INFORMATION || cost > old_cost) {
546 master_grid.
setCost(i, j, cost);
554 if (footprint_clearing_enabled_ && restore_cleared_footprint_) {
570 return std::abs(a - b) < epsilon;
574 const std::vector<rclcpp::Parameter> & parameters)
576 rcl_interfaces::msg::SetParametersResult result;
577 result.successful =
true;
578 for (
const auto & parameter : parameters) {
579 const auto & param_type = parameter.get_type();
580 const auto & param_name = parameter.get_name();
581 if (param_name.find(name_ +
".") != 0) {
585 if (param_name == name_ +
"." +
"map_subscribe_transient_local" ||
586 param_name == name_ +
"." +
"map_topic" ||
587 param_name == name_ +
"." +
"subscribe_to_updates" ||
588 param_name == name_ +
"." +
"resize_master")
591 logger_,
"%s is not a dynamic parameter "
592 "cannot be changed while running. Rejecting parameter update.", param_name.c_str());
593 }
else if (param_type == ParameterType::PARAMETER_BOOL &&
594 param_name == name_ +
"." +
"restore_cleared_footprint")
596 if (!footprint_clearing_enabled_) {
598 logger_,
"restore_cleared_footprint cannot be used "
599 "when footprint_clearing_enabled is False. Rejecting parameter update.");
600 result.successful =
false;
609 const std::vector<rclcpp::Parameter> & parameters)
611 std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
613 for (
const auto & parameter : parameters) {
614 const auto & param_type = parameter.get_type();
615 const auto & param_name = parameter.get_name();
616 if (param_name.find(name_ +
".") != 0) {
620 if (param_type == ParameterType::PARAMETER_BOOL) {
621 if (param_name == name_ +
"." +
"enabled" && enabled_ != parameter.as_bool()) {
622 enabled_ = parameter.as_bool();
629 }
else if (param_name == name_ +
"." +
"footprint_clearing_enabled") {
630 footprint_clearing_enabled_ = parameter.as_bool();
631 }
else if (param_name == name_ +
"." +
"restore_cleared_footprint") {
632 restore_cleared_footprint_ = parameter.as_bool();
634 }
else if (param_type == ParameterType::PARAMETER_DOUBLE &&
635 param_name == name_ +
"." +
"transform_staleness_threshold")
A QoS profile for latched, reliable topics with a history of 10 messages.
A QoS profile for standard reliable topics with a history of 10 messages.
A 2D costmap provides a mapping between points in the world and their associated "costs".
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)
Resize the costmap.
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Convert from world coordinates to map coordinates.
double getResolution() const
Accessor for the resolution of the costmap.
double getSizeInMetersY() const
Accessor for the y size of the costmap in meters.
double getSizeInMetersX() const
Accessor for the x size of the costmap in meters.
void setMapRegionOccupiedByPolygon(const std::vector< MapLocation > &polygon_map_region, unsigned char new_cost_value)
Sets the given map region to desired value.
void restoreMapRegionOccupiedByPolygon(const std::vector< MapLocation > &polygon_map_region)
Restores the corresponding map region using given map region.
bool getMapRegionOccupiedByPolygon(const std::vector< geometry_msgs::msg::Point > &polygon, std::vector< MapLocation > &polygon_map_region)
Gets the map region occupied by polygon.
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
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.
double getOriginX() const
Accessor for the x origin of the costmap.
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
void addExtraBounds(double mx0, double my0, double mx1, double my1)
void touch(double x, double y, double *min_x, double *min_y, double *max_x, double *max_y)
Abstract class for layered costmap plugin implementations.
std::string joinWithParentNamespace(const std::string &topic)
void setCurrent(bool current)
Set whether the data in the layer is up to date.
const std::vector< geometry_msgs::msg::Point > & getFootprint() const
Convenience function for layered_costmap_->getFootprint().
bool isRolling()
If this costmap is rolling or not.
Costmap2D * getCostmap()
Get the costmap pointer to the master costmap.
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
Resize the map to a new size, resolution, or origin.
bool isSizeLocked()
Get if the size of the costmap is locked.
Takes in a map generated from SLAM to add costs to costmap.
void getParameters()
Get parameters of layer.
bool usesMasterCostmapSize() const
Whether this layer's grid is the master's grid (non-rolling, resize_master true). Otherwise the map k...
virtual ~StaticLayer()
Static Layer destructor.
virtual void deactivate()
Deactivate this layer.
bool has_updated_data_
frame that map is located in
void updateParametersCallback(const std::vector< rclcpp::Parameter > ¶meters)
Apply parameter updates after validation This callback is executed when parameters have been successf...
unsigned char interpretValue(unsigned char value)
Interpret the value in the static map given on the topic to convert into costs for the costmap to uti...
StaticLayer()
Static Layer constructor.
virtual void matchSize()
Match the size of the master costmap.
virtual void onInitialize()
Initialization process of layer on startup.
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
Clear costmap layer info below the robot's footprint.
virtual void activate()
Activate this layer.
void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
Callback to update the costmap's map from the map_server (or SLAM) with an update in a particular are...
bool isEqual(double a, double b, double epsilon)
Check if two double values are equal within a given epsilon.
double transform_staleness_threshold_
Maximum map-frame transform age in seconds; zero disables the check.
std::string global_frame_
The global frame for the costmap.
void processMap(const nav_msgs::msg::OccupancyGrid &new_map)
Process a new map coming from a topic.
virtual void updateCosts(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
Update the costs in the master costmap in the window.
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(const std::vector< rclcpp::Parameter > ¶meters)
Validate incoming parameter updates before applying them. This callback is triggered when one or more...
void incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &new_map)
Callback to update the costmap's map from the map_server.
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
Update the bounds of the master costmap by this layer's update dimensions.
virtual void reset()
Reset this costmap.
void transformFootprint(double x, double y, double theta, const std::vector< geometry_msgs::msg::Point > &footprint_spec, std::vector< geometry_msgs::msg::Point > &oriented_footprint)
Given a pose and base footprint, build the oriented footprint of the robot (list of Points)