Skip to content

Commit

Permalink
refactor:Put params into namespace #1
Browse files Browse the repository at this point in the history
  • Loading branch information
@dobaybalazs
dobaybalazs committed Feb 26, 2022
1 parent 1d40417 commit 37b94b8
Show file tree
Hide file tree
Showing 7 changed files with 129 additions and 128 deletions.
43 changes: 22 additions & 21 deletions include/urban_road_filter/data_structures.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,27 +63,28 @@ struct box //struct for detection beams
float o, d; //internal parameters (trigonometry)
};

extern std::string fixedFrame; /* Fixed Frame.*/
extern std::string topicName; /* subscribed topic.*/
extern bool x_zero_method, z_zero_method, star_shaped_method ; /*Methods of roadside detection*/
extern bool blind_spots; /*Vakfolt jav铆t贸 algoritmus.*/
extern int xDirection; /*A vakfolt lev谩g谩s milyen ir谩ny煤.*/
extern float interval; /*A LIDAR vertik谩lis sz枚gfelbont谩s谩nak, elfogadott intervalluma.*/
extern float curbHeight; /*Becs眉lt minimum szeg茅ly magass谩g.*/
extern int curbPoints; /*A pontok becs眉lt sz谩ma, a szeg茅lyen.*/
extern float beamZone; /*A vizsg谩lt sug谩rz贸na m茅rete.*/
extern float angleFilter1; /*X = 0 茅rt茅k mellett, h谩rom pont 谩ltal bez谩rt sz枚g.*/
extern float angleFilter2; /*Z = 0 茅rt茅k mellett, k茅t vektor 谩ltal bez谩rt sz枚g.*/
extern float angleFilter3; /*Csapl谩r L谩szl贸 k贸dj谩hoz sz眉ks茅ges. Sug谩r ir谩ny煤 hat谩r茅rt茅k (fokban).*/
extern float min_X, max_X, min_Y, max_Y, min_Z, max_Z; /*A vizsg谩lt ter眉let m茅retei.*/
extern int dmin_param; //(see below)
extern float kdev_param; //(see below)
extern float kdist_param; //(see below)
extern bool polysimp_allow; /*polygon-eygszer疟s铆t茅s enged茅lyez茅se*/
extern bool zavg_allow; /*egyszer疟s铆tett polygon z-koordin谩t谩i 谩tlagb贸l (enged茅ly)*/
extern float polysimp; /*polygon-egyszer疟s铆t茅si t茅nyez艖 (Ramer-Douglas-Peucker)*/
extern float polyz; /*manu谩lisan megadott z-koordin谩ta (polygon)*/

namespace params{
extern std::string fixedFrame; /* Fixed Frame.*/
extern std::string topicName; /* subscribed topic.*/
extern bool x_zero_method, z_zero_method, star_shaped_method ; /*Methods of roadside detection*/
extern bool blind_spots; /*Vakfolt jav铆t贸 algoritmus.*/
extern int xDirection; /*A vakfolt lev谩g谩s milyen ir谩ny煤.*/
extern float interval; /*A LIDAR vertik谩lis sz枚gfelbont谩s谩nak, elfogadott intervalluma.*/
extern float curbHeight; /*Becs眉lt minimum szeg茅ly magass谩g.*/
extern int curbPoints; /*A pontok becs眉lt sz谩ma, a szeg茅lyen.*/
extern float beamZone; /*A vizsg谩lt sug谩rz贸na m茅rete.*/
extern float angleFilter1; /*X = 0 茅rt茅k mellett, h谩rom pont 谩ltal bez谩rt sz枚g.*/
extern float angleFilter2; /*Z = 0 茅rt茅k mellett, k茅t vektor 谩ltal bez谩rt sz枚g.*/
extern float angleFilter3; /*Csapl谩r L谩szl贸 k贸dj谩hoz sz眉ks茅ges. Sug谩r ir谩ny煤 hat谩r茅rt茅k (fokban).*/
extern float min_X, max_X, min_Y, max_Y, min_Z, max_Z; /*A vizsg谩lt ter眉let m茅retei.*/
extern int dmin_param; //(see below)
extern float kdev_param; //(see below)
extern float kdist_param; //(see below)
extern bool polysimp_allow; /*polygon-eygszer疟s铆t茅s enged茅lyez茅se*/
extern bool zavg_allow; /*egyszer疟s铆tett polygon z-koordin谩t谩i 谩tlagb贸l (enged茅ly)*/
extern float polysimp; /*polygon-egyszer疟s铆t茅si t茅nyez艖 (Ramer-Douglas-Peucker)*/
extern float polyz; /*manu谩lisan megadott z-koordin谩ta (polygon)*/
};
/*For pointcloud filtering*/
template <typename PointT>
class FilteringCondition : public pcl::ConditionBase<PointT>
Expand Down
36 changes: 19 additions & 17 deletions src/blind_spots.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
#include "urban_road_filter/data_structures.hpp"

