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

22
src/costmap_2d.cpp
View File

@@ -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<std::size_t>(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<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)

43
src/costmap_2d_robot.cpp
View File

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

126
src/layered_costmap.cpp
View File

@@ -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<double>(now - performance_window_start_).count();
if (elapsed < performance_metrics_period_seconds_)
return;
const double average_cycle_ms =
static_cast<double>(performance_cycle_nanoseconds_) / performance_cycles_ / 1.0e6;
const double average_reset_ms =
static_cast<double>(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<std::size_t>(
percentile * static_cast<double>(performance_cycle_samples_.size() - 1));
return static_cast<double>(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<unsigned long long>(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<double>(stats.bounds_nanoseconds) / stats.bounds_calls / 1.0e6;
const double average_costs_ms = stats.costs_calls == 0 ? 0.0 :
static_cast<double>(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<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
@@ -111,23 +175,35 @@ namespace robot_costmap_2d
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
for (vector<boost::shared_ptr<Layer>>::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<Layer>& 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::nanoseconds>(
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<boost::shared_ptr<Layer>>::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::nanoseconds>(
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<Layer>& 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::nanoseconds>(
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::uint64_t>(
std::chrono::duration_cast<std::chrono::nanoseconds>(
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()

154
src/observation_buffer.cpp
View File

@@ -40,6 +40,8 @@
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <cstring>
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<robot_sensor_msgs::PointCloud2>(*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<std::size_t>(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<float> 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<float> iter_z(global_frame_cloud, "z");
std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
std::vector<unsigned char>::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<robot_sensor_msgs::DepthCameraData>(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<DepthCameraObservation>& obs
purgeStaleDepthObservations();
// now we'll just copy the observations for the caller
list<DepthCameraObservation>::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