fixes

Cardinal-Space-Mining · Apr 21, 2024 · 3ad18e1 · 3ad18e1
1 parent 5c523cf
commit 3ad18e1
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 9 deletions.
diff --git 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
@@ -157,6 +157,7 @@ void PerceptionNode::trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSh
 
 
 void PerceptionNode::process(const pcl::PointCloud<pcl::PointXYZ>& 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<pcl::PointXYZ>& 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