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

otimal

Browse Source
This commit is contained in:
duongtd
2026-07-23 16:12:55 +07:00
parent 0e84ac53cb
commit c888af3b7c
7 changed files with 170 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/costmap_2d_robot.cpp
View File

@@ -400,6 +400,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
int frustum_clearing_pixel_step = 8;
double frustum_min_range = 0.2;
double frustum_max_range = 3.0;
double frustum_skip_distance = -1.0;
bool frustum_column_clearing = false;
double column_clear_min_height = 0.10;
double column_clear_max_height = -1.0;
double column_skip_distance = 0.02;
double column_cover_distance = -1.0;
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -417,6 +423,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_skip_distance", frustum_skip_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_column_clearing", frustum_column_clearing);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_min_height", column_clear_min_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
}
@@ -459,6 +471,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
int frustum_clearing_pixel_step = 8;
double frustum_min_range = 0.2;
double frustum_max_range = 3.0;
double frustum_skip_distance = -1.0;
bool frustum_column_clearing = false;
double column_clear_min_height = 0.10;
double column_clear_max_height = -1.0;
double column_skip_distance = 0.02;
double column_cover_distance = -1.0;
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -476,6 +494,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_skip_distance", frustum_skip_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_column_clearing", frustum_column_clearing);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_min_height", column_clear_min_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
}

15
src/observation_buffer.cpp
View File

@@ -60,19 +60,19 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, unsigned int frustum_pixel_step,
double frustum_min_range, double frustum_max_range,
double raytrace_range, const DepthFrustumConfig& frustum_config,
tf3::BufferCore& tf3_buffer, string global_frame,
string sensor_frame, double tf_tolerance) :
tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range),
frustum_pixel_step_(std::max(1u, frustum_pixel_step)),
frustum_min_range_(std::max(0.0, frustum_min_range)),
frustum_max_range_(std::max(frustum_max_range, frustum_min_range_)),
tf_tolerance_(tf_tolerance)
tf_tolerance_(tf_tolerance),
frustum_config_(frustum_config)
{
frustum_config_.pixel_step = std::max(1u, frustum_config_.pixel_step);
frustum_config_.min_range = std::max(0.0, frustum_config_.min_range);
frustum_config_.max_range = std::max(frustum_config_.max_range, frustum_config_.min_range);
}
ObservationBuffer::~ObservationBuffer()
@@ -240,8 +240,7 @@ void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::Co
return;
DepthCameraObservation observation(
std::move(depth_camera_data), topic_name_, robot::Time::now(),
frustum_pixel_step_, frustum_min_range_, frustum_max_range_);
std::move(depth_camera_data), topic_name_, robot::Time::now(), frustum_config_);
if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty())
{