From 3ad18e10df3cd2d1ac6f187d0d9a332a1b2bf228 Mon Sep 17 00:00:00 2001 From: Sam Richter Date: Sat, 20 Apr 2024 23:18:12 -0500 Subject: [PATCH] fixes --- src/ldrp/grid.hpp | 3 ++- src/ldrp/perception.cpp | 17 +++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/ldrp/grid.hpp b/src/ldrp/grid.hpp index 97d3fb2..302783e 100644 --- a/src/ldrp/grid.hpp +++ b/src/ldrp/grid.hpp @@ -179,7 +179,8 @@ class GridBase { _min = this->boundingLoc(min.x(), min.y()), // grid cell locations containing min and max, aligned with current offsets _max = this->boundingLoc(max.x(), max.y()); - if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { + // if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { + if( (_min.x() < _zero.x() || _min.y() < _zero.y()) || (_max.x() > this->grid_size.x() || _max.y() > this->grid_size.y()) ) { const Vec2i _low = _min.cwiseMin(_zero), // new high and low bounds for the map _high = _max.cwiseMax(this->grid_size) + _one, // need to add an additional row + col since indexing happens in the corner, thus by default the difference between corners will not cover the entire size diff --git a/src/ldrp/perception.cpp b/src/ldrp/perception.cpp index 1f1b0a9..c960628 100644 --- a/src/ldrp/perception.cpp +++ b/src/ldrp/perception.cpp @@ -157,6 +157,7 @@ void PerceptionNode::trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSh void PerceptionNode::process(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin) { + RCLCPP_INFO(this->get_logger(), "Processing...\n"); static const pcl::Indices DEFAULT_NO_SELECTION = pcl::Indices{}; @@ -174,15 +175,15 @@ void PerceptionNode::process(const pcl::PointCloud& cloud, const #if LDRP_ENABLE_TUNING float - max_pmf_range_m = (float) this->get_parameter("max_pmf_range_m").as_double() * 1e-2f, - max_z_thresh_m = (float) this->get_parameter("max_z_thresh_m").as_double() * 1e-2f, - min_z_thresh_m = (float) this->get_parameter("min_z_thresh_m").as_double() * 1e-2f, - voxel_size_m = (float) this->get_parameter("voxel_size_m").as_double() * 1e-2f, + max_pmf_range_m = (float) this->get_parameter("max_pmf_range_cm").as_double() * 1e-2f, + max_z_thresh_m = (float) this->get_parameter("max_z_thresh_cm").as_double() * 1e-2f, + min_z_thresh_m = (float) this->get_parameter("min_z_thresh_cm").as_double() * 1e-2f, + voxel_size_m = (float) this->get_parameter("voxel_size_cm").as_double() * 1e-2f, pmf_window_base = (float) this->get_parameter("pmf_window_base").as_double(), - pmf_max_window_size_m = (float) this->get_parameter("pmf_max_window_size_m").as_double() * 1e-2f, - pmf_cell_size_m = (float) this->get_parameter("pmf_cell_size_m").as_double() * 1e-2f, - pmf_init_distance_m = (float) this->get_parameter("pmf_init_distance_m").as_double() * 1e-2f, - pmf_max_distance_m = (float) this->get_parameter("pmf_max_distance_m").as_double() * 1e-2f, + pmf_max_window_size_m = (float) this->get_parameter("pmf_max_window_size_cm").as_double() * 1e-2f, + pmf_cell_size_m = (float) this->get_parameter("pmf_cell_size_cm").as_double() * 1e-2f, + pmf_init_distance_m = (float) this->get_parameter("pmf_init_distance_cm").as_double() * 1e-2f, + pmf_max_distance_m = (float) this->get_parameter("pmf_max_distance_cm").as_double() * 1e-2f, pmf_slope = (float) this->get_parameter("pmf_slope").as_double() ; #else