Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
I
iri_ground_segmentation
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Iván del Pino
iri_ground_segmentation
Commits
4ce32cd2
Commit
4ce32cd2
authored
3 years ago
by
Iván del Pino
Browse files
Options
Downloads
Patches
Plain Diff
renamed things to make the node more general (instead of velodyne, now using lidar)
parent
2ae90c88
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/ground_segmentation_alg_node.h
+16
-22
16 additions, 22 deletions
include/ground_segmentation_alg_node.h
src/ground_segmentation_alg_node.cpp
+46
-83
46 additions, 83 deletions
src/ground_segmentation_alg_node.cpp
with
62 additions
and
105 deletions
include/ground_segmentation_alg_node.h
+
16
−
22
View file @
4ce32cd2
...
...
@@ -43,37 +43,31 @@ class GroundSegmentationAlgNode : public algorithm_base::IriBaseAlgorithm<
GroundSegmentationAlgorithm
>
{
private:
std
::
string
input_pointcloud_topic_
;
std
::
string
output_pointcloud_segmented_topic_
;
std
::
string
output_roadmap_markers_topic_
;
std
::
string
output_roadmap_nodes_topic_
;
std
::
string
output_roadmap_edges_topic_
;
std_msgs
::
Header
common_header_
;
bool
flag_new_
velodyne
_data_
;
ros
::
Publisher
velodyne_re
publisher_
;
bool
flag_new_
lidar
_data_
;
ros
::
Publisher
lidar_points_ground_segmented_
publisher_
;
ros
::
Publisher
local_goal
s_publisher_
;
visualization_msgs
::
MarkerArray
local_goals
_MarkerArray_msg_
;
ros
::
Publisher
roadmap_marker
s_publisher_
;
visualization_msgs
::
MarkerArray
roadmap
_MarkerArray_msg_
;
// [subscriber attributes]
// Variables for copying the callbacks inputs
sensor_msgs
::
PointCloud2
velodyne
_ros_cloud_
;
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>
velodyne
_pcl_cloud_
;
sensor_msgs
::
PointCloud2
lidar
_ros_cloud_
;
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>
lidar
_pcl_cloud_
;
ros
::
Publisher
g
ro
und_referenc
es_publisher_
;
sensor_msgs
::
PointCloud2
g
ro
und_referenc
es_ros_cloud_
;
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>
g
ro
und_referenc
es_pcl_cloud_
;
ros
::
Publisher
ro
admap_nod
es_publisher_
;
sensor_msgs
::
PointCloud2
ro
admap_nod
es_ros_cloud_
;
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>
ro
admap_nod
es_pcl_cloud_
;
ros
::
Publisher
edges_publisher_
;
iri_ground_segmentation
::
edges_array
edges_array_
;
ros
::
Publisher
roadmap_
edges_publisher_
;
iri_ground_segmentation
::
edges_array
roadmap_
edges_array_
;
ros
::
Subscriber
velodyne
_subscriber_
;
void
cb_
velodyne
(
const
sensor_msgs
::
PointCloud2
::
ConstPtr
&
msg
);
ros
::
Subscriber
lidar
_subscriber_
;
void
cb_
lidar
(
const
sensor_msgs
::
PointCloud2
::
ConstPtr
&
msg
);
pthread_mutex_t
velodyne
_mutex_
;
void
velodyne
_mutex_enter
(
void
);
void
velodyne
_mutex_exit
(
void
);
pthread_mutex_t
lidar
_mutex_
;
void
lidar
_mutex_enter
(
void
);
void
lidar
_mutex_exit
(
void
);
void
rosPointCloudtoPCLPointCloud
(
const
sensor_msgs
::
PointCloud2
&
ros_point_cloud
,
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>&
pcl_point_cloud
);
...
...
This diff is collapsed.
Click to expand it.
src/ground_segmentation_alg_node.cpp
+
46
−
83
View file @
4ce32cd2
...
...
@@ -11,70 +11,33 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this
->
setRate
(
this
->
config_
.
rate
);
if
(
!
this
->
private_node_handle_
.
getParam
(
"input_pointcloud_topic"
,
this
->
config_
.
input_pointcloud_topic
))
{
ROS_WARN
(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'input_pointcloud_topic' not found"
);
}
else
input_pointcloud_topic_
=
config_
.
input_pointcloud_topic
;
if
(
!
this
->
private_node_handle_
.
getParam
(
"output_pointcloud_segmented_topic"
,
this
->
config_
.
output_pointcloud_segmented_topic
))
{
ROS_WARN
(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_pointcloud_segmented_topic' not found"
);
}
else
output_pointcloud_segmented_topic_
=
config_
.
output_pointcloud_segmented_topic
;
if
(
!
this
->
private_node_handle_
.
getParam
(
"output_roadmap_markers_topic"
,
this
->
config_
.
output_roadmap_markers_topic
))
{
ROS_WARN
(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_markers' not found"
);
}
else
output_roadmap_markers_topic_
=
config_
.
output_roadmap_markers_topic
;
if
(
!
this
->
private_node_handle_
.
getParam
(
"output_roadmap_nodes_topic"
,
this
->
config_
.
output_roadmap_nodes_topic
))
{
ROS_WARN
(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_nodes_topic' not found"
);
}
else
output_roadmap_nodes_topic_
=
config_
.
output_roadmap_nodes_topic
;
if
(
!
this
->
private_node_handle_
.
getParam
(
"output_roadmap_edges_topic"
,
this
->
config_
.
output_roadmap_edges_topic
))
{
ROS_WARN
(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_edges_topic' not found"
);
}
else
output_roadmap_edges_topic_
=
config_
.
output_roadmap_edges_topic
;
flag_new_velodyne_data_
=
false
;
flag_new_lidar_data_
=
false
;
// [init publishers]
this
->
velodyne_re
publisher_
=
this
->
private_node_handle_
.
advertise
<
sensor_msgs
::
PointCloud2
>
(
output
_point
cl
oud_segmented
_topic_
,
1
);
this
->
lidar_points_ground_segmented_
publisher_
=
this
->
private_node_handle_
.
advertise
<
sensor_msgs
::
PointCloud2
>
(
"lidar
_point
s_gr
ou
n
d_segmented
"
,
1
);
this
->
local_goal
s_publisher_
=
this
->
private_node_handle_
.
advertise
<
visualization_msgs
::
MarkerArray
>
(
output_
roadmap_markers
_topic_
,
1
);
this
->
roadmap_marker
s_publisher_
=
this
->
private_node_handle_
.
advertise
<
visualization_msgs
::
MarkerArray
>
(
"
roadmap_markers
"
,
1
);
this
->
g
ro
und_referenc
es_publisher_
=
this
->
private_node_handle_
.
advertise
<
sensor_msgs
::
PointCloud2
>
(
output_
roadmap_nodes
_topic_
,
1
);
this
->
ro
admap_nod
es_publisher_
=
this
->
private_node_handle_
.
advertise
<
sensor_msgs
::
PointCloud2
>
(
"
roadmap_nodes
"
,
1
);
this
->
edges_publisher_
=
this
->
private_node_handle_
.
advertise
<
iri_ground_segmentation
::
edges_array
>
(
output_
roadmap_edges
_topic_
,
1
);
this
->
roadmap_
edges_publisher_
=
this
->
private_node_handle_
.
advertise
<
iri_ground_segmentation
::
edges_array
>
(
"
roadmap_edges
"
,
1
);
// [init subscribers]
this
->
velodyne
_subscriber_
=
this
->
public_node_handle_
.
subscribe
(
input_pointcloud_topic_
,
1
,
&
GroundSegmentationAlgNode
::
cb_
velodyne
,
this
);
this
->
lidar
_subscriber_
=
this
->
public_node_handle_
.
subscribe
(
"/lidar_points_extended_structure"
,
1
,
&
GroundSegmentationAlgNode
::
cb_
lidar
,
this
);
pthread_mutex_init
(
&
this
->
velodyne
_mutex_
,
NULL
);
pthread_mutex_init
(
&
this
->
lidar
_mutex_
,
NULL
);
// [init services]
}
GroundSegmentationAlgNode
::~
GroundSegmentationAlgNode
(
void
)
{
this
->
showPerformanceStatistics
();
pthread_mutex_destroy
(
&
this
->
velodyne
_mutex_
);
pthread_mutex_destroy
(
&
this
->
lidar
_mutex_
);
}
void
GroundSegmentationAlgNode
::
rosPointCloudtoPCLPointCloud
(
const
sensor_msgs
::
PointCloud2
&
ros_point_cloud
,
...
...
@@ -95,19 +58,19 @@ void GroundSegmentationAlgNode::PCLPointCloudToRosPointCloud(
void
GroundSegmentationAlgNode
::
mutexAll
(
void
)
{
this
->
velodyne
_mutex_enter
();
this
->
lidar
_mutex_enter
();
}
void
GroundSegmentationAlgNode
::
unmutexAll
(
void
)
{
this
->
velodyne
_mutex_exit
();
this
->
lidar
_mutex_exit
();
}
bool
GroundSegmentationAlgNode
::
isAllRequiredDataAlreadyReceived
(
void
)
{
bool
ready_to_process
=
false
;
if
(
flag_new_
velodyne
_data_
)
if
(
flag_new_
lidar
_data_
)
{
ready_to_process
=
true
;
//std::cout << "GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived..." << std::endl;
...
...
@@ -118,46 +81,46 @@ bool GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived(void)
void
GroundSegmentationAlgNode
::
storeHeaderForSync
(
void
)
{
common_header_
=
velodyne
_ros_cloud_
.
header
;
common_header_
=
lidar
_ros_cloud_
.
header
;
}
void
GroundSegmentationAlgNode
::
convertInputsToSuitableFormats
(
void
)
{
//std::cout << "GroundSegmentationAlgNode::convertInputsToSuitableFormats..." << std::endl;
//std::cout << " 1) rosPointCloudtoPCLPointCloud with
velodyne
_ros_cloud_..." << std::endl;
this
->
rosPointCloudtoPCLPointCloud
(
velodyne
_ros_cloud_
,
velodyne
_pcl_cloud_
);
flag_new_
velodyne
_data_
=
false
;
//std::cout << " 1) rosPointCloudtoPCLPointCloud with
lidar
_ros_cloud_..." << std::endl;
this
->
rosPointCloudtoPCLPointCloud
(
lidar
_ros_cloud_
,
lidar
_pcl_cloud_
);
flag_new_
lidar
_data_
=
false
;
}
void
GroundSegmentationAlgNode
::
convertOutputsToSuitableFormats
(
void
)
{
//std::cout << "Extracted header = " << std::endl << common_header_;
//std::cout << "Original header = " << std::endl <<
velodyne
_ros_cloud_.header << std::endl;
//std::cout << "Original header = " << std::endl <<
lidar
_ros_cloud_.header << std::endl;
//std::cout << "Message header = " << std::endl << perimeters_ros_.header << std::endl;
this
->
PCLPointCloudToRosPointCloud
(
velodyne
_pcl_cloud_
,
velodyne
_ros_cloud_
);
velodyne
_ros_cloud_
.
header
=
common_header_
;
this
->
PCLPointCloudToRosPointCloud
(
lidar
_pcl_cloud_
,
lidar
_ros_cloud_
);
lidar
_ros_cloud_
.
header
=
common_header_
;
this
->
PCLPointCloudToRosPointCloud
(
g
ro
und_referenc
es_pcl_cloud_
,
g
ro
und_referenc
es_ros_cloud_
);
g
ro
und_referenc
es_ros_cloud_
.
header
=
common_header_
;
this
->
PCLPointCloudToRosPointCloud
(
ro
admap_nod
es_pcl_cloud_
,
ro
admap_nod
es_ros_cloud_
);
ro
admap_nod
es_ros_cloud_
.
header
=
common_header_
;
}
void
GroundSegmentationAlgNode
::
publishAll
(
void
)
{
//std::cout << "Starting GroundSegmentationAlgNode::publishAll.." << std::endl;
this
->
velodyne_re
publisher_
.
publish
(
this
->
velodyne
_ros_cloud_
);
this
->
g
ro
und_referenc
es_publisher_
.
publish
(
this
->
g
ro
und_referenc
es_ros_cloud_
);
this
->
local_goal
s_publisher_
.
publish
(
this
->
local_goals
_MarkerArray_msg_
);
this
->
edges_publisher_
.
publish
(
this
->
edges_array_
);
this
->
lidar_points_ground_segmented_
publisher_
.
publish
(
this
->
lidar
_ros_cloud_
);
this
->
ro
admap_nod
es_publisher_
.
publish
(
this
->
ro
admap_nod
es_ros_cloud_
);
this
->
roadmap_marker
s_publisher_
.
publish
(
this
->
roadmap
_MarkerArray_msg_
);
this
->
roadmap_
edges_publisher_
.
publish
(
this
->
roadmap_
edges_array_
);
}
void
GroundSegmentationAlgNode
::
prepareForNextIteration
(
void
)
{
velodyne
_pcl_cloud_
.
clear
();
g
ro
und_referenc
es_pcl_cloud_
.
clear
();
local_goals
_MarkerArray_msg_
.
markers
.
clear
();
edges_array_
.
edges_array
.
clear
();
lidar
_pcl_cloud_
.
clear
();
ro
admap_nod
es_pcl_cloud_
.
clear
();
roadmap
_MarkerArray_msg_
.
markers
.
clear
();
roadmap_
edges_array_
.
edges_array
.
clear
();
}
void
GroundSegmentationAlgNode
::
showPerformanceStatistics
(
void
)
...
...
@@ -180,8 +143,8 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
this
->
storeHeaderForSync
();
this
->
convertInputsToSuitableFormats
();
this
->
alg_
.
segmentGround
(
velodyne
_pcl_cloud_
,
g
ro
und_referenc
es_pcl_cloud_
,
local_goals
_MarkerArray_msg_
,
edges_array_
,
common_header_
);
this
->
alg_
.
segmentGround
(
lidar
_pcl_cloud_
,
ro
admap_nod
es_pcl_cloud_
,
roadmap
_MarkerArray_msg_
,
roadmap_edges_array_
,
common_header_
);
this
->
convertOutputsToSuitableFormats
();
this
->
publishAll
();
...
...
@@ -194,25 +157,25 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
/* [subscriber callbacks] */
void
GroundSegmentationAlgNode
::
cb_
velodyne
(
const
sensor_msgs
::
PointCloud2
::
ConstPtr
&
msg
)
void
GroundSegmentationAlgNode
::
cb_
lidar
(
const
sensor_msgs
::
PointCloud2
::
ConstPtr
&
msg
)
{
//this->
velodyne
_mutex_enter();
velodyne
_ros_cloud_
=
*
msg
;
//std::cout << "GroundSegmentationAlgNode::cb_
velodyne --> Velodyne
msg received!" << std::endl;
assert
(
msg
!=
NULL
&&
"Null pointer!!! in function cb_
velodyne
!"
);
//this->
lidar
_mutex_enter();
lidar
_ros_cloud_
=
*
msg
;
//std::cout << "GroundSegmentationAlgNode::cb_
lidar --> lidar
msg received!" << std::endl;
assert
(
msg
!=
NULL
&&
"Null pointer!!! in function cb_
lidar
!"
);
flag_new_
velodyne
_data_
=
true
;
//this->
velodyne
_mutex_exit();
flag_new_
lidar
_data_
=
true
;
//this->
lidar
_mutex_exit();
}
void
GroundSegmentationAlgNode
::
velodyne
_mutex_enter
(
void
)
void
GroundSegmentationAlgNode
::
lidar
_mutex_enter
(
void
)
{
pthread_mutex_lock
(
&
this
->
velodyne
_mutex_
);
pthread_mutex_lock
(
&
this
->
lidar
_mutex_
);
}
void
GroundSegmentationAlgNode
::
velodyne
_mutex_exit
(
void
)
void
GroundSegmentationAlgNode
::
lidar
_mutex_exit
(
void
)
{
pthread_mutex_unlock
(
&
this
->
velodyne
_mutex_
);
pthread_mutex_unlock
(
&
this
->
lidar
_mutex_
);
}
/* [service callbacks] */
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment