diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 030a85f38cd..950c839796b 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -152,7 +152,8 @@ class AmclNode : public nav2_util::LifecycleNode std::recursive_mutex mutex_; rclcpp::Subscription::ConstSharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING - static std::vector> free_space_indices; + struct Point2D { int32_t x; int32_t y; }; + static std::vector free_space_indices; #endif // Transforms diff --git a/nav2_amcl/include/nav2_amcl/map/map.hpp b/nav2_amcl/include/nav2_amcl/map/map.hpp index c9557efa0b0..3cf6f0d597b 100644 --- a/nav2_amcl/include/nav2_amcl/map/map.hpp +++ b/nav2_amcl/include/nav2_amcl/map/map.hpp @@ -41,20 +41,21 @@ struct _rtk_fig_t; // Limits #define MAP_WIFI_MAX_LEVELS 8 - +// make sure that the sizeof(map_cell_t) == 5 +#pragma pack(push, 1) // Description for a single map cell. typedef struct { // Occupancy state (-1 = free, 0 = unknown, +1 = occ) - int occ_state; + int8_t occ_state; // Distance to the nearest occupied cell - double occ_dist; + float occ_dist; // Wifi levels // int wifi_levels[MAP_WIFI_MAX_LEVELS]; } map_cell_t; - +#pragma pack(pop) // Description for a map typedef struct diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index cdd4c5adbdb..e68625d23b3 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -227,6 +227,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) add_parameter( "first_map_only", rclcpp::ParameterValue(false), "Set this to true, when you want to load a new map published from the map_server"); + + add_parameter( + "freespace_downsampling", rclcpp::ParameterValue( + false), + "If true, downsample the free space used by the Pose Generator. This will save memory in VERY large maps"); } AmclNode::~AmclNode() @@ -404,7 +409,7 @@ AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time las } #if NEW_UNIFORM_SAMPLING -std::vector> AmclNode::free_space_indices; +std::vector AmclNode::free_space_indices; #endif bool @@ -446,10 +451,10 @@ AmclNode::uniformPoseGenerator(void * arg) #if NEW_UNIFORM_SAMPLING unsigned int rand_index = drand48() * free_space_indices.size(); - std::pair free_point = free_space_indices[rand_index]; + AmclNode::Point2D free_point = free_space_indices[rand_index]; pf_vector_t p; - p.v[0] = MAP_WXGX(map, free_point.first); - p.v[1] = MAP_WYGY(map, free_point.second); + p.v[0] = MAP_WXGX(map, free_point.x); + p.v[1] = MAP_WYGY(map, free_point.y); p.v[2] = drand48() * 2 * M_PI - M_PI; #else double min_x, max_x, min_y, max_y; @@ -1106,6 +1111,7 @@ AmclNode::initParameters() get_parameter("always_reset_initial_pose", always_reset_initial_pose_); get_parameter("scan_topic", scan_topic_); get_parameter("map_topic", map_topic_); + get_parameter("freespace_downsampling", freespace_downsampling_); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); @@ -1435,12 +1441,14 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg) void AmclNode::createFreeSpaceVector() { + int delta = freespace_downsampling_ ? 2 : 1; // Index of free space free_space_indices.resize(0); - for (int i = 0; i < map_->size_x; i++) { - for (int j = 0; j < map_->size_y; j++) { + for (int i = 0; i < map_->size_x; i += delta) { + for (int j = 0; j < map_->size_y; j += delta) { if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) { - free_space_indices.push_back(std::make_pair(i, j)); + AmclNode::Point2D point = {i, j}; + free_space_indices.push_back(point); } } }