From a263fd180b45de7d5c8e7314590537cc18f2efb7 Mon Sep 17 00:00:00 2001
From: idelpino <telecosg@gmail.com>
Date: Thu, 25 Apr 2024 18:56:57 +0200
Subject: [PATCH] Fixed bug, now detects low obstacles both to left and right
---
src/ground_segmentation_alg_node.cpp | 14 +++++++-------
1 file changed, 7 insertions(+), 7 deletions(-)
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index c05b933..2319839 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -652,10 +652,10 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
std::cout << "Starting detectLowObstacles function!" << std::endl;
float res = res_deg * M_PI / 180.0;
float r_max = h / alpha;
- float K = 1 / sin(res);
+ float K = 1.0 / sin(res);
float d_max = K * r_max;
- float cell_side = r_max / sqrt(8);
- int num_of_cells = (d_max * 2 / cell_side);
+ float cell_side = r_max / sqrt(8.0);
+ int num_of_cells = (d_max * 2.0 / cell_side);
std::cout << "r_max = " << r_max << std::endl;
std::cout << "K = " << K << std::endl;
@@ -670,8 +670,8 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
if ((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
{
//std::cout << "Analyzing ground point!" << std::endl;
- int x = i->x / cell_side;
- int y = i->y / cell_side;
+ int x = i->x / cell_side + num_of_cells / 2.0;
+ int y = i->y / cell_side + num_of_cells / 2.0;
//std::cout << "x cell number = " << x << " y cell number = " << y << std::endl;
if (x >= 0 && x < num_of_cells && y >= 0 && y < num_of_cells)
{
@@ -718,8 +718,8 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
{
if ((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
{
- int x = i->x / cell_side;
- int y = i->y / cell_side;
+ int x = i->x / cell_side + num_of_cells / 2.0;
+ int y = i->y / cell_side + num_of_cells / 2.0;
if (x >= 0 && x < num_of_cells && y >= 0 && y < num_of_cells)
{
if (grid[x][y].label == 1)
--
GitLab