From bdbb03aa5150a3468f4979da6231a53f738c2565 Mon Sep 17 00:00:00 2001 From: duongtd Date: Tue, 14 Jul 2026 11:08:23 +0700 Subject: [PATCH] otimal deep coppy obj --- CMakeLists.txt | 1 + config/costmap_params.yaml | 4 +- include/robot_costmap_2d/costmap_2d.h | 1 + include/robot_costmap_2d/inflation_layer.h | 15 +- include/robot_costmap_2d/layered_costmap.h | 24 +++ include/robot_costmap_2d/observation.h | 157 +++++++++++------ include/robot_costmap_2d/observation_buffer.h | 2 + include/robot_costmap_2d/obstacle_layer.h | 2 +- include/robot_costmap_2d/voxel_layer.h | 24 ++- plugins/inflation_layer.cpp | 107 +++++++---- plugins/obstacle_layer.cpp | 54 +++--- plugins/voxel_layer.cpp | 166 ++++++++++++------ src/costmap_2d.cpp | 22 ++- src/costmap_2d_robot.cpp | 43 ++--- src/layered_costmap.cpp | 126 +++++++++++-- src/observation_buffer.cpp | 154 ++++++++-------- test/coordinates_test.cpp | 101 ++++++++++- 17 files changed, 702 insertions(+), 301 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 800f710..cab576d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -299,6 +299,7 @@ if(BUILD_COSTMAP_TESTS) if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp) add_executable(test_costmap test/coordinates_test.cpp) target_link_libraries(test_costmap PRIVATE + plugins robot_costmap_2d GTest::GTest GTest::Main diff --git a/config/costmap_params.yaml b/config/costmap_params.yaml index 35e2c17..c9c3cb4 100644 --- a/config/costmap_params.yaml +++ b/config/costmap_params.yaml @@ -25,6 +25,8 @@ robot_costmap_2d: - [-0.3, 0.3] transform_tolerance: 0.0 + performance_metrics_enabled: false + performance_metrics_period: 5.0 update_frequency: 1.0 width: 0.0 height: 0.0 @@ -33,4 +35,4 @@ robot_costmap_2d: origin_y: 0.0 footprint_padding: 0.0 - robot_radius: 0.0 \ No newline at end of file + robot_radius: 0.0 diff --git a/include/robot_costmap_2d/costmap_2d.h b/include/robot_costmap_2d/costmap_2d.h index faf741b..1740281 100755 --- a/include/robot_costmap_2d/costmap_2d.h +++ b/include/robot_costmap_2d/costmap_2d.h @@ -425,6 +425,7 @@ protected: double origin_y_; unsigned char* costmap_; unsigned char default_value_; + std::vector rolling_window_scratch_; class MarkCell { diff --git a/include/robot_costmap_2d/inflation_layer.h b/include/robot_costmap_2d/inflation_layer.h index 3b4c855..ba220db 100755 --- a/include/robot_costmap_2d/inflation_layer.h +++ b/include/robot_costmap_2d/inflation_layer.h @@ -42,6 +42,9 @@ #include #include +#include +#include + namespace robot_costmap_2d { /** @@ -77,8 +80,7 @@ public: virtual ~InflationLayer() { deleteKernels(); - if (seen_) - delete[] seen_; + delete inflation_access_; } virtual void onInitialize(); @@ -184,10 +186,13 @@ private: unsigned int cell_inflation_radius_; unsigned int cached_cell_inflation_radius_; - std::map > inflation_cells_; + std::vector> inflation_cells_; + std::vector distance_levels_; + std::vector distance_bin_lookup_; + unsigned int distance_lookup_size_ = 0; - bool* seen_; - int seen_size_; + std::vector seen_; + std::uint32_t seen_generation_ = 0; unsigned char** cached_costs_; double** cached_distances_; diff --git a/include/robot_costmap_2d/layered_costmap.h b/include/robot_costmap_2d/layered_costmap.h index f7438c8..223dab0 100755 --- a/include/robot_costmap_2d/layered_costmap.h +++ b/include/robot_costmap_2d/layered_costmap.h @@ -43,6 +43,8 @@ #include #include #include +#include +#include namespace robot_costmap_2d { @@ -71,6 +73,8 @@ public: */ void updateMap(double robot_x, double robot_y, double robot_yaw); + void setPerformanceMetrics(bool enabled, double reporting_period_seconds); + inline const std::string& getGlobalFrameID() const noexcept { return global_frame_; @@ -155,6 +159,17 @@ public: double getInscribedRadius() { return inscribed_radius_; } private: + struct LayerPerformance + { + std::uint64_t bounds_nanoseconds = 0; + std::uint64_t costs_nanoseconds = 0; + std::uint64_t bounds_calls = 0; + std::uint64_t costs_calls = 0; + }; + + void resetPerformanceMetrics(); + void maybeReportPerformance(); + Costmap2D costmap_; std::string global_frame_; @@ -170,6 +185,15 @@ private: bool size_locked_; double circumscribed_radius_, inscribed_radius_; std::vector footprint_; + + bool performance_metrics_enabled_ = false; + double performance_metrics_period_seconds_ = 5.0; + std::chrono::steady_clock::time_point performance_window_start_; + std::uint64_t performance_cycle_nanoseconds_ = 0; + std::uint64_t performance_reset_nanoseconds_ = 0; + std::uint64_t performance_cycles_ = 0; + std::vector performance_cycle_samples_; + std::vector layer_performance_; }; } // namespace robot_costmap_2d diff --git a/include/robot_costmap_2d/observation.h b/include/robot_costmap_2d/observation.h index 39e9b87..bdd4ca4 100755 --- a/include/robot_costmap_2d/observation.h +++ b/include/robot_costmap_2d/observation.h @@ -35,6 +35,9 @@ #include #include #include +#include +#include +#include namespace robot_costmap_2d { @@ -49,7 +52,8 @@ class DepthCameraObservation { public: DepthCameraObservation() - : data_(nullptr), + : data_handle_(), + data_(nullptr), topic_(), pixel_step_(0), min_range_(0.0), @@ -64,7 +68,25 @@ public: unsigned int pixel_step, double min_range, double max_range) - : data_(new robot_sensor_msgs::DepthCameraData(data)), + : data_handle_(boost::make_shared(data)), + data_(data_handle_.get()), + topic_(std::move(topic)), + received_time_(received_time), + pixel_step_(pixel_step), + min_range_(min_range), + max_range_(max_range) + { + } + + DepthCameraObservation( + robot_sensor_msgs::DepthCameraData::ConstPtr data, + std::string topic, + const robot::Time& received_time, + unsigned int pixel_step, + double min_range, + double max_range) + : data_handle_(std::move(data)), + data_(data_handle_.get()), topic_(std::move(topic)), received_time_(received_time), pixel_step_(pixel_step), @@ -73,11 +95,9 @@ public: { } - // Copy constructor: deep copy DepthCameraObservation(const DepthCameraObservation& other) - : data_(other.data_ - ? new robot_sensor_msgs::DepthCameraData(*other.data_) - : nullptr), + : data_handle_(other.data_handle_), + data_(data_handle_.get()), topic_(other.topic_), received_time_(other.received_time_), pixel_step_(other.pixel_step_), @@ -86,37 +106,9 @@ public: { } - // Copy assignment: deep copy - DepthCameraObservation& operator=(const DepthCameraObservation& other) - { - if (this == &other) - { - return *this; - } - - robot_sensor_msgs::DepthCameraData* new_data = nullptr; - - if (other.data_ != nullptr) - { - new_data = - new robot_sensor_msgs::DepthCameraData(*other.data_); - } - - delete data_; - data_ = new_data; - - topic_ = other.topic_; - received_time_ = other.received_time_; - pixel_step_ = other.pixel_step_; - min_range_ = other.min_range_; - max_range_ = other.max_range_; - - return *this; - } - - // Move constructor: chuyển quyền sở hữu DepthCameraObservation(DepthCameraObservation&& other) noexcept - : data_(other.data_), + : data_handle_(std::move(other.data_handle_)), + data_(data_handle_.get()), topic_(std::move(other.topic_)), received_time_(other.received_time_), pixel_step_(other.pixel_step_), @@ -129,17 +121,28 @@ public: other.max_range_ = 0.0; } - // Move assignment + DepthCameraObservation& operator=(const DepthCameraObservation& other) + { + if (this == &other) + return *this; + + data_handle_ = other.data_handle_; + data_ = data_handle_.get(); + topic_ = other.topic_; + received_time_ = other.received_time_; + pixel_step_ = other.pixel_step_; + min_range_ = other.min_range_; + max_range_ = other.max_range_; + return *this; + } + DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept { if (this == &other) - { return *this; - } - delete data_; - - data_ = other.data_; + data_handle_ = std::move(other.data_handle_); + data_ = data_handle_.get(); topic_ = std::move(other.topic_); received_time_ = other.received_time_; pixel_step_ = other.pixel_step_; @@ -154,13 +157,10 @@ public: return *this; } - ~DepthCameraObservation() - { - delete data_; - data_ = nullptr; - } + ~DepthCameraObservation() = default; - robot_sensor_msgs::DepthCameraData* data_; + robot_sensor_msgs::DepthCameraData::ConstPtr data_handle_; + const robot_sensor_msgs::DepthCameraData* data_; std::string topic_; robot::Time received_time_; unsigned int pixel_step_; @@ -180,14 +180,12 @@ public: * @brief Creates an empty observation */ Observation() : - cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0) + cloud_handle_(boost::make_shared()), + cloud_(cloud_handle_.get()), obstacle_range_(0.0), raytrace_range_(0.0) { } - virtual ~Observation() - { - delete cloud_; - } + virtual ~Observation() = default; /** * @brief Creates an observation from an origin point and a point cloud @@ -198,7 +196,17 @@ public: */ Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range, double raytrace_range) : - origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)), + origin_(origin), cloud_handle_(boost::make_shared(cloud)), + cloud_(cloud_handle_.get()), + obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) + { + } + + Observation(robot_geometry_msgs::Point origin, + boost::shared_ptr cloud, + double obstacle_range, double raytrace_range) : + origin_(std::move(origin)), cloud_handle_(std::move(cloud)), + cloud_(cloud_handle_.get()), obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) { } @@ -208,22 +216,59 @@ public: * @param obs The observation to copy */ Observation(const Observation& obs) : - origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))), + origin_(obs.origin_), cloud_handle_(obs.cloud_handle_), cloud_(cloud_handle_.get()), obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_) { } + Observation(Observation&& obs) noexcept : + origin_(std::move(obs.origin_)), cloud_handle_(std::move(obs.cloud_handle_)), + cloud_(cloud_handle_.get()), obstacle_range_(obs.obstacle_range_), + raytrace_range_(obs.raytrace_range_) + { + obs.cloud_ = nullptr; + } + + Observation& operator=(const Observation& obs) + { + if (this == &obs) + return *this; + + origin_ = obs.origin_; + cloud_handle_ = obs.cloud_handle_; + cloud_ = cloud_handle_.get(); + obstacle_range_ = obs.obstacle_range_; + raytrace_range_ = obs.raytrace_range_; + return *this; + } + + Observation& operator=(Observation&& obs) noexcept + { + if (this == &obs) + return *this; + + origin_ = std::move(obs.origin_); + cloud_handle_ = std::move(obs.cloud_handle_); + cloud_ = cloud_handle_.get(); + obstacle_range_ = obs.obstacle_range_; + raytrace_range_ = obs.raytrace_range_; + obs.cloud_ = nullptr; + return *this; + } + /** * @brief Creates an observation from a point cloud * @param cloud The point cloud of the observation * @param obstacle_range The range out to which an observation should be able to insert obstacles */ Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) : - cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0) + cloud_handle_(boost::make_shared(cloud)), + cloud_(cloud_handle_.get()), obstacle_range_(obstacle_range), raytrace_range_(0.0) { } robot_geometry_msgs::Point origin_; + boost::shared_ptr cloud_handle_; robot_sensor_msgs::PointCloud2* cloud_; double obstacle_range_, raytrace_range_; }; diff --git a/include/robot_costmap_2d/observation_buffer.h b/include/robot_costmap_2d/observation_buffer.h index 081afec..da16f3e 100755 --- a/include/robot_costmap_2d/observation_buffer.h +++ b/include/robot_costmap_2d/observation_buffer.h @@ -109,6 +109,8 @@ public: */ void bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth); + void bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth); + /** * @brief Pushes copies of all current observations onto the end of the vector passed in * @param observations The vector to be filled diff --git a/include/robot_costmap_2d/obstacle_layer.h b/include/robot_costmap_2d/obstacle_layer.h index 0bc9366..2b43f0e 100755 --- a/include/robot_costmap_2d/obstacle_layer.h +++ b/include/robot_costmap_2d/obstacle_layer.h @@ -134,7 +134,7 @@ protected: /** * @brief Buffer a depth image and its camera model for frustum clearing. */ - void depthImageCallback(const robot_sensor_msgs::DepthCameraData& message, + void depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message, const boost::shared_ptr& buffer); /** diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index 85bca7c..1601dc4 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -96,9 +96,12 @@ private: double* min_x, double* min_y, double* max_x, double* max_y); bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v, double& depth_m, bool& is_valid) const; + void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step, + double fx, double fy, double cx, double cy); bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz); bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz, - double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y); + double raytrace_range, unsigned int cell_raytrace_range, + double* min_x, double* min_y, double* max_x, double* max_y); bool publish_voxel_; @@ -106,6 +109,25 @@ private: double z_resolution_, origin_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_; robot_sensor_msgs::PointCloud clearing_endpoints_; + std::vector rolling_costmap_scratch_; + std::vector rolling_voxel_scratch_; + + struct DepthRay + { + unsigned int u; + unsigned int v; + double x; + double y; + double z; + }; + std::vector depth_ray_cache_; + unsigned int cached_depth_width_ = 0; + unsigned int cached_depth_height_ = 0; + unsigned int cached_depth_pixel_step_ = 0; + double cached_fx_ = 0.0; + double cached_fy_ = 0.0; + double cached_cx_ = 0.0; + double cached_cy_ = 0.0; inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz) { diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index 05c797b..0d43141 100755 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -58,7 +58,6 @@ InflationLayer::InflationLayer() , inflate_unknown_(false) , cell_inflation_radius_(0) , cached_cell_inflation_radius_(0) - , seen_(NULL) , cached_costs_(NULL) , cached_distances_(NULL) , last_min_x_(-std::numeric_limits::max()) @@ -76,10 +75,8 @@ void InflationLayer::onInitialize() boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); current_ = true; - if (seen_) - delete[] seen_; - seen_ = NULL; - seen_size_ = 0; + seen_.clear(); + seen_generation_ = 0; need_reinflation_ = false; std::string config_file_name = "inflation_layer_params.yaml"; // std::cout << "InflationLayer: " << config_file_name << std::endl; @@ -144,10 +141,8 @@ void InflationLayer::matchSize() computeCaches(); unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY(); - if (seen_) - delete[] seen_; - seen_size_ = size_x * size_y; - seen_ = new bool[seen_size_]; + seen_.assign(static_cast(size_x) * size_y, 0); + seen_generation_ = 0; } void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, @@ -203,26 +198,28 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m if (cell_inflation_radius_ == 0) return; - // make sure the inflation list is empty at the beginning of the cycle (should always be true) - if(!inflation_cells_.empty()) - robot::log_error("The inflation list must be empty at the beginning of inflation\n"); + for (std::vector& cells : inflation_cells_) + cells.clear(); unsigned char* master_array = master_grid.getCharMap(); unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); - if (seen_ == NULL) { - robot::log_error("InflationLayer::updateCosts(): seen_ array is NULL\n"); - seen_size_ = size_x * size_y; - seen_ = new bool[seen_size_]; - } - else if (seen_size_ != size_x * size_y) + const std::size_t map_size = static_cast(size_x) * size_y; + if (seen_.size() != map_size) { - robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n"); - delete[] seen_; - seen_size_ = size_x * size_y; - seen_ = new bool[seen_size_]; + seen_.assign(map_size, 0); + seen_generation_ = 0; + } + + if (seen_generation_ == std::numeric_limits::max()) + { + std::fill(seen_.begin(), seen_.end(), 0); + seen_generation_ = 1; + } + else + { + ++seen_generation_; } - memset(seen_, false, size_x * size_y * sizeof(bool)); // We need to include in the inflation cells outside the bounding // box min_i...max_j, by the amount cell_inflation_radius_. Cells @@ -238,11 +235,13 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m max_i = std::min(int(size_x), max_i); max_j = std::min(int(size_y), max_j); - // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle - // We use a map to emulate the priority queue used before, with a notable performance boost + // Precomputed distance buckets preserve priority ordering without a tree lookup + // for every enqueued cell. // Start with lethal obstacles: by definition distance is 0.0 - std::vector& obs_bin = inflation_cells_[0.0]; + if (inflation_cells_.empty()) + return; + std::vector& obs_bin = inflation_cells_.front(); for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) @@ -258,23 +257,22 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they // can overtake previously inserted but farther away cells - std::map >::iterator bin; - for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) + for (std::vector& bin : inflation_cells_) { - for (int i = 0; i < bin->second.size(); ++i) + for (std::size_t i = 0; i < bin.size(); ++i) { // process all cells at distance dist_bin.first - const CellData& cell = bin->second[i]; + const CellData& cell = bin[i]; unsigned int index = cell.index_; // ignore if already visited - if (seen_[index]) + if (seen_[index] == seen_generation_) { continue; } - seen_[index] = true; + seen_[index] = seen_generation_; unsigned int mx = cell.x_; unsigned int my = cell.y_; @@ -301,7 +299,6 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m } } - inflation_cells_.clear(); } /** @@ -316,7 +313,7 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y) { - if (!seen_[index]) + if (seen_[index] != seen_generation_) { // we compute our distance table one cell further than the inflation radius dictates so we can make the check below double distance = distanceLookup(mx, my, src_x, src_y); @@ -325,8 +322,10 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne if (distance > cell_inflation_radius_) return; - // push the cell data onto the inflation list and mark - inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); + const unsigned int dx = std::abs(static_cast(mx) - static_cast(src_x)); + const unsigned int dy = std::abs(static_cast(my) - static_cast(src_y)); + const unsigned int bin_index = distance_bin_lookup_[dx * distance_lookup_size_ + dy]; + inflation_cells_[bin_index].push_back(CellData(index, mx, my, src_x, src_y)); } } @@ -354,6 +353,38 @@ void InflationLayer::computeCaches() } cached_cell_inflation_radius_ = cell_inflation_radius_; + + distance_lookup_size_ = cell_inflation_radius_ + 2; + distance_levels_.clear(); + for (unsigned int i = 0; i < distance_lookup_size_; ++i) + { + for (unsigned int j = 0; j < distance_lookup_size_; ++j) + { + if (cached_distances_[i][j] <= cell_inflation_radius_) + distance_levels_.push_back(cached_distances_[i][j]); + } + } + std::sort(distance_levels_.begin(), distance_levels_.end()); + distance_levels_.erase( + std::unique(distance_levels_.begin(), distance_levels_.end()), distance_levels_.end()); + + inflation_cells_.clear(); + inflation_cells_.resize(distance_levels_.size()); + distance_bin_lookup_.assign( + static_cast(distance_lookup_size_) * distance_lookup_size_, 0); + for (unsigned int i = 0; i < distance_lookup_size_; ++i) + { + for (unsigned int j = 0; j < distance_lookup_size_; ++j) + { + const double distance = cached_distances_[i][j]; + if (distance > cell_inflation_radius_) + continue; + distance_bin_lookup_[i * distance_lookup_size_ + j] = + static_cast( + std::lower_bound(distance_levels_.begin(), distance_levels_.end(), distance) - + distance_levels_.begin()); + } + } } for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) @@ -367,6 +398,10 @@ void InflationLayer::computeCaches() void InflationLayer::deleteKernels() { + inflation_cells_.clear(); + distance_levels_.clear(); + distance_bin_lookup_.clear(); + distance_lookup_size_ = 0; if (cached_distances_ != NULL) { for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 4fcfdf3..0277ca0 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -269,10 +269,11 @@ void ObstacleLayer::handleImpl(const void* data, const std::type_info& type, const std::string& topic) { - if(!stop_receiving_data_) + if (!enabled_ || stop_receiving_data_) + return; + + if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr)) { - if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) ) - { const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr = *static_cast(data); if (!depth_camera_data_ptr) @@ -333,7 +334,8 @@ void ObstacleLayer::handleImpl(const void* data, // std::lock_guard lock(depth_camera_data_mutex_); // pending_depth_camera_data_ = depth_camera_data_ptr; - if(depth_observation_buffers_.empty() || callback_depth_infos_.empty()) return; + if (depth_observation_buffers_.empty() || callback_depth_infos_.empty()) + return; int size_callback_depth = static_cast(callback_depth_infos_.size()); for(int i = 0; i < size_callback_depth; i++) @@ -343,17 +345,17 @@ void ObstacleLayer::handleImpl(const void* data, topic == callback_depth_infos_[i].topic) { // robot::log_error_throttle(1.0,"TEST"); - depthImageCallback(depth_camera_data, buffer); + depthImageCallback(depth_camera_data_ptr, buffer); } } - // return; - } - else - { - if(observation_buffers_.empty() || callback_infos_.empty()) return; - + } + else + { + if (observation_buffers_.empty() || callback_infos_.empty()) + return; + int size_callback = static_cast(callback_infos_.size()); - for(int i = 0; i < size_callback; i++) + for (int i = 0; i < size_callback; i++) { boost::shared_ptr& buffer = observation_buffers_[i]; @@ -403,12 +405,6 @@ void ObstacleLayer::handleImpl(const void* data, // << "topic check: " << callback_infos_[i].topic << std::endl << std::endl; // } } - } - } - else - { - robot::log_info("Stop receiving data!\n"); - return; } } @@ -510,12 +506,11 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me buffer->unlock(); } -void ObstacleLayer::depthImageCallback(const robot_sensor_msgs::DepthCameraData& message, +void ObstacleLayer::depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message, const boost::shared_ptr& buffer) { buffer->lock(); - // robot::log_error_throttle(1.0, "depth data size 1: %d", (int)message.depth.data.size()); - buffer->bufferDepthCamera(message); + buffer->bufferDepthCamera(std::move(message)); buffer->unlock(); } @@ -557,6 +552,9 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya robot_sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); robot_sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); + std::size_t rejected_height = 0; + std::size_t rejected_range = 0; + std::size_t rejected_bounds = 0; for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { double px = *iter_x, py = *iter_y, pz = *iter_z; @@ -564,7 +562,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { - robot::log_error("The point is too high\n"); + ++rejected_height; continue; } @@ -575,7 +573,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya // if the point is far enough away... we won't consider it if (sq_dist >= sq_obstacle_range) { - robot::log_error("The point is too far away\n"); + ++rejected_range; continue; } @@ -583,7 +581,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { - robot::log_error("Computing map coords failed\n"); + ++rejected_bounds; continue; } @@ -591,6 +589,14 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya costmap_[index] = LETHAL_OBSTACLE; touch(px, py, min_x, min_y, max_x, max_y); } + + if (rejected_height + rejected_range + rejected_bounds > 0) + { + robot::log_info_throttle( + 5.0, + "ObstacleLayer filtered points: height=%zu range=%zu outside_map=%zu\n", + rejected_height, rejected_range, rejected_bounds); + } } updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index b0f9821..f21d910 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -456,6 +456,43 @@ bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned return false; } +void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height, + unsigned int pixel_step, double fx, double fy, + double cx, double cy) +{ + if (cached_depth_width_ == width && cached_depth_height_ == height && + cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy && + cached_cx_ == cx && cached_cy_ == cy) + { + return; + } + + cached_depth_width_ = width; + cached_depth_height_ = height; + cached_depth_pixel_step_ = pixel_step; + cached_fx_ = fx; + cached_fy_ = fy; + cached_cx_ = cx; + cached_cy_ = cy; + + const std::size_t rows = (height + pixel_step - 1) / pixel_step; + const std::size_t columns = (width + pixel_step - 1) / pixel_step; + depth_ray_cache_.clear(); + depth_ray_cache_.reserve(rows * columns); + + for (unsigned int v = 0; v < height; v += pixel_step) + { + for (unsigned int u = 0; u < width; u += pixel_step) + { + const double x = (static_cast(u) - cx) / fx; + const double y = (static_cast(v) - cy) / fy; + const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0); + depth_ray_cache_.push_back( + DepthRay{u, v, x * inverse_norm, y * inverse_norm, inverse_norm}); + } + } +} + bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz) { double a = wx - ox; @@ -494,7 +531,8 @@ bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& w } bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz, - double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y) + double raytrace_range, unsigned int cell_raytrace_range, + double* min_x, double* min_y, double* max_x, double* max_y) { double sensor_x, sensor_y, sensor_z; if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) @@ -509,7 +547,7 @@ bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, doubl robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, - cellDistance(raytrace_range)); + cell_raytrace_range); updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y); return true; } @@ -583,56 +621,60 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, const double skip_dist = 2.0 * resolution_; const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width); const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height); + updateDepthRayCache(width, height, step, fx, fy, cx, cy); + + double qx = tfm.transform.rotation.x; + double qy = tfm.transform.rotation.y; + double qz = tfm.transform.rotation.z; + double qw = tfm.transform.rotation.w; + const double quaternion_norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw); + if (quaternion_norm <= 0.0) + return false; + qx /= quaternion_norm; + qy /= quaternion_norm; + qz /= quaternion_norm; + qw /= quaternion_norm; + + const double r00 = 1.0 - 2.0 * (qy * qy + qz * qz); + const double r01 = 2.0 * (qx * qy - qz * qw); + const double r02 = 2.0 * (qx * qz + qy * qw); + const double r10 = 2.0 * (qx * qy + qz * qw); + const double r11 = 1.0 - 2.0 * (qx * qx + qz * qz); + const double r12 = 2.0 * (qy * qz - qx * qw); + const double r20 = 2.0 * (qx * qz - qy * qw); + const double r21 = 2.0 * (qy * qz + qx * qw); + const double r22 = 1.0 - 2.0 * (qx * qx + qy * qy); + const unsigned int cell_raytrace_range = cellDistance(max_range); bool cleared_any = false; - for (unsigned int v = 0; v < height; v += step) + for (const DepthRay& local_ray : depth_ray_cache_) { - for (unsigned int u = 0; u < width; u += step) - { - double depth_m = 0.0; - bool valid = false; - if (!readDepthMeters(depth, u, v, depth_m, valid)) - continue; + double depth_m = 0.0; + bool valid = false; + if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid)) + continue; - double ray_len = max_range; - if (valid && depth_m < max_range) - ray_len = std::max(0.0, depth_m - skip_dist); + double ray_len = max_range; + if (valid && depth_m < max_range) + ray_len = std::max(0.0, depth_m - skip_dist); - if (ray_len <= min_range) - continue; + if (ray_len <= min_range) + continue; - double dx = (static_cast(u) - cx) / fx; - double dy = (static_cast(v) - cy) / fy; - double dz = 1.0; - const double norm = std::sqrt(dx * dx + dy * dy + dz * dz); - if (norm <= 0.0) - continue; + robot_geometry_msgs::Vector3 global_ray; + global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z; + global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z; + global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z; - robot_geometry_msgs::Vector3 local_ray; - local_ray.x = dx / norm; - local_ray.y = dy / norm; - local_ray.z = dz / norm; + const double sx = ox + global_ray.x * min_range; + const double sy = oy + global_ray.y * min_range; + const double sz = oz + global_ray.z * min_range; + const double wx = ox + global_ray.x * ray_len; + const double wy = oy + global_ray.y * ray_len; + const double wz = oz + global_ray.z * ray_len; - robot_geometry_msgs::Vector3 global_ray; - tf3::doTransform(local_ray, global_ray, tfm); - const double global_norm = - std::sqrt(global_ray.x * global_ray.x + global_ray.y * global_ray.y + global_ray.z * global_ray.z); - if (global_norm <= 0.0) - continue; - - global_ray.x /= global_norm; - global_ray.y /= global_norm; - global_ray.z /= global_norm; - - const double sx = ox + global_ray.x * min_range; - const double sy = oy + global_ray.y * min_range; - const double sz = oz + global_ray.z * min_range; - const double wx = ox + global_ray.x * ray_len; - const double wy = oy + global_ray.y * ray_len; - const double wz = oz + global_ray.z * ray_len; - - cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, min_x, min_y, max_x, max_y) || cleared_any; - } + cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, cell_raytrace_range, + min_x, min_y, max_x, max_y) || cleared_any; } return cleared_any; @@ -645,6 +687,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) cell_ox = int((new_origin_x - origin_x_) / resolution_); cell_oy = int((new_origin_y - origin_y_) / resolution_); + // Most update cycles do not cross a costmap cell boundary. Avoid copying and + // resetting the complete 2D/3D grids when the cell-aligned origin is unchanged. + if (cell_ox == 0 && cell_oy == 0) + return; + // compute the associated world coordinates for the origin cell // beacuase we want to keep things grid-aligned double new_grid_ox, new_grid_oy; @@ -665,15 +712,20 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) unsigned int cell_size_x = upper_right_x - lower_left_x; unsigned int cell_size_y = upper_right_y - lower_left_y; - // we need a map to store the obstacles in the window temporarily - unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; - unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y]; + const std::size_t overlap_size = static_cast(cell_size_x) * cell_size_y; + rolling_costmap_scratch_.resize(overlap_size); + rolling_voxel_scratch_.resize(overlap_size); + unsigned char* local_map = rolling_costmap_scratch_.data(); + unsigned int* local_voxel_map = rolling_voxel_scratch_.data(); unsigned int* voxel_map = robot_voxel_grid_.getData(); - // copy the local window in the costmap to the local map - copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); - copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x, - cell_size_y); + if (overlap_size > 0) + { + copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, + cell_size_x, cell_size_x, cell_size_y); + copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, + cell_size_x, cell_size_x, cell_size_y); + } // we'll reset our maps to unknown space if appropriate resetMaps(); @@ -687,12 +739,14 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) int start_y = lower_left_y - cell_oy; // now we want to copy the overlapping information back into the map, but in its new location - copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); - copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y); + if (overlap_size > 0) + { + copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, + size_x_, cell_size_x, cell_size_y); + copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, + size_x_, cell_size_x, cell_size_y); + } - // make sure to clean up - delete[] local_map; - delete[] local_voxel_map; } // Export factory function diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp index e5d7575..134cb57 100755 --- a/src/costmap_2d.cpp +++ b/src/costmap_2d.cpp @@ -288,11 +288,17 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) unsigned int cell_size_x = upper_right_x - lower_left_x; unsigned int cell_size_y = upper_right_y - lower_left_y; - // we need a map to store the obstacles in the window temporarily - unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; + const std::size_t overlap_size = static_cast(cell_size_x) * cell_size_y; - // copy the local window in the costmap to the local map - copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); + // Reuse the temporary window to avoid allocating on every rolling-window shift. + rolling_window_scratch_.resize(overlap_size); + unsigned char* local_map = rolling_window_scratch_.data(); + + if (overlap_size > 0) + { + copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, + cell_size_x, cell_size_x, cell_size_y); + } // now we'll set the costmap to be completely unknown if we track unknown space resetMaps(); @@ -306,10 +312,12 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) int start_y = lower_left_y - cell_oy; // now we want to copy the overlapping information back into the map, but in its new location - copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); + if (overlap_size > 0) + { + copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, + size_x_, cell_size_x, cell_size_y); + } - // make sure to clean up - delete[] local_map; } bool Costmap2D::setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value) diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 2d8a5ef..a04bd9e 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -155,9 +155,20 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::st if (priv_nh.hasParam("track_unknown_space")) priv_nh.getParam("track_unknown_space", track_unknown_space); + bool performance_metrics_enabled = + loadParam(layer, "performance_metrics_enabled", false); + double performance_metrics_period = + loadParam(layer, "performance_metrics_period", 5.0); + if (priv_nh.hasParam("performance_metrics_enabled")) + priv_nh.getParam("performance_metrics_enabled", performance_metrics_enabled); + if (priv_nh.hasParam("performance_metrics_period")) + priv_nh.getParam("performance_metrics_period", performance_metrics_period); + if (priv_nh.hasParam("library_path")) path_plugins = loader.findLibraryPath(name_); layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space); + layered_costmap_->setPerformanceMetrics( + performance_metrics_enabled, performance_metrics_period); // find size parameters double map_width_meters = loadParam(layer, "width", 0.0); @@ -692,35 +703,9 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) // get the global pose of the robot try { - // use current time if possible (makes sure it's not in the future) - if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time())) - { - tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time()); - tf3::doTransform(robot_pose, global_pose, transform); - // robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(), - // global_pose.pose.position.x, - // global_pose.pose.position.y, - // global_pose.pose.position.z, - // global_pose.pose.orientation.x, - // global_pose.pose.orientation.y, - // global_pose.pose.orientation.z, - // global_pose.pose.orientation.w); - // transform.transform.rotation.x, - // transform.transform.rotation.y, - // transform.transform.rotation.z, - // transform.transform.rotation.w); - } - // use the latest otherwise - else - { - // tf_.transform(robot_pose, global_pose, global_frame_); - tf3::TransformStampedMsg transform = tf_.lookupTransform( - global_frame_, // frame đích - robot_base_frame_, // frame nguồn - tf3::Time() - ); - tf3::doTransform(robot_pose, global_pose, transform); - } + const tf3::TransformStampedMsg transform = + tf_.lookupTransform(global_frame_, robot_base_frame_, tf3::Time()); + tf3::doTransform(robot_pose, global_pose, transform); } catch (tf3::LookupException& ex) { diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp index 92b0297..815d364 100755 --- a/src/layered_costmap.cpp +++ b/src/layered_costmap.cpp @@ -69,6 +69,68 @@ namespace robot_costmap_2d costmap_.setDefaultValue(NO_INFORMATION); else costmap_.setDefaultValue(FREE_SPACE); + performance_window_start_ = std::chrono::steady_clock::now(); + } + + void LayeredCostmap::setPerformanceMetrics(bool enabled, double reporting_period_seconds) + { + performance_metrics_enabled_ = enabled; + performance_metrics_period_seconds_ = reporting_period_seconds > 0.0 ? reporting_period_seconds : 5.0; + resetPerformanceMetrics(); + } + + void LayeredCostmap::resetPerformanceMetrics() + { + performance_window_start_ = std::chrono::steady_clock::now(); + performance_cycle_nanoseconds_ = 0; + performance_reset_nanoseconds_ = 0; + performance_cycles_ = 0; + performance_cycle_samples_.clear(); + performance_cycle_samples_.reserve(128); + layer_performance_.assign(plugins_.size(), LayerPerformance()); + } + + void LayeredCostmap::maybeReportPerformance() + { + if (!performance_metrics_enabled_ || performance_cycles_ == 0) + return; + + const auto now = std::chrono::steady_clock::now(); + const double elapsed = std::chrono::duration(now - performance_window_start_).count(); + if (elapsed < performance_metrics_period_seconds_) + return; + + const double average_cycle_ms = + static_cast(performance_cycle_nanoseconds_) / performance_cycles_ / 1.0e6; + const double average_reset_ms = + static_cast(performance_reset_nanoseconds_) / performance_cycles_ / 1.0e6; + std::sort(performance_cycle_samples_.begin(), performance_cycle_samples_.end()); + const auto percentile_ms = [this](double percentile) { + if (performance_cycle_samples_.empty()) + return 0.0; + const std::size_t index = static_cast( + percentile * static_cast(performance_cycle_samples_.size() - 1)); + return static_cast(performance_cycle_samples_[index]) / 1.0e6; + }; + robot::log_info( + "Costmap performance: cycles=%llu avg_cycle_ms=%.3f p95_cycle_ms=%.3f " + "p99_cycle_ms=%.3f avg_reset_ms=%.3f\n", + static_cast(performance_cycles_), average_cycle_ms, + percentile_ms(0.95), percentile_ms(0.99), average_reset_ms); + + for (std::size_t i = 0; i < plugins_.size() && i < layer_performance_.size(); ++i) + { + const LayerPerformance& stats = layer_performance_[i]; + const double average_bounds_ms = stats.bounds_calls == 0 ? 0.0 : + static_cast(stats.bounds_nanoseconds) / stats.bounds_calls / 1.0e6; + const double average_costs_ms = stats.costs_calls == 0 ? 0.0 : + static_cast(stats.costs_nanoseconds) / stats.costs_calls / 1.0e6; + robot::log_info( + "Costmap layer [%s]: avg_bounds_ms=%.3f avg_costs_ms=%.3f\n", + plugins_[i]->getName().c_str(), average_bounds_ms, average_costs_ms); + } + + resetPerformanceMetrics(); } LayeredCostmap::~LayeredCostmap() @@ -94,6 +156,8 @@ namespace robot_costmap_2d void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { + const auto cycle_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() : + std::chrono::steady_clock::time_point(); // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) // implement thread unsafe updateBounds() functions. boost::unique_lock lock(*(costmap_.getMutex())); @@ -111,23 +175,35 @@ namespace robot_costmap_2d minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; - for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) + if (performance_metrics_enabled_ && layer_performance_.size() != plugins_.size()) + layer_performance_.assign(plugins_.size(), LayerPerformance()); + + for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index) { - if (!(*plugin)->isEnabled()) + const boost::shared_ptr& plugin = plugins_[plugin_index]; + if (!plugin->isEnabled()) continue; double prev_minx = minx_; double prev_miny = miny_; double prev_maxx = maxx_; double prev_maxy = maxy_; - (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); + const auto bounds_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() : + std::chrono::steady_clock::time_point(); + plugin->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); + if (performance_metrics_enabled_) + { + layer_performance_[plugin_index].bounds_nanoseconds += + std::chrono::duration_cast( + std::chrono::steady_clock::now() - bounds_start).count(); + ++layer_performance_[plugin_index].bounds_calls; + } if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n", prev_minx, prev_miny, prev_maxx, prev_maxy, minx_, miny_, maxx_, maxy_, - (*plugin)->getName().c_str()); + plugin->getName().c_str()); } } @@ -143,13 +219,32 @@ namespace robot_costmap_2d if (xn < x0 || yn < y0) return; + const auto reset_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() : + std::chrono::steady_clock::time_point(); costmap_.resetMap(x0, y0, xn, yn); - - for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) + if (performance_metrics_enabled_) { - if ((*plugin)->isEnabled()) - (*plugin)->updateCosts(costmap_, x0, y0, xn, yn); + performance_reset_nanoseconds_ += + std::chrono::duration_cast( + std::chrono::steady_clock::now() - reset_start).count(); + } + + for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index) + { + const boost::shared_ptr& plugin = plugins_[plugin_index]; + if (!plugin->isEnabled()) + continue; + + const auto costs_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() : + std::chrono::steady_clock::time_point(); + plugin->updateCosts(costmap_, x0, y0, xn, yn); + if (performance_metrics_enabled_) + { + layer_performance_[plugin_index].costs_nanoseconds += + std::chrono::duration_cast( + std::chrono::steady_clock::now() - costs_start).count(); + ++layer_performance_[plugin_index].costs_calls; + } } bx0_ = x0; @@ -158,6 +253,17 @@ namespace robot_costmap_2d byn_ = yn; initialized_ = true; + + if (performance_metrics_enabled_) + { + const std::uint64_t cycle_nanoseconds = static_cast( + std::chrono::duration_cast( + std::chrono::steady_clock::now() - cycle_start).count()); + performance_cycle_nanoseconds_ += cycle_nanoseconds; + performance_cycle_samples_.push_back(cycle_nanoseconds); + ++performance_cycles_; + maybeReportPerformance(); + } } bool LayeredCostmap::isCurrent() diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index ae69447..6baad2d 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -40,6 +40,8 @@ #include #include +#include + using namespace std; using namespace tf3; @@ -97,6 +99,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) { Observation& obs = *obs_it; + if (!obs.cloud_handle_.unique()) + { + obs.cloud_handle_ = boost::make_shared(*obs.cloud_); + obs.cloud_ = obs.cloud_handle_.get(); + } + robot_geometry_msgs::PointStamped origin; origin.header.frame_id = global_frame_; origin.header.stamp = data_convert::convertTime(transform_time); @@ -137,9 +145,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud) { robot_geometry_msgs::PointStamped global_origin; - - // create a new observation on the list to be populated - observation_list_.push_front(Observation()); + Observation observation; // check whether the origin frame has been set explicitly or whether we should get it from the cloud string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; @@ -154,81 +160,68 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud) local_origin.point.y = 0; local_origin.point.z = 0; // tf3_buffer_.transform(local_origin, global_origin, global_frame_); - tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform( - global_frame_, // frame đích - local_origin.header.frame_id, // frame nguồn - tf3::Time() - // data_convert::convertTime(cloud.header.stamp) - ); - tf3::doTransform(local_origin, global_origin, tfm_1); + const tf3::TransformStampedMsg cloud_transform = tf3_buffer_.lookupTransform( + global_frame_, cloud.header.frame_id, tf3::Time()); + if (origin_frame == cloud.header.frame_id) + tf3::doTransform(local_origin, global_origin, cloud_transform); + else + tf3::doTransform( + local_origin, global_origin, + tf3_buffer_.lookupTransform(global_frame_, origin_frame, tf3::Time())); - ///////////////////////////////////////////////// - ///////////chú ý hàm này///////////////////////// - tf3::convert(global_origin.point, observation_list_.front().origin_); - ///////////////////////////////////////////////// - ///////////////////////////////////////////////// + tf3::convert(global_origin.point, observation.origin_); + observation.raytrace_range_ = raytrace_range_; + observation.obstacle_range_ = obstacle_range_; - // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations - observation_list_.front().raytrace_range_ = raytrace_range_; - observation_list_.front().obstacle_range_ = obstacle_range_; + robot_sensor_msgs::PointCloud2& observation_cloud = *observation.cloud_; + tf3::doTransform(cloud, observation_cloud, cloud_transform); + observation_cloud.header.stamp = cloud.header.stamp; - robot_sensor_msgs::PointCloud2 global_frame_cloud; + const std::size_t cloud_size = + static_cast(observation_cloud.height) * observation_cloud.width; + const std::size_t point_step = observation_cloud.point_step; + std::size_t point_count = 0; + robot_sensor_msgs::PointCloud2Iterator iter_z(observation_cloud, "z"); - // transform the point cloud - // tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_); - tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform( - global_frame_, // frame đích - cloud.header.frame_id, // frame nguồn - tf3::Time() - // data_convert::convertTime(cloud.header.stamp) - ); - tf3::doTransform(cloud, global_frame_cloud, tfm_2); - global_frame_cloud.header.stamp = cloud.header.stamp; - - // now we need to remove observations from the cloud that are below or above our height thresholds - robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_); - observation_cloud.height = global_frame_cloud.height; - observation_cloud.width = global_frame_cloud.width; - observation_cloud.fields = global_frame_cloud.fields; - observation_cloud.is_bigendian = global_frame_cloud.is_bigendian; - observation_cloud.point_step = global_frame_cloud.point_step; - observation_cloud.row_step = global_frame_cloud.row_step; - observation_cloud.is_dense = global_frame_cloud.is_dense; - - unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width; - robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud); - modifier.resize(cloud_size); - unsigned int point_count = 0; - - // copy over the points that are within our height bounds - robot_sensor_msgs::PointCloud2Iterator iter_z(global_frame_cloud, "z"); - std::vector::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end(); - std::vector::iterator iter_obs = observation_cloud.data.begin(); - for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step) + // Compact accepted points in-place. This avoids allocating and copying a + // second full-size filtered cloud after the TF transform. + for (std::size_t read_index = 0; read_index < cloud_size; ++read_index, ++iter_z) { - if ((*iter_z) <= max_obstacle_height_ - && (*iter_z) >= min_obstacle_height_) + if ((*iter_z) > max_obstacle_height_ || (*iter_z) < min_obstacle_height_) + continue; + + if (point_count != read_index) { - std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs); - iter_obs += global_frame_cloud.point_step; - ++point_count; + std::memmove(observation_cloud.data.data() + point_count * point_step, + observation_cloud.data.data() + read_index * point_step, + point_step); } + ++point_count; } - // resize the cloud for the number of legal points - modifier.resize(point_count); - observation_cloud.header.stamp = cloud.header.stamp; - observation_cloud.header.frame_id = global_frame_cloud.header.frame_id; + if (point_count != cloud_size) + { + robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud); + modifier.resize(point_count); + } } catch (TransformException& ex) { - // if an exception occurs, we need to remove the empty observation from the list - observation_list_.pop_front(); robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); return; } + if (observation_keep_time_ == robot::Duration(0.0) && !observation_list_.empty()) + { + observation_list_.front() = std::move(observation); + observation_list_.erase(++observation_list_.begin(), observation_list_.end()); + } + else + { + observation_list_.push_front(std::move(observation)); + } + // if the update was successful, we want to update the last updated time last_updated_ = robot::Time::now(); @@ -238,21 +231,28 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud) void ObservationBuffer::bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth_camera_data) { - depth_observation_list_.push_front(DepthCameraObservation()); - if (depth_observation_list_.front().data_ == nullptr) + bufferDepthCamera(boost::make_shared(depth_camera_data)); +} + +void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth_camera_data) +{ + if (!depth_camera_data) + return; + + DepthCameraObservation observation( + std::move(depth_camera_data), topic_name_, robot::Time::now(), + frustum_pixel_step_, frustum_min_range_, frustum_max_range_); + + if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty()) { - depth_observation_list_.front().data_ = - new robot_sensor_msgs::DepthCameraData(depth_camera_data); + depth_observation_list_.front() = std::move(observation); + depth_observation_list_.erase(++depth_observation_list_.begin(), depth_observation_list_.end()); } else { - *depth_observation_list_.front().data_ = depth_camera_data; + depth_observation_list_.push_front(std::move(observation)); } - depth_observation_list_.front().pixel_step_ = frustum_pixel_step_; - depth_observation_list_.front().min_range_ = frustum_min_range_; - depth_observation_list_.front().max_range_ = frustum_max_range_; - // if the update was successful, we want to update the last updated time last_updated_ = robot::Time::now(); @@ -280,11 +280,18 @@ void ObservationBuffer::getDepthObservations(vector& obs purgeStaleDepthObservations(); // now we'll just copy the observations for the caller - list::iterator obs_it; - for (obs_it = depth_observation_list_.begin(); obs_it != depth_observation_list_.end(); ++obs_it) + if (observation_keep_time_ == robot::Duration(0.0)) { - observations.push_back(*obs_it); + if (!depth_observation_list_.empty()) + { + observations.push_back(std::move(depth_observation_list_.front())); + depth_observation_list_.clear(); + } + return; } + + observations.insert( + observations.end(), depth_observation_list_.begin(), depth_observation_list_.end()); } void ObservationBuffer::purgeStaleObservations() @@ -356,4 +363,3 @@ void ObservationBuffer::resetLastUpdated() last_updated_ = robot::Time::now(); } } // namespace robot_costmap_2d - diff --git a/test/coordinates_test.cpp b/test/coordinates_test.cpp index c1e2cc9..3da7ff5 100644 --- a/test/coordinates_test.cpp +++ b/test/coordinates_test.cpp @@ -36,6 +36,14 @@ #include #include +#include +#include +#include +#include +#include + +#include +#include using namespace robot_costmap_2d; @@ -124,9 +132,100 @@ TEST(CostmapCoordinates, hard_coordinates_test) EXPECT_EQ(my, 2); } +TEST(CostmapPerformanceRegression, rolling_origin_preserves_overlap) +{ + Costmap2D costmap(4, 3, 1.0, 0.0, 0.0, FREE_SPACE); + costmap.setCost(1, 1, LETHAL_OBSTACLE); + costmap.setCost(3, 2, INSCRIBED_INFLATED_OBSTACLE); + + costmap.updateOrigin(0.25, 0.25); + EXPECT_DOUBLE_EQ(costmap.getOriginX(), 0.0); + EXPECT_DOUBLE_EQ(costmap.getOriginY(), 0.0); + EXPECT_EQ(costmap.getCost(1, 1), LETHAL_OBSTACLE); + + costmap.updateOrigin(1.0, 0.0); + EXPECT_DOUBLE_EQ(costmap.getOriginX(), 1.0); + EXPECT_EQ(costmap.getCost(0, 1), LETHAL_OBSTACLE); + EXPECT_EQ(costmap.getCost(3, 2), FREE_SPACE); +} + +TEST(CostmapPerformanceRegression, voxel_origin_subcell_shift_is_noop) +{ + VoxelLayer layer; + layer.resizeMap(4, 3, 1.0, 0.0, 0.0); + layer.setCost(1, 1, LETHAL_OBSTACLE); + + layer.updateOrigin(0.25, 0.25); + + EXPECT_DOUBLE_EQ(layer.getOriginX(), 0.0); + EXPECT_DOUBLE_EQ(layer.getOriginY(), 0.0); + EXPECT_EQ(layer.getCost(1, 1), LETHAL_OBSTACLE); +} + +TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload) +{ + robot_geometry_msgs::Point origin; + robot_sensor_msgs::PointCloud2 cloud; + cloud.height = 1; + cloud.width = 1; + cloud.point_step = 4; + cloud.row_step = 4; + cloud.data = {1, 2, 3, 4}; + + Observation observation(origin, cloud, 2.5, 3.0); + Observation copied = observation; + + EXPECT_EQ(copied.cloud_, observation.cloud_); + EXPECT_EQ(copied.cloud_handle_.use_count(), 2); + EXPECT_EQ(copied.cloud_->data, cloud.data); +} + +TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once) +{ + tf3::BufferCore tf_buffer(tf3::Duration(10.0)); + ObservationBuffer buffer( + "/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0, + 8, 0.2, 3.0, tf_buffer, "odom", "", 0.2); + + robot_sensor_msgs::DepthCameraData::ConstPtr depth = + boost::make_shared(); + buffer.bufferDepthCamera(depth); + + std::vector first_snapshot; + buffer.getDepthObservations(first_snapshot); + ASSERT_EQ(first_snapshot.size(), 1u); + EXPECT_EQ(first_snapshot.front().data_, depth.get()); + EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data"); + + std::vector second_snapshot; + buffer.getDepthObservations(second_snapshot); + EXPECT_TRUE(second_snapshot.empty()); +} + +TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs) +{ + ASSERT_EQ(setenv("PNKX_NAV_CORE_CONFIG_DIR", ROBOT_COSTMAP_2D_DIR, 1), 0); + + LayeredCostmap layered_costmap("map", false, false); + layered_costmap.resizeMap(7, 7, 1.0, 0.0, 0.0, true); + tf3::BufferCore tf_buffer(tf3::Duration(10.0)); + InflationLayer inflation; + inflation.initialize(&layered_costmap, "inflation", &tf_buffer); + inflation.setInflationParameters(2.0, 1.0); + + Costmap2D& master = *layered_costmap.getCostmap(); + master.setCost(3, 3, LETHAL_OBSTACLE); + inflation.updateCosts(master, 0, 0, 7, 7); + + EXPECT_EQ(master.getCost(3, 3), LETHAL_OBSTACLE); + EXPECT_EQ(master.getCost(2, 3), master.getCost(4, 3)); + EXPECT_EQ(master.getCost(3, 2), master.getCost(3, 4)); + EXPECT_GT(master.getCost(4, 3), master.getCost(5, 3)); + EXPECT_EQ(master.getCost(6, 3), FREE_SPACE); +} + int main(int argc, char** argv) { testing::InitGoogleTest( &argc, argv ); return RUN_ALL_TESTS(); } -