bool blind_spots; /*Vakfolt jav铆t贸 algoritmus.*/
int params::xDirection; /*A vakfolt lev谩g谩s milyen ir谩ny煤.*/
bool params::blind_spots; /*Vakfolt jav铆t贸 algoritmus.*/
float params::beamZone; /*A vizsg谩lt sug谩rz贸na m茅rete.*/

void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,int* indexArray,float* maxDistance){
/*Vakfolt keres茅s:
Expand All @@ -12,7 +14,7 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
int c1 = -1, c2 = -1, c3 = -1, c4 = -1; /*A tal谩lt pontok ID-ja az els艖 k枚rvonalon.*/
int i,j,k,l; /*Seg茅d v谩ltoz贸k*/

if (blind_spots)
if (params::blind_spots)
{
for (int i = 0; i < indexArray[1]; i++)
{
Expand Down Expand Up @@ -60,25 +62,25 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
float currentDegree; /*Az aktu谩lis k枚r铆ven, a sz枚g nagys谩ga.*/

/*A k枚r铆v m茅ret meghat谩roz谩sa.*/
arcDistance = ((maxDistance[0] * M_PI) / 180) * beamZone;
arcDistance = ((maxDistance[0] * M_PI) / 180) * params::beamZone;

/*0掳-t贸l 360掳 - beamZone-ig.*/
for (int i = 0; i <= 360 - beamZone; i++)
for (int i = 0; i <= 360 - params::beamZone; i++)
{
blindSpot = 0;

if (blind_spots)
if (params::blind_spots)
{
/*Ha ezek a felt茅telek teljes眉lnek, akkor egy vakfoltba l茅pt眉nk 茅s itt nem vizsg谩l贸dunk.*/
if (xDirection == 0)
if (params::xDirection == 0)
{
/*+-X ir谩nyba is vizsg谩ljuk a pontokat.*/
if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3))
{
blindSpot = 1;
}
}
else if (xDirection == 1)
else if (params::xDirection == 1)
{
/*+X ir谩nyba vizsg谩ljuk a pontokat.*/
if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270)))
Expand All @@ -102,7 +104,7 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
notRoad = 0;

/*Az els艖 k枚r adott szakasz谩nak vizsg谩lata.*/
for (j = 0; array3D[0][j].alpha <= i + beamZone && j < indexArray[0]; j++)
for (j = 0; array3D[0][j].alpha <= i + params::beamZone && j < indexArray[0]; j++)
{
if (array3D[0][j].alpha >= i)
{
Expand All @@ -119,7 +121,7 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
if (notRoad == 0)
{
/*Az els艖 k枚r szakasz谩t elfogadjuk.*/
for (j = 0; array3D[0][j].alpha <= i + beamZone && j < indexArray[0]; j++)
for (j = 0; array3D[0][j].alpha <= i + params::beamZone && j < indexArray[0]; j++)
{
if (array3D[0][j].alpha >= i)
{
Expand All @@ -131,7 +133,7 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
for (k = 1; k < index; k++)
{
/*脷j sz枚get kell meghat谩rozni, hogy a t谩volabbi k枚rvonalakon is, ugyanakkora k枚r铆v hosszt vizsg谩ljunk.*/
if (i == 360 - beamZone)
if (i == 360 - params::beamZone)
{
currentDegree = 360;
}
Expand Down Expand Up @@ -172,22 +174,22 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
}

/*Ugyanaz, mint az el艖z艖, csak itt 360掳-t贸l 0掳 + beamZone-ig vizsg谩ljuk a pontokat.*/
for (i = 360; i >= 0 + beamZone; --i)
for (i = 360; i >= 0 + params::beamZone; --i)
{
blindSpot = 0;

if (blind_spots)
if (params::blind_spots)
{
/*Ha ezek a felt茅telek teljes眉lnek, akkor egy vakfoltba l茅pt眉nk 茅s itt nem vizsg谩l贸dunk.*/
if (xDirection == 0)
if (params::xDirection == 0)
{
/*+-X ir谩nyba is vizsg谩ljuk a pontokat.*/
if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3))
{
blindSpot = 1;
}
}
else if (xDirection == 1)
else if (params::xDirection == 1)
{
/*+X ir谩nyba vizsg谩ljuk a pontokat.*/
if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270)))
Expand All @@ -211,7 +213,7 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
notRoad = 0;

/*Az els艖 k枚r adott szakasz谩nak vizsg谩lata.*/
for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - beamZone && j >= 0; --j)
for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - params::beamZone && j >= 0; --j)
{
if (array3D[0][j].alpha <= i)
{
Expand All @@ -228,7 +230,7 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
if (notRoad == 0)
{
/*Az els艖 k枚r szakasz谩t elfogadjuk.*/
for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - beamZone && j >= 0; --j)
for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - params::beamZone && j >= 0; --j)
{
if (array3D[0][j].alpha <= i)
{
Expand All @@ -240,7 +242,7 @@ void Detector::blindSpots(std::vector<std::vector<Point3D>>& array3D,int index,i
for (k = 1; k < index; k++)
{
/*脷j sz枚get kell meghat谩rozni, hogy a t谩volabbi k枚rvonalakon is, ugyanakkora k枚r铆v hosszt vizsg谩ljunk.*/
if (i == 0 + beamZone)
if (i == 0 + params::beamZone)
{
currentDegree = 0;
}
Expand Down
66 changes: 31 additions & 35 deletions src/lidar_segmentation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,27 +2,23 @@

/*Global variables.*/
int channels = 64; /* The number of channels of the LIDAR .*/
std::string fixedFrame; /* Fixed Frame.*/
std::string topicName; /* subscribed topic.*/
bool x_zero_method, z_zero_method, star_shaped_method ; /*Methods of roadside detection*/
int xDirection; /*A vakfolt lev谩g谩s milyen ir谩ny煤.*/
float interval; /*A LIDAR vertik谩lis sz枚gfelbont谩s谩nak, elfogadott intervalluma.*/
float curbHeight; /*Becs眉lt minimum szeg茅ly magass谩g.*/
int curbPoints; /*A pontok becs眉lt sz谩ma, a szeg茅lyen.*/
float beamZone; /*A vizsg谩lt sug谩rz贸na m茅rete.*/
float min_X, max_X, min_Y, max_Y, min_Z, max_Z; /*A vizsg谩lt ter眉let m茅retei.*/

bool polysimp_allow = true; /*polygon-eygszer疟s铆t茅s enged茅lyez茅se*/
bool zavg_allow = true; /*egyszer疟s铆tett polygon z-koordin谩t谩i 谩tlagb贸l (enged茅ly)*/
float polysimp = 0.5; /*polygon-egyszer疟s铆t茅si t茅nyez艖 (Ramer-Douglas-Peucker)*/
float polyz = -1.5; /*manu谩lisan megadott z-koordin谩ta (polygon)*/
std::string params::fixedFrame; /* Fixed Frame.*/
std::string params::topicName; /* subscribed topic.*/
bool params::x_zero_method, params::z_zero_method, params::star_shaped_method ; /*Methods of roadside detection*/
float params::interval; /*A LIDAR vertik谩lis sz枚gfelbont谩s谩nak, elfogadott intervalluma.*/
float params::min_X, params::max_X, params::min_Y, params::max_Y, params::min_Z, params::max_Z; /*A vizsg谩lt ter眉let m茅retei.*/

bool params::polysimp_allow = true; /*polygon-eygszer疟s铆t茅s enged茅lyez茅se*/
bool params::zavg_allow = true; /*egyszer疟s铆tett polygon z-koordin谩t谩i 谩tlagb贸l (enged茅ly)*/
float params::polysimp = 0.5; /*polygon-egyszer疟s铆t茅si t茅nyez艖 (Ramer-Douglas-Peucker)*/
float params::polyz = -1.5; /*manu谩lisan megadott z-koordin谩ta (polygon)*/

int ghostcount = 0; /* seg茅dv谩ltoz贸 az elavult markerek (ghost) elt谩vol铆t谩s谩hoz */


Detector::Detector(ros::NodeHandle* nh){
/*Feliratkoz谩s az adott topicra.*/
sub = nh->subscribe(topicName, 1, &Detector::filtered,this);
sub = nh->subscribe(params::topicName, 1, &Detector::filtered,this);
/*A sz疟rt adatok h铆rdet茅se.*/
pub_road = nh->advertise<pcl::PCLPointCloud2>("road", 1);
pub_high = nh->advertise<pcl::PCLPointCloud2>("curb", 1);
Expand Down Expand Up @@ -74,9 +70,9 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){

auto filterCondition = boost::make_shared<FilteringCondition<pcl::PointXYZI>>(
[=](const pcl::PointXYZI& point){
return point.x >= min_X && point.x <= max_X &&
point.y >= min_Y && point.y <= max_Y &&
point.z >= min_Z && point.z <= max_Z &&
return point.x >= params::min_X && point.x <= params::max_X &&
point.y >= params::min_Y && point.y <= params::max_Y &&
point.z >= params::min_Z && point.z <= params::max_Z &&
point.x + point.y + point.z != 0;
}
);
Expand Down Expand Up @@ -152,7 +148,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
if (angle[j] == 0)
break;

if (abs(angle[j] - array2D[i].alpha) <= interval)
if (abs(angle[j] - array2D[i].alpha) <= params::interval)
{
newCircle = 0;
break;
Expand All @@ -172,7 +168,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
}
}
/*Csapl谩r L谩szl贸 k贸dj谩nak megh铆v谩sa 茅s a sz眉ks茅ges hat谩r茅rt茅k be谩ll铆t谩sa.*/
if (star_shaped_method )
if (params::star_shaped_method )
Detector::starShapedSearch(array2D);


Expand Down Expand Up @@ -210,7 +206,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
/*A megfelel艖 k枚r铆v kiv谩laszt谩sa.*/
for (j = 0; j < index; j++)
{
if (abs(angle[j] - array2D[i].alpha) <= interval)
if (abs(angle[j] - array2D[i].alpha) <= params::interval)
{
results = 1;
break;
Expand All @@ -223,7 +219,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
array3D[j][indexArray[j]].p = array2D[i].p;

/*A m谩r ismert magaspontok.*/
if (star_shaped_method )
if (params::star_shaped_method )
array3D[j][indexArray[j]].isCurbPoint = array2D[i].isCurbPoint;

/*Itt annyi k眉l枚nbs茅g lesz, hogy a "z" 茅rt茅k n茅lk眉l adjuk hozz谩 a t谩vols谩got.*/
Expand Down Expand Up @@ -262,9 +258,9 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
}
}

if(x_zero_method)
if(params::x_zero_method)
Detector::xZeroMethod(array3D,index,indexArray);
if(z_zero_method)
if(params::z_zero_method)
Detector::zZeroMethod(array3D,index,indexArray);

float d;
Expand Down Expand Up @@ -400,7 +396,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){

int lineStripID = 0; /*Az adott line strip ID-ja.*/

line_strip.header.frame_id = fixedFrame;
line_strip.header.frame_id = params::fixedFrame;
line_strip.header.stamp = ros::Time();
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
line_strip.action = visualization_msgs::Marker::ADD;
Expand Down Expand Up @@ -465,17 +461,17 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
line_strip.color.b = 0.0;
}

if (polysimp_allow)
if (params::polysimp_allow)
{
line_strip.points.clear();
boost::geometry::clear(simplified);
boost::geometry::simplify(line, simplified, polysimp);
boost::geometry::simplify(line, simplified, params::polysimp);
for(boost::geometry::model::linestring<xy>::const_iterator it = simplified.begin(); it != simplified.end(); it++)
{
geometry_msgs::Point p;
p.x = boost::geometry::get<0>(*it);
p.y = boost::geometry::get<1>(*it);
p.z = polyz;
p.z = params::polyz;

line_strip.points.push_back(p);
}
Expand Down Expand Up @@ -516,17 +512,17 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
line_strip.color.g = 0.0;
line_strip.color.b = 0.0;

if (polysimp_allow)
if (params::polysimp_allow)
{
line_strip.points.clear();
boost::geometry::clear(simplified);
boost::geometry::simplify(line, simplified, polysimp);
boost::geometry::simplify(line, simplified, params::polysimp);
for(boost::geometry::model::linestring<xy>::const_iterator it = simplified.begin(); it != simplified.end(); it++)
{
geometry_msgs::Point p;
p.x = boost::geometry::get<0>(*it);
p.y = boost::geometry::get<1>(*it);
p.z = polyz;
p.z = params::polyz;

line_strip.points.push_back(p);
}
Expand Down Expand Up @@ -566,17 +562,17 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
line_strip.color.g = 1.0;
line_strip.color.b = 0.0;

if (polysimp_allow)
if (params::polysimp_allow)
{
line_strip.points.clear();
boost::geometry::clear(simplified);
boost::geometry::simplify(line, simplified, polysimp);
boost::geometry::simplify(line, simplified, params::polysimp);
for(boost::geometry::model::linestring<xy>::const_iterator it = simplified.begin(); it != simplified.end(); it++)
{
geometry_msgs::Point p;
p.x = boost::geometry::get<0>(*it);
p.y = boost::geometry::get<1>(*it);
p.z = polyz;
p.z = params::polyz;

line_strip.points.push_back(p);
}
Expand All @@ -602,7 +598,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud){
}
line_strip.lifetime = ros::Duration(0);
}
if (zavg_allow)
if (params::zavg_allow)
{
for (int seg=0; seg < ma.markers.size(); seg++)
{
Expand Down
Loading

0 comments on commit 37b94b8

Please sign in to comment.