From ad999d9edee641b45a5e14a497a59fe8604b7fd5 Mon Sep 17 00:00:00 2001 From: "C. Andy Martin" Date: Mon, 11 Mar 2024 16:46:05 -0400 Subject: [PATCH] costmap_3d: query: abstract out the distance cache Abstract out the distance caches into an internal distance cache class. Move all the distance cache logic and methods into that internal class. This change should make no functional difference, as it is simply reoganizing the code. A future change could be to further split the internal classes up into separate classes and files for clarity. This is not yet done as it will take even more reorganization to disentangle the caches from the queries. The main spirit of this commit is to remove all the places where the code was repeating itself by getting all the details of the various distance caches into one DistanceCache class. --- .../include/costmap_3d/costmap_3d_query.h | 516 ++++++++++++--- costmap_3d/src/costmap_3d_query.cpp | 595 +++++------------- 2 files changed, 601 insertions(+), 510 deletions(-) diff --git a/costmap_3d/include/costmap_3d/costmap_3d_query.h b/costmap_3d/include/costmap_3d/costmap_3d_query.h index a0dbf76e8..f8718b636 100644 --- a/costmap_3d/include/costmap_3d/costmap_3d_query.h +++ b/costmap_3d/include/costmap_3d/costmap_3d_query.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -54,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -389,12 +391,6 @@ class Costmap3DQuery const DistanceOptions& opts, DistanceResult* return_result = nullptr); - /** @brief recalculate the distance cache thresholds. - * - * The caller must already hold an exclusive lock on the instance_mutex_. - */ - virtual void calculateCacheDistanceThresholds(); - private: // Common initialization between all constructors void init(); @@ -474,12 +470,19 @@ class Costmap3DQuery class DistanceCacheKey { public: - DistanceCacheKey( + // Distance cache keys aren't valid until they are initialized. + // DistanceCacheKeys are used on the fast-path of the queries, so dynamic + // memory needs to be avoided. The user of the cache then will allocate + // a DistanceCacheKey on their stack and then have the DistanceCache + // initialize it separately. + DistanceCacheKey() = default; + + void initialize( const geometry_msgs::Pose& pose, QueryRegion query_region, - bool query_obstacles, - int bins_per_meter, - int bins_per_rotation) + QueryObstacles query_obstacles, + int bins_per_meter = 0, + int bins_per_rotation = 0) { // If bins_per_meter or bins_per_rotation are zero, the cache is disabled. // The key is allowed to be calculated anyway as an exact key (and this @@ -497,14 +500,6 @@ class Costmap3DQuery query_obstacles_ = query_obstacles; hash_ = hash_value(); } - // Constructor for the "exact" cache where the poses are not binned - DistanceCacheKey(const geometry_msgs::Pose& pose, QueryRegion query_region, bool query_obstacles) - { - binned_pose_ = pose; - query_region_ = query_region; - query_obstacles_ = query_obstacles; - hash_ = hash_value(); - } // Directly store the hash value in a public location for speed. size_t hash_; @@ -543,7 +538,7 @@ class Costmap3DQuery binned_pose_.position.x, binned_pose_.position.y, binned_pose_.position.z, - {.uint = {static_cast(query_region_) << 8 | static_cast(query_obstacles_)}}, + {.uint = (static_cast(query_region_) << 8 | static_cast(query_obstacles_))}, }; uint64_t primes[8] = { 30011, @@ -727,7 +722,7 @@ class Costmap3DQuery // Test if the given distance cache entry is inside the region. // Regions are always inclusive. A voxel on the region boundary is // considered "inside". - bool distanceCacheEntryInside(const DistanceCacheEntry& cache_entry) + bool distanceCacheEntryInside(const DistanceCacheEntry& cache_entry) const { const auto& box = cache_entry.octomap_box; const auto& box_center = cache_entry.octomap_box_center; @@ -762,10 +757,429 @@ class Costmap3DQuery double handleDistanceInteriorCollisions( const DistanceCacheEntry& cache_entry, const geometry_msgs::Pose& pose); - using DistanceCache = - std::unordered_map; - using ExactDistanceCache = + using DistanceCacheMap = std::unordered_map; + template + class DistanceCacheImpl + { + public: + using DistanceFunction = std::function; + + void setDistanceFunction(DistanceFunction distance_function) + { + distance_function_ = distance_function; + } + + void setRobotMeshPoints(const pcl::PointCloud::ConstPtr& robot_mesh_points) + { + if (exact_cache) + { + // exact caches don't need the robot mesh points + return; + } + // Called sparingly, not on normal planning path, so just grab write lock + unique_lock cache_write_lock(mutex_); + robot_mesh_points_ = robot_mesh_points; + calculateDistanceThresholds(); + } + + void setBinSize(unsigned int bins_per_meter, unsigned int bins_per_rotation) + { + if (exact_cache) + { + // exact caches don't need bins + return; + } + bool changed = false; + { + shared_lock cache_read_lock(mutex_); + if (bins_per_meter != bins_per_meter_ || bins_per_rotation != bins_per_rotation_) + { + changed = true; + } + } + if (changed) + { + unique_lock cache_write_lock(mutex_); + if (bins_per_meter != bins_per_meter_ || bins_per_rotation != bins_per_rotation_) + { + cache_.clear(); + bins_per_meter_ = bins_per_meter; + bins_per_rotation_ = bins_per_rotation; + ROS_INFO_STREAM(debug_prefix_ << "distance cache bins per meter: " << bins_per_meter_); + ROS_INFO_STREAM(debug_prefix_ << "distance cache bins per rotation: " << bins_per_rotation); + calculateDistanceThresholds(); + } + } + } + + void setThresholdParameters(bool threshold_two_d_mode, double threshold_factor) + { + if (exact_cache) + { + // exact caches don't need thresholds, just skip as printing them is pointless + return; + } + bool changed = false; + { + shared_lock cache_read_lock(mutex_); + if (threshold_two_d_mode_ != threshold_two_d_mode || threshold_factor_ != threshold_factor) + { + changed = true; + } + } + if (changed) + { + unique_lock cache_write_lock(mutex_); + if (threshold_two_d_mode_ != threshold_two_d_mode || threshold_factor_ != threshold_factor) + { + threshold_two_d_mode_ = threshold_two_d_mode; + threshold_factor_ = threshold_factor; + calculateDistanceThresholds(); + } + } + } + + void calculateDistanceThresholds() + { + if (exact_cache) + { + // exact caches don't need thresholds, just skip as printing them is pointless + return; + } + if (!robot_mesh_points_) + { + ROS_INFO_STREAM(debug_prefix_ << "distance cache: skipping threshold calculation: no robot mesh points"); + return; + } + // The distance thresholds must be greater than the maximum translation + // error plus the maximum rotation error, otherwise the query may not + // return a collision when one is actually present. In a robot with full + // three-dimensional movement this would equate to the length of the + // diagonal of a bin plus the chord length corresponding to the size of a + // rotational bin given the maximum radius of the robot mesh. For + // two-dimensional movement of a robot on a plane aligned with the + // costmap, the threshold is lower at only the diagonal of the bin + // projected into the plane as a square and the chord length given the + // radius of the robot mesh projected into the plane (the robot footprint + // radius). + Eigen::Vector4f origin(0.0, 0.0, 0.0, 0.0), max_pt(0.0, 0.0, 0.0, 0.0); + if (threshold_two_d_mode_) + { + // Create a 2D version of the mesh cloud by truncating the Z value. + pcl::PointCloud mesh_points_2d(*robot_mesh_points_); + for (auto& point : mesh_points_2d) + { + point.z = 0.0; + } + pcl::getMaxDistance(mesh_points_2d, origin, max_pt); + } + else + { + pcl::getMaxDistance(*robot_mesh_points_, origin, max_pt); + } + const Eigen::Vector3f max_pt3 = max_pt.head<3>(); + double mesh_radius = max_pt3.norm(); + ROS_INFO_STREAM(debug_prefix_ << "distance cache mesh radius: " << mesh_radius << "m"); + if (bins_per_meter_ > 0) + { + const double diagonal_factor = threshold_two_d_mode_ ? std::sqrt(2.0) : std::sqrt(3.0); + // Because binPoseAngularDistanceLimit measures the maximum rotational size + // of a bin, it may return up to 2 * M_PI, but the maximum chord length is + // at an angle of M_PI, so do not use an angular error higher than M_PI. + const double max_angular_error = std::min(M_PI, binPoseAngularDistanceLimit( + bins_per_rotation_, threshold_two_d_mode_)); + // Add the diagonal length of positional error plus the chord length of the + // maximum angular error. + threshold_ = diagonal_factor / bins_per_meter_ + + 2.0 * mesh_radius * std::sin(0.5 * max_angular_error); + threshold_ *= threshold_factor_; + ROS_INFO_STREAM(debug_prefix_ << "distance cache threshold: " << threshold_ << "m"); + } + else + { + threshold_ = std::numeric_limits::infinity(); + } + } + + inline void initializeKey( + const geometry_msgs::Pose& pose, + const QueryRegion query_region, + const QueryObstacles query_obstacles, + DistanceCacheKey* key_ptr) + { + // Do not grab the mutex at all, if the caller alters the number of bins + // per pose this key will just bin wrong, which does mean the cache will + // miss, but because a cache is only to improve performance there is no + // error introduced by this. Grabbing the lock is more expensive as it + // holds off other writers to the cache while binning the pose. + key_ptr->initialize(pose, query_region, query_obstacles, bins_per_meter_, bins_per_rotation_); + } + + // First return value is true when cache is hit. + // Second return value is true for a fast hit. + std::pair checkCache( + const DistanceCacheKey& key, + const geometry_msgs::Pose& pose, + DistanceCacheEntry* cache_entry_ptr, + double* distance_ptr, + const RegionsOfInterestAtPose* rois_ptr = nullptr, + bool track_statistics = false, + const std::chrono::high_resolution_clock::time_point* start_time_ptr = nullptr, + std::atomic* fast_hits_since_clear_ptr = nullptr, + std::atomic* fast_hits_since_clear_us_ptr = nullptr, + bool exact_signed_distance = false, + bool directly_use_cache_when_above_threshold = false) + { + // Be sure the entry is cleared in case the caller is reusing the same + // entry, otherwise there will be a false hit. + cache_entry_ptr->clear(); + if (exact_cache || (bins_per_meter_ > 0 && bins_per_rotation_ > 0)) + { + shared_lock cache_read_lock(mutex_); + auto found_entry = cache_.find(key); + if (found_entry != cache_.end()) + { + *cache_entry_ptr = found_entry->second; + } + } + if (*cache_entry_ptr && (!rois_ptr || rois_ptr->distanceCacheEntryInside(*cache_entry_ptr))) + { + // Cache hit, find the distance between the mesh triangle at the new + // pose and the octomap box, and use this as our initial guess in the + // result. This greatly prunes the search tree, yielding a big increase + // in runtime performance. + assert(distance_function_); + double distance = distance_function_(*cache_entry_ptr, pose); + *distance_ptr = distance; + // Fast path. + // Take the fast path on a hit in an exact cache, or + // when not in exact signed distance and there is a collision or + // when directly using the cache above the threshold and the cache + // entry is still valid and above the threshold + if (exact_cache || + (!exact_signed_distance && distance <= 0.0) || + (directly_use_cache_when_above_threshold && + std::isfinite(cache_entry_ptr->distance) && ( + cache_entry_ptr->distance > threshold_ || + distance > threshold_))) + { + if (track_statistics) + { + fast_hits_since_clear_ptr->fetch_add(1, std::memory_order_relaxed); + fast_hits_since_clear_us_ptr->fetch_add( + std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - *start_time_ptr).count(), + std::memory_order_relaxed); + } + return std::make_pair(true, true); + } + return std::make_pair(true, false); + } + return std::make_pair(false, false); + } + + void updateCache(const DistanceCacheKey& key, const DistanceCacheEntry& entry) + { + if (exact_cache || (bins_per_meter_ > 0 && bins_per_rotation_ > 0)) + { + // While it may seem expensive to copy the cache entries into the + // caches, it prevents cache aliasing and avoids dynamic memory. + unique_lock cache_write_lock(mutex_); + cache_[key] = entry; + } + } + + // Delete any distance cache entries that have had their corresponding + // octomap cells removed. It is fine to keep entries in the presence of + // additions, as the entry only defines an upper bound. Because the size of + // the tree is limited, the size of the cache has a natural limit. If this + // limit is ever too large, a separate cache size may need to be set. Also + // invalidate the fast path for any cache entries that are left. This + // method is only useful for caches that are primarily used to limit the + // tree, such as the main distance cache and the milli distance cache. + // Other caches are unlikely to benefit from this and should simply be + // cleared on an update. + void handleCostmapUpdate( + const octomap::OcTree* octree_ptr, + const octomap::OcTree* nonlethal_octree_ptr) + { + unique_lock cache_write_lock(mutex_); + size_t start_size = cache_.size(); + auto it = cache_.begin(); + while (it != cache_.end()) + { + bool erase = true; + Costmap3DIndex index; + unsigned int depth; + const octomap::OcTree* octree_to_query; + if (it->first.getQueryObstacles() == NONLETHAL_ONLY) + { + octree_to_query = nonlethal_octree_ptr; + } + else + { + octree_to_query = octree_ptr; + } + if (it->second.getCostmapIndexAndDepth(*octree_to_query, &index, &depth)) + { + unsigned int found_depth; + auto* node = octree_to_query->search(index, depth, &found_depth); + if (node && + !octree_to_query->nodeHasChildren(node) && + depth == found_depth && + octree_to_query->isNodeOccupied(node)) + { + // The node exists and has no children (so its a leaf and not an + // inner node), is at the correct depth and is considered occupied + // for the proper octree. Keep this entry. + erase = false; + } + } + + if (erase) + { + it = cache_.erase(it); + } + else + { + // While the entry is still good for setting an upper distance bound + // on the octree solver, it is no good for the fast-path. This is + // because the new costmap may have a closer box than previously. + // Therefore invalidate this entry for the fast-path by setting the + // recorded distance to infinity. The fast-path is only taken for + // finite recorded distance. + it->second.distance = std::numeric_limits::infinity(); + ++it; + } + } + ROS_DEBUG_STREAM_NAMED("query_distance_cache", + debug_prefix_ << "distance cache removed " << start_size - cache_.size() << " entries"); + } + + void clear() + { + unique_lock cache_write_lock(mutex_); + cache_.clear(); + } + + void printDistanceCacheDebug() + { + ROS_DEBUG_STREAM_NAMED( + "query_distance_cache", debug_prefix_ << "distance cache: " << + " size: " << cache_.size() << + " bucket count: " << cache_.bucket_count() << + " badness: " << cache_map_badness()); + } + + void setDebugPrefix(const std::string& debug_prefix) + { + debug_prefix_ = debug_prefix; + } + + private: + double cache_map_badness() + { + double ratio = cache_.size(); + ratio /= cache_.bucket_count(); + + double cost = 0.0; + for (auto const& entry : cache_) + cost += cache_.bucket_size(cache_.bucket(entry.first)); + cost /= cache_.size(); + + return std::max(0.0, cost / (1 + ratio) - 1); + } + + shared_mutex mutex_; + std::string debug_prefix_; + // Zero means disabled + unsigned int bins_per_meter_ = 0; + unsigned int bins_per_rotation_ = 0; + /** + * Whether the caller wants 2D mode for distance cache thresholds. + * When in 2D mode, the cache thresholds can be tighter and are calculated + * differently than when not in 2D mode. The default is to run in 3D mode. + * The milli and micro caches may be much more effective in 2D mode for a 2D + * robot, as only the radius of the robot matters if query poses will be for + * 2D navigation. + */ + bool threshold_two_d_mode_ = false; + /** + * Safety factor for distance cache thresholds. + * The minimum value that prevents false-negative collisions is increased by + * this factor. + */ + double threshold_factor_ = 1.05; + /** + * The distance cache allows a very fast path when the nearest obstacle is + * more than a threshold of distance away. This results in a massive speed + * up for cases where the nearest obstacle is further than the threshold_. + * This value is automatically calculated based on the robot mesh, the + * desired mode (2D or 3D) and the threshold_factor_. + */ + double threshold_ = std::numeric_limits::infinity(); + DistanceCacheMap cache_; + DistanceFunction distance_function_; + pcl::PointCloud::ConstPtr robot_mesh_points_; + }; + + using DistanceCache = DistanceCacheImpl; + using ExactDistanceCache = DistanceCacheImpl; + + // Version of DistanceCache which does not bin on pose, but just remembers + // the previous cache entry for a particular query mode. Includes a + // DistanceCache member and wraps it in place of inheritance for speed. + class LastDistanceCache + { + public: + void setDistanceFunction(ExactDistanceCache::DistanceFunction distance_function) + { + cache_.setDistanceFunction(distance_function); + } + + // Returns true on a hit. + // Fast hits make no sense for a last distance cache, so return a bool. + bool checkCache( + const DistanceCacheKey& cache_key, + const geometry_msgs::Pose& pose, + DistanceCacheEntry* cache_entry_ptr, + double* distance_ptr, + const RegionsOfInterestAtPose* rois_ptr = nullptr) + { + // Reuse ExactDistanceCache, but provide a key with an all-zero pose so + // the previous call results are re-used for a given query_region and + // query_obstacles for any pose. + return cache_.checkCache( + cache_key, + pose, + cache_entry_ptr, + distance_ptr, + rois_ptr).first; + } + + void updateCache(const DistanceCacheKey& key, const DistanceCacheEntry& entry) + { + cache_.updateCache(key, entry); + } + + inline void initializeKey( + const QueryRegion query_region, + const QueryObstacles query_obstacles, + DistanceCacheKey* key_ptr) + { + key_ptr->initialize(geometry_msgs::Pose(), query_region, query_obstacles); + } + + void clear() + { + cache_.clear(); + } + + private: + ExactDistanceCache cache_; + }; + /** * The distance cache allows us to find a very good distance guess quickly. * The cache memorizes to a hash table for a pose rounded to the number of @@ -776,60 +1190,16 @@ class Costmap3DQuery * speed-up when distance queries are over the same space. */ DistanceCache distance_cache_; - shared_mutex distance_cache_mutex_; - // Last entries are useful for setting good bounds on misses. - DistanceCacheEntry last_distance_cache_entries_[MAX][OBSTACLES_MAX]; - /** - * Whether the caller wants 2D mode for distance cache thresholds. - * When in 2D mode, the cache thresholds can be tighter and are calculated - * differently than when not in 2D mode. The default is to run in 3D mode. - * The milli and micro caches may be much more effective in 2D mode for a 2D - * robot, as only the radius of the robot matters if query poses will be for - * 2D navigation. - */ - bool threshold_two_d_mode_ = false; - /** - * Safety factor for distance cache thresholds. - * The minimum value that prevents false-negative collisions is increased by - * this factor. - */ - double threshold_factor_ = 1.05; - /** - * The milli-distance cache allows a very fast path when the nearest - * obstacle is more than a threshold of distance away. This results in a - * massive speed up for cases where the nearest obstacle is further than the - * milli_cache_threshold_ - */ - double milli_cache_threshold_; DistanceCache milli_distance_cache_; - shared_mutex milli_distance_cache_mutex_; - /** - * The micro-distance cache allows a very fast path when the nearest - * obstacle is more than a threshold of distance away. This results in a - * massive speed up for cases where the nearest obstacle is further than the - * micro_cache_threshold_ - */ - double micro_cache_threshold_; DistanceCache micro_distance_cache_; - shared_mutex micro_distance_cache_mutex_; - // Immediately return the distance for an exact duplicate query - // This avoid any calculation in the case that the calculation has been done - // since the costmap was updated. + // Use an exact cache to immediately return the distance for an exact + // duplicate query. The costmap is often queried for the same pose, for + // instance by the path planner and by the path validator. ExactDistanceCache exact_distance_cache_; - shared_mutex exact_distance_cache_mutex_; + // If all other caches are missed, using the previous result provides a decent + // bound and is better than no bound at all. + LastDistanceCache last_distance_cache_; unsigned int last_layered_costmap_update_number_; - //! Distance cache bins per meter for binning the pose's position - unsigned int pose_bins_per_meter_; - //! Distance cache bins per rotation for binning the pose's orientation - unsigned int pose_bins_per_rotation_; - //! Milli-distance cache bins per meter for binning the pose's position - unsigned int pose_milli_bins_per_meter_; - //! Milli-distance cache bins per rotation for binning the pose's orientation - unsigned int pose_milli_bins_per_rotation_; - //! Micro-distance cache bins per meter for binning the pose's position - unsigned int pose_micro_bins_per_meter_; - //! Micro-distance cache bins per rotation for binning the pose's orientation - unsigned int pose_micro_bins_per_rotation_; // Statistics gathered between clearing cycles void printStatistics(); void clearStatistics(); diff --git a/costmap_3d/src/costmap_3d_query.cpp b/costmap_3d/src/costmap_3d_query.cpp index 70afaa05c..3d557d5ac 100644 --- a/costmap_3d/src/costmap_3d_query.cpp +++ b/costmap_3d/src/costmap_3d_query.cpp @@ -35,8 +35,11 @@ * Author: C. Andy Martin *********************************************************************/ #include + #include #include +#include + #include #include #include @@ -69,16 +72,17 @@ Costmap3DQuery::Costmap3DQuery( unsigned int pose_micro_bins_per_meter, unsigned int pose_micro_bins_per_rotation) : layered_costmap_3d_(layered_costmap_3d), - query_region_scale_(query_region_scale), - pose_bins_per_meter_(pose_bins_per_meter), - pose_bins_per_rotation_(pose_bins_per_rotation), - pose_milli_bins_per_meter_(pose_milli_bins_per_meter), - pose_milli_bins_per_rotation_(pose_milli_bins_per_rotation), - pose_micro_bins_per_meter_(pose_micro_bins_per_meter), - pose_micro_bins_per_rotation_(pose_micro_bins_per_rotation) + query_region_scale_(query_region_scale) { init(); updateMeshResource(mesh_resource, padding); + setCacheBinSize( + pose_bins_per_meter, + pose_bins_per_rotation, + pose_milli_bins_per_meter, + pose_milli_bins_per_rotation, + pose_micro_bins_per_meter, + pose_micro_bins_per_rotation); } Costmap3DQuery::Costmap3DQuery(const Costmap3DConstPtr& costmap_3d, @@ -92,13 +96,7 @@ Costmap3DQuery::Costmap3DQuery(const Costmap3DConstPtr& costmap_3d, unsigned int pose_micro_bins_per_meter, unsigned int pose_micro_bins_per_rotation) : layered_costmap_3d_(nullptr), - query_region_scale_(query_region_scale), - pose_bins_per_meter_(pose_bins_per_meter), - pose_bins_per_rotation_(pose_bins_per_rotation), - pose_milli_bins_per_meter_(pose_milli_bins_per_meter), - pose_milli_bins_per_rotation_(pose_milli_bins_per_rotation), - pose_micro_bins_per_meter_(pose_micro_bins_per_meter), - pose_micro_bins_per_rotation_(pose_micro_bins_per_rotation) + query_region_scale_(query_region_scale) { init(); // Make a local copy of the costmap in question @@ -109,6 +107,13 @@ Costmap3DQuery::Costmap3DQuery(const Costmap3DConstPtr& costmap_3d, // an entire planning cycle. octree_ptr_.reset(new Costmap3D(*costmap_3d)); updateMeshResource(mesh_resource, padding); + setCacheBinSize( + pose_bins_per_meter, + pose_bins_per_rotation, + pose_milli_bins_per_meter, + pose_milli_bins_per_rotation, + pose_micro_bins_per_meter, + pose_micro_bins_per_rotation); // For a buffered query, go ahead and setup the interior collision LUT now, // as the resolution can not change. checkInteriorCollisionLUT(); @@ -123,6 +128,19 @@ void Costmap3DQuery::init() clearStatistics(); robot_model_halfspaces_.reset(new std::vector>); last_octomap_resolution_ = 0.0; + auto distance_function = std::bind( + &Costmap3DQuery::handleDistanceInteriorCollisions, + this, + std::placeholders::_1, + std::placeholders::_2); + distance_cache_.setDistanceFunction(distance_function); + milli_distance_cache_.setDistanceFunction(distance_function); + micro_distance_cache_.setDistanceFunction(distance_function); + exact_distance_cache_.setDistanceFunction(distance_function); + last_distance_cache_.setDistanceFunction(distance_function); + milli_distance_cache_.setDebugPrefix("milli-"); + micro_distance_cache_.setDebugPrefix("micro-"); + exact_distance_cache_.setDebugPrefix("exact "); } void Costmap3DQuery::setCacheBinSize( @@ -133,129 +151,18 @@ void Costmap3DQuery::setCacheBinSize( unsigned int pose_micro_bins_per_meter, unsigned int pose_micro_bins_per_rotation) { - unique_lock instance_lock(instance_mutex_); - if (pose_bins_per_meter != pose_bins_per_meter_ || - pose_bins_per_rotation != pose_bins_per_rotation_ || - pose_milli_bins_per_meter != pose_milli_bins_per_meter_ || - pose_milli_bins_per_rotation != pose_milli_bins_per_rotation_ || - pose_micro_bins_per_meter != pose_micro_bins_per_meter_ || - pose_micro_bins_per_rotation != pose_micro_bins_per_rotation_) - { - // There is no need to invalidate the existing cache entries. If the bins - // per meter change, either some or all of the new calls will miss the - // existing cache entries. The existing entries are all still correct, so - // keeping them is fine. Also, if only the thresholds change all the cache - // entries will still work. - pose_bins_per_meter_ = pose_bins_per_meter; - pose_bins_per_rotation_ = pose_bins_per_rotation; - pose_milli_bins_per_meter_ = pose_milli_bins_per_meter; - pose_milli_bins_per_rotation_ = pose_milli_bins_per_rotation; - pose_micro_bins_per_meter_ = pose_micro_bins_per_meter; - pose_micro_bins_per_rotation_ = pose_micro_bins_per_rotation; - ROS_INFO_STREAM("Set cache bins per meter: " << pose_bins_per_meter_); - ROS_INFO_STREAM("Set cache bins per rotation: " << pose_bins_per_rotation_); - ROS_INFO_STREAM("Set milli-cache bins per meter: " << pose_milli_bins_per_meter_); - ROS_INFO_STREAM("Set milli-cache bins per rotation: " << pose_milli_bins_per_rotation_); - ROS_INFO_STREAM("Set micro-cache bins per meter: " << pose_micro_bins_per_meter_); - ROS_INFO_STREAM("Set micro-cache bins per rotation: " << pose_micro_bins_per_rotation_); - calculateCacheDistanceThresholds(); - } + distance_cache_.setBinSize(pose_bins_per_meter, pose_bins_per_rotation); + milli_distance_cache_.setBinSize(pose_milli_bins_per_meter, pose_milli_bins_per_rotation); + micro_distance_cache_.setBinSize(pose_micro_bins_per_meter, pose_micro_bins_per_rotation); } void Costmap3DQuery::setCacheThresholdParameters( bool threshold_two_d_mode, double threshold_factor) { - unique_lock instance_lock(instance_mutex_); - if (threshold_two_d_mode != threshold_two_d_mode_ || - threshold_factor != threshold_factor_) - { - threshold_two_d_mode_ = threshold_two_d_mode; - threshold_factor_ = threshold_factor; - ROS_INFO_STREAM("Set threshold 2D mode: " << threshold_two_d_mode_ ? "true" : "false"); - ROS_INFO_STREAM("Set threshold factor: " << threshold_factor_); - calculateCacheDistanceThresholds(); - } -} - -inline double calculateCacheDistanceThreshold( - int bins_per_meter, - int bins_per_rotation, - double mesh_radius, - double threshold_factor, - bool two_d_mode) -{ - double diagonal_factor = two_d_mode ? std::sqrt(2.0) : std::sqrt(3.0); - // Because binPoseAngularDistanceLimit measures the maximum rotational size - // of a bin, it may return up to 2 * M_PI, but the maximum chord length is - // at an angle of M_PI, so do not use an angular error higher than M_PI. - double max_angular_error = std::min(M_PI, binPoseAngularDistanceLimit( - bins_per_rotation, two_d_mode)); - // Add the diagonal length of positional error plus the chord length of the - // maximum angular error. - double threshold = diagonal_factor / bins_per_meter + - 2.0 * mesh_radius * std::sin(0.5 * max_angular_error); - return threshold * threshold_factor; -} - -void Costmap3DQuery::calculateCacheDistanceThresholds() -{ - // The distance thresholds must be greater than the maximum translation error - // plus the maximum rotation error, otherwise the query may return - // no-collision when one is actually present. In a robot with full - // three-dimensional movement this would equate to the length of the diagonal - // of a bin plus the chord length corresponding to the size of a rotational - // bin given the maximum radius of the robot mesh. For two-dimensional - // movement of a robot on a plane aligned with the costmap, the threshold - // is lower at only the diagonal of the bin projected into the plane as - // a square and the chord length given the radius of the robot mesh projected - // into the plane (the robot footprint radius). - Eigen::Vector4f origin(0.0, 0.0, 0.0, 0.0), max_pt(0.0, 0.0, 0.0, 0.0); - if (threshold_two_d_mode_) - { - // Create a 2D version of the mesh cloud by truncating the Z value. - pcl::PointCloud mesh_points_2d(*robot_mesh_points_); - for (auto& point : mesh_points_2d) - { - point.z = 0.0; - } - pcl::getMaxDistance(mesh_points_2d, origin, max_pt); - } - else - { - pcl::getMaxDistance(*robot_mesh_points_, origin, max_pt); - } - const Eigen::Vector3f max_pt3 = max_pt.head<3>(); - double mesh_radius = max_pt3.norm(); - ROS_INFO_STREAM("Calculated mesh radius: " << mesh_radius << "m"); - if (pose_milli_bins_per_meter_ > 0) - { - milli_cache_threshold_ = calculateCacheDistanceThreshold( - pose_milli_bins_per_meter_, - pose_milli_bins_per_rotation_, - mesh_radius, - threshold_factor_, - threshold_two_d_mode_); - ROS_INFO_STREAM("Calculated milli-distance cache threshold: " << milli_cache_threshold_ << "m"); - } - else - { - milli_cache_threshold_ = std::numeric_limits::infinity(); - } - if (pose_micro_bins_per_meter_ > 0) - { - micro_cache_threshold_ = calculateCacheDistanceThreshold( - pose_micro_bins_per_meter_, - pose_micro_bins_per_rotation_, - mesh_radius, - threshold_factor_, - threshold_two_d_mode_); - ROS_INFO_STREAM("Calculated micro-distance cache threshold: " << micro_cache_threshold_ << "m"); - } - else - { - micro_cache_threshold_ = std::numeric_limits::infinity(); - } + distance_cache_.setThresholdParameters(threshold_two_d_mode, threshold_factor); + milli_distance_cache_.setThresholdParameters(threshold_two_d_mode, threshold_factor); + micro_distance_cache_.setThresholdParameters(threshold_two_d_mode, threshold_factor); } void Costmap3DQuery::updateCostmap(const Costmap3DConstPtr& costmap_3d) @@ -291,32 +198,6 @@ bool Costmap3DQuery::needCheckCostmap(std::shared_ptr new return need_update; } -// Borrowed from https://artificial-mind.net/blog/2021/10/09/unordered-map-badness -template -static double unordered_map_badness(Map const& map) -{ - auto const lambda = map.size() / static_cast(map.bucket_count()); - - auto cost = 0.; - for (auto const& entry : map) - cost += map.bucket_size(map.bucket(entry.first)); - cost /= map.size(); - - return std::max(0., cost / (1 + lambda) - 1); -} - -template -static void printDistanceCacheDebug( - const Map& cache, - const std::string& prefix) -{ - ROS_DEBUG_STREAM_NAMED( - "query_distance_cache", prefix << - " size: " << cache.size() << - " bucket count: " << cache.bucket_count() << - " badness: " << unordered_map_badness(cache)); -} - // Caller must already hold the instance mutex void Costmap3DQuery::checkCostmap(std::shared_ptr new_octree) { @@ -326,11 +207,6 @@ void Costmap3DQuery::checkCostmap(std::shared_ptr new_oct bool need_update = needCheckCostmap(new_octree); if (need_update) { - unique_lock distance_cache_lock(distance_cache_mutex_); - unique_lock milli_distance_cache_lock(milli_distance_cache_mutex_); - unique_lock micro_distance_cache_lock(micro_distance_cache_mutex_); - unique_lock exact_distance_cache_lock(exact_distance_cache_mutex_); - if (layered_costmap_3d_) { if (layered_costmap_3d_->getCostmap3D() != octree_ptr_) @@ -349,129 +225,28 @@ void Costmap3DQuery::checkCostmap(std::shared_ptr new_oct } nonlethal_octree_ptr_ = std::static_pointer_cast(octree_ptr_)->nonlethalOnly(); checkInteriorCollisionLUT(); - // The costmap has been updated since the last query, reset our caches - // Delete any distance cache entries that have had their corresponding - // octomap cells removed. It is fine to keep entries in the presence of - // additions, as the entry only defines an upper bound. Because the size of - // the tree is limited, the size of the cache has a natural limit. If this - // limit is ever too large, a separate cache size may need to be set. - { - printDistanceCacheDebug(distance_cache_, "distance cache: "); - size_t start_size = distance_cache_.size(); - auto it = distance_cache_.begin(); - while (it != distance_cache_.end()) - { - bool erase = true; - Costmap3DIndex index; - unsigned int depth; - const octomap::OcTree* octree_to_query; - if (it->first.getQueryObstacles() == NONLETHAL_ONLY) - { - octree_to_query = nonlethal_octree_ptr_.get(); - } - else - { - octree_to_query = octree_ptr_.get(); - } - if (it->second.getCostmapIndexAndDepth(*octree_to_query, &index, &depth)) - { - unsigned int found_depth; - auto* node = octree_to_query->search(index, depth, &found_depth); - if (node && - !octree_to_query->nodeHasChildren(node) && - depth == found_depth && - octree_to_query->isNodeOccupied(node)) - { - // The node exists and has no children (so its a leaf and not an - // inner node), is at the correct depth and is considered occupied - // for the proper octree. Keep this entry. - erase = false; - } - } - - if (erase) - it = distance_cache_.erase(it); - else - ++it; - } - ROS_DEBUG_STREAM_NAMED("query_distance_cache", - "distance cache removed " << start_size - distance_cache_.size() << " entries"); - } + // The costmap has been updated since the last query, reset our caches + distance_cache_.printDistanceCacheDebug(); + distance_cache_.handleCostmapUpdate(octree_ptr_.get(), nonlethal_octree_ptr_.get()); - // Delete any milli distance cache entries that have had their - // corresponding octomap cells removed. If the cell has not been removed, - // invalidate the milli cache entry from being used in the fast-path, but - // let it still be used as an upper-bound for the slow path, as it greatly - // reduces subsequent query times. Also note that the caches are always - // updated after a successful query, so the staleness of the bound will be - // fixed the next query that hits the cache. - { - printDistanceCacheDebug(milli_distance_cache_, "milli-distance cache: "); - size_t start_size = milli_distance_cache_.size(); - auto it = milli_distance_cache_.begin(); - while (it != milli_distance_cache_.end()) - { - bool erase = true; - Costmap3DIndex index; - unsigned int depth; - const octomap::OcTree* octree_to_query; - if (it->first.getQueryObstacles() == NONLETHAL_ONLY) - { - octree_to_query = nonlethal_octree_ptr_.get(); - } - else - { - octree_to_query = octree_ptr_.get(); - } - if (it->second.getCostmapIndexAndDepth(*octree_to_query, &index, &depth)) - { - unsigned int found_depth; - auto* node = octree_to_query->search(index, depth, &found_depth); - if (node && - !octree_to_query->nodeHasChildren(node) && - depth == found_depth && - octree_to_query->isNodeOccupied(node)) - { - // The node exists and has no children (so its a leaf and not an - // inner node), is at the correct depth and is considered occupied - // for the proper octree. Keep this entry. - erase = false; - } - } - - if (erase) - { - it = milli_distance_cache_.erase(it); - } - else - { - // Fast-path only used for finite recorded distance - it->second.distance = std::numeric_limits::infinity(); - ++it; - } - } - ROS_DEBUG_STREAM_NAMED("query_distance_cache", - "milli-distance cache removed " << start_size - milli_distance_cache_.size() << " entries"); - } + milli_distance_cache_.printDistanceCacheDebug(); + milli_distance_cache_.handleCostmapUpdate(octree_ptr_.get(), nonlethal_octree_ptr_.get()); // Drop the micro cache as even if we removed invalid entries we can not // use the fast path which is the micro cache's strong point. - printDistanceCacheDebug(micro_distance_cache_, "micro-distance cache: "); + micro_distance_cache_.printDistanceCacheDebug(); micro_distance_cache_.clear(); + // We must drop the exact cache after the costmap has changed - printDistanceCacheDebug(exact_distance_cache_, "exact distance cache: "); + exact_distance_cache_.printDistanceCacheDebug(); exact_distance_cache_.clear(); + // Invalidate the last cache. We could keep entries that still point to // valid boxes, but there is little to gain, as any misses on the next // cycle will quickly have a last cache entry to speed them up. - for (unsigned int i=0; i < MAX; ++i) - { - for (unsigned int j=0; j < OBSTACLES_MAX; ++j) - { - last_distance_cache_entries_[i][j].clear(); - } - } + last_distance_cache_.clear(); + printStatistics(); clearStatistics(); if (layered_costmap_3d_) @@ -590,7 +365,6 @@ void Costmap3DQuery::addPCLPolygonToFCLTriangles( } } - void Costmap3DQuery::addPCLPolygonMeshToRobotModel( const pcl::PolygonMesh& pcl_mesh, double padding, @@ -654,7 +428,9 @@ void Costmap3DQuery::updateMeshResource(const std::string& mesh_resource, double robot_model_->vertices[tri[1]], robot_model_->vertices[tri[2]])); } - calculateCacheDistanceThresholds(); + distance_cache_.setRobotMeshPoints(robot_mesh_points_); + milli_distance_cache_.setRobotMeshPoints(robot_mesh_points_); + micro_distance_cache_.setRobotMeshPoints(robot_mesh_points_); } std::string Costmap3DQuery::getFileNameFromPackageURL(const std::string& url) @@ -889,24 +665,23 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, return std::numeric_limits::max(); } - DistanceCacheKey exact_cache_key(pose, query_region, query_obstacles); + double distance; + DistanceCacheKey exact_cache_key; + DistanceCacheEntry cache_entry; + exact_distance_cache_.initializeKey(pose, query_region, query_obstacles, &exact_cache_key); + if (exact_distance_cache_.checkCache( + exact_cache_key, + pose, + &cache_entry, + &distance, + nullptr, + track_statistics, + &start_time, + &exact_hits_since_clear_, + &exact_hits_since_clear_us_).first) { - shared_lock cache_read_lock(exact_distance_cache_mutex_); - auto exact_cache_entry = exact_distance_cache_.find(exact_cache_key); - if (exact_cache_entry != exact_distance_cache_.end()) - { - double distance = exact_cache_entry->second.distance; - exact_cache_entry->second.setupResult(return_result); - if (track_statistics) - { - exact_hits_since_clear_.fetch_add(1, std::memory_order_relaxed); - exact_hits_since_clear_us_.fetch_add( - std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start_time).count(), - std::memory_order_relaxed); - } - return distance; - } + cache_entry.setupResult(return_result); + return distance; } typename OcTreeMeshSolver::DistanceResult result; @@ -917,142 +692,105 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, // Store the region on the stack to avoid dynamic memory allocation. RegionsOfInterestAtPose rois(query_region, query_region_scale_, pose); - DistanceCacheKey milli_cache_key( - pose, query_region, query_obstacles, pose_milli_bins_per_meter_, pose_milli_bins_per_rotation_); + DistanceCacheKey milli_cache_key; + milli_distance_cache_.initializeKey(pose, query_region, query_obstacles, &milli_cache_key); bool milli_hit = false; - DistanceCacheEntry cache_entry; - if (pose_milli_bins_per_meter_ > 0 && pose_milli_bins_per_rotation_ > 0) + bool fast_hit = false; + std::tie(milli_hit, fast_hit) = milli_distance_cache_.checkCache( + milli_cache_key, + pose, + &cache_entry, + &distance, + &rois, + track_statistics, + &start_time, + &fast_milli_hits_since_clear_, + &fast_milli_hits_since_clear_us_, + opts.exact_signed_distance, + opts.directly_use_cache_when_above_threshold); + if (fast_hit) { - shared_lock cache_read_lock(milli_distance_cache_mutex_); - auto milli_cache_entry = milli_distance_cache_.find(milli_cache_key); - if (milli_cache_entry != milli_distance_cache_.end()) - { - cache_entry = milli_cache_entry->second; - } + cache_entry.setupResult(return_result); + return distance; } - if (cache_entry && - rois.distanceCacheEntryInside(cache_entry)) + else if (milli_hit) { - double distance = handleDistanceInteriorCollisions( - cache_entry, - pose); - if ((!opts.exact_signed_distance && distance <= 0.0) || - opts.directly_use_cache_when_above_threshold && - std::isfinite(cache_entry.distance) && ( - cache_entry.distance > milli_cache_threshold_ || - distance > milli_cache_threshold_)) - { - cache_entry.setupResult(return_result); - if (track_statistics) - { - fast_milli_hits_since_clear_.fetch_add(1, std::memory_order_relaxed); - fast_milli_hits_since_clear_us_.fetch_add( - std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start_time).count(), - std::memory_order_relaxed); - } - return distance; - } - else + // Too close to directly use the cache, but use the + // calculated distance as the pose_distance upper bound. + if (distance < pose_distance) { - // we are too close to directly use the milli-cache, but use the - // calculated distance as the pose_distance upper bound. - if (distance < pose_distance) - { - milli_hit = true; - pose_distance = distance; - cache_entry.setupResult(&result); - } + pose_distance = distance; + cache_entry.setupResult(&result); } } - DistanceCacheKey micro_cache_key( - pose, query_region, query_obstacles, pose_micro_bins_per_meter_, pose_micro_bins_per_rotation_); + DistanceCacheKey micro_cache_key; + micro_distance_cache_.initializeKey(pose, query_region, query_obstacles, µ_cache_key); bool micro_hit = false; - cache_entry.clear(); - if (pose_micro_bins_per_meter_ > 0 && pose_micro_bins_per_rotation_ > 0) + std::tie(micro_hit, fast_hit) = micro_distance_cache_.checkCache( + micro_cache_key, + pose, + &cache_entry, + &distance, + &rois, + track_statistics, + &start_time, + &fast_micro_hits_since_clear_, + &fast_micro_hits_since_clear_us_, + opts.exact_signed_distance, + opts.directly_use_cache_when_above_threshold); + if (fast_hit) { - shared_lock cache_read_lock(micro_distance_cache_mutex_); - auto micro_cache_entry = micro_distance_cache_.find(micro_cache_key); - if (micro_cache_entry != micro_distance_cache_.end()) - { - cache_entry = micro_cache_entry->second; - } + cache_entry.setupResult(return_result); + return distance; } - if (cache_entry && - rois.distanceCacheEntryInside(cache_entry)) + else if (micro_hit) { - double distance = handleDistanceInteriorCollisions( - cache_entry, - pose); - if ((!opts.exact_signed_distance && distance <= 0.0) || - opts.directly_use_cache_when_above_threshold && ( - cache_entry.distance > micro_cache_threshold_ || - distance > micro_cache_threshold_)) - { - cache_entry.setupResult(return_result); - if (track_statistics) - { - fast_micro_hits_since_clear_.fetch_add(1, std::memory_order_relaxed); - fast_micro_hits_since_clear_us_.fetch_add( - std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start_time).count(), - std::memory_order_relaxed); - } - return distance; - } - else + // Too close to directly use the cache, but use the + // calculated distance as the pose_distance upper bound. + if (distance < pose_distance) { - // we are too close to directly use the micro-cache, but use the - // calculated distance as the pose_distance upper bound. - if (distance < pose_distance) - { - micro_hit = true; - pose_distance = distance; - cache_entry.setupResult(&result); - } + pose_distance = distance; + cache_entry.setupResult(&result); } } - DistanceCacheKey cache_key(pose, query_region, query_obstacles, pose_bins_per_meter_, pose_bins_per_rotation_); + // The keys are needed to update below. + // Initialize them even if skipping the cache checks. + DistanceCacheKey cache_key; + distance_cache_.initializeKey(pose, query_region, query_obstacles, &cache_key); + DistanceCacheKey last_cache_key; + last_distance_cache_.initializeKey(query_region, query_obstacles, &cache_key); bool cache_hit = false; // Assume a milli hit or micro hit is much better than the normal distance // cache at setting an upper bound, skip the normal cache lookup in such // cases. if (!milli_hit && !micro_hit) { - bool count_as_hit = false; - cache_entry.clear(); - if (pose_bins_per_meter_ > 0 && pose_bins_per_rotation_ > 0) + if (distance_cache_.checkCache( + cache_key, + pose, + &cache_entry, + &distance, + &rois).first) { - shared_lock cache_read_lock(distance_cache_mutex_); - auto distance_cache_entry = distance_cache_.find(cache_key); - if (distance_cache_entry != distance_cache_.end()) - { - // If we end up using this entry, count this as a hit for statistics. - count_as_hit = true; - cache_entry = distance_cache_entry->second; - } - else + if (distance < pose_distance) { - // Missed the distance cache, but use the last entry if available. - count_as_hit = false; - cache_entry = last_distance_cache_entries_[query_region][query_obstacles]; + cache_hit = true; + pose_distance = distance; + cache_entry.setupResult(&result); } } - if (cache_entry && - rois.distanceCacheEntryInside(cache_entry)) + // Missed the distance cache, but use the last entry if available. + else if (last_distance_cache_.checkCache( + cache_key, + pose, + &cache_entry, + &distance, + &rois)) { - // Cache hit, find the distance between the mesh triangle at the new pose - // and the octomap box, and use this as our initial guess in the result. - // This greatly prunes the search tree, yielding a big increase in runtime - // performance. - double distance = handleDistanceInteriorCollisions( - cache_entry, - pose); if (distance < pose_distance) { - cache_hit = count_as_hit; pose_distance = distance; cache_entry.setupResult(&result); } @@ -1070,8 +808,6 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, result.min_distance = pose_distance; } - double distance; - if (result.min_distance > 0.0 || opts.exact_signed_distance) { // Because FCL's OcTree/Mesh distance treats the Mesh as hollow, we must @@ -1114,13 +850,11 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, &result); } - if (opts.directly_use_cache_when_below_threshold && (micro_hit || milli_hit)) + // When directly using the micro or milli cache even below the threshold, + // use the cache distance as long as there is no collision + if (opts.directly_use_cache_when_below_threshold && (micro_hit || milli_hit) && result.min_distance > 0.0) { - // If there was no collision, use the closest cache entry as the distance. - if (result.min_distance > 0.0) - { - distance = pose_distance; - } + distance = pose_distance; } else { @@ -1138,29 +872,13 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, new_entry.distance = distance; // Update distance caches. - // While it may seem expensive to copy the cache entries into the - // caches, it prevents cache aliasing and avoids dynamic memory. - if (pose_bins_per_meter_ > 0 && pose_bins_per_rotation_ > 0) - { - unique_lock cache_write_lock(distance_cache_mutex_); - distance_cache_[cache_key] = new_entry; - last_distance_cache_entries_[query_region][query_obstacles] = new_entry; - } - if (pose_milli_bins_per_meter_ > 0 && pose_milli_bins_per_rotation_ > 0) - { - unique_lock cache_write_lock(milli_distance_cache_mutex_); - milli_distance_cache_[milli_cache_key] = new_entry; - } - if (pose_micro_bins_per_meter_ > 0 && pose_micro_bins_per_rotation_ > 0) - { - unique_lock cache_write_lock(micro_distance_cache_mutex_); - micro_distance_cache_[micro_cache_key] = new_entry; - } - { - unique_lock cache_write_lock(exact_distance_cache_mutex_); - exact_distance_cache_[exact_cache_key] = new_entry; - } - cache_entry.setupResult(return_result); + distance_cache_.updateCache(cache_key, new_entry); + milli_distance_cache_.updateCache(milli_cache_key, new_entry); + micro_distance_cache_.updateCache(micro_cache_key, new_entry); + exact_distance_cache_.updateCache(exact_cache_key, new_entry); + last_distance_cache_.updateCache(last_cache_key, new_entry); + + new_entry.setupResult(return_result); } else { @@ -1183,8 +901,6 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start_time).count(), std::memory_order_relaxed); - hit_fcl_bv_distance_calculations_.fetch_add(result.bv_distance_calculations); - hit_fcl_primitive_distance_calculations_.fetch_add(result.primitive_distance_calculations); } else if (milli_hit) { @@ -1193,8 +909,6 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start_time).count(), std::memory_order_relaxed); - hit_fcl_bv_distance_calculations_.fetch_add(result.bv_distance_calculations); - hit_fcl_primitive_distance_calculations_.fetch_add(result.primitive_distance_calculations); } else if (cache_hit) { @@ -1203,15 +917,22 @@ double Costmap3DQuery::calculateDistance(const geometry_msgs::Pose& pose, std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start_time).count(), std::memory_order_relaxed); - hit_fcl_bv_distance_calculations_.fetch_add(result.bv_distance_calculations); - hit_fcl_primitive_distance_calculations_.fetch_add(result.primitive_distance_calculations); } else { + // Currently last cache hits are lumped into misses misses_since_clear_us_.fetch_add( std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start_time).count(), std::memory_order_relaxed); + } + if (micro_hit || milli_hit || cache_hit) + { + hit_fcl_bv_distance_calculations_.fetch_add(result.bv_distance_calculations); + hit_fcl_primitive_distance_calculations_.fetch_add(result.primitive_distance_calculations); + } + else + { miss_fcl_bv_distance_calculations_.fetch_add(result.bv_distance_calculations); miss_fcl_primitive_distance_calculations_.fetch_add(result.primitive_distance_calculations); }