DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

otimal deep coppy obj

Browse Source
This commit is contained in:
duongtd
2026-07-14 11:08:23 +07:00
parent 6a9834d3a8
commit bdbb03aa51
17 changed files with 702 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/robot_costmap_2d/costmap_2d.h
View File

@@ -425,6 +425,7 @@ protected:
double origin_y_;
unsigned char* costmap_;
unsigned char default_value_;
std::vector<unsigned char> rolling_window_scratch_;
class MarkCell
{

15
include/robot_costmap_2d/inflation_layer.h
View File

@@ -42,6 +42,9 @@
#include <robot_costmap_2d/layered_costmap.h>
#include <boost/thread.hpp>
#include <cstdint>
#include <vector>
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<double, std::vector<CellData> > inflation_cells_;
std::vector<std::vector<CellData>> inflation_cells_;
std::vector<double> distance_levels_;
std::vector<unsigned int> distance_bin_lookup_;
unsigned int distance_lookup_size_ = 0;
bool* seen_;
int seen_size_;
std::vector<std::uint32_t> seen_;
std::uint32_t seen_generation_ = 0;
unsigned char** cached_costs_;
double** cached_distances_;

24
include/robot_costmap_2d/layered_costmap.h
View File

@@ -43,6 +43,8 @@
#include <robot_costmap_2d/costmap_2d.h>
#include <vector>
#include <string>
#include <chrono>
#include <cstdint>
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<robot_geometry_msgs::Point> 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<std::uint64_t> performance_cycle_samples_;
std::vector<LayerPerformance> layer_performance_;
};
} // namespace robot_costmap_2d

157
include/robot_costmap_2d/observation.h
View File

@@ -35,6 +35,9 @@
#include <robot_geometry_msgs/Point.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/DepthCameraData.h>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <utility>
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<robot_sensor_msgs::DepthCameraData>(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<robot_sensor_msgs::PointCloud2>()),
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<robot_sensor_msgs::PointCloud2>(cloud)),
cloud_(cloud_handle_.get()),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
{
}
Observation(robot_geometry_msgs::Point origin,
boost::shared_ptr<robot_sensor_msgs::PointCloud2> 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<robot_sensor_msgs::PointCloud2>(cloud)),
cloud_(cloud_handle_.get()), obstacle_range_(obstacle_range), raytrace_range_(0.0)
{
}
robot_geometry_msgs::Point origin_;
boost::shared_ptr<robot_sensor_msgs::PointCloud2> cloud_handle_;
robot_sensor_msgs::PointCloud2* cloud_;
double obstacle_range_, raytrace_range_;
};

2
include/robot_costmap_2d/observation_buffer.h
View File

@@ -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

2
include/robot_costmap_2d/obstacle_layer.h
View File

@@ -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<robot_costmap_2d::ObservationBuffer>& buffer);
/**

24
include/robot_costmap_2d/voxel_layer.h
View File

@@ -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<unsigned char> rolling_costmap_scratch_;
std::vector<unsigned int> rolling_voxel_scratch_;
struct DepthRay
{
unsigned int u;
unsigned int v;
double x;
double y;
double z;
};
std::vector<DepthRay> 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)
{