Skip to content

Commit 9312409

Browse files
author
Benjamin Maidel
authored
Merge pull request #3 from ipa-svn/indigo-devel
P2L without parameter lookup in callback
2 parents da0288e + 3aa3081 commit 9312409

File tree

8 files changed

+315
-104
lines changed

8 files changed

+315
-104
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,10 +29,10 @@ target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
2929
add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp)
3030
target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES})
3131

32-
add_library(ipa_pointcloud_to_laserscan src/ipa_pointcloud_to_laserscan_nodelet.cpp)
32+
add_library(ipa_pointcloud_to_laserscan src/ipa_pointcloud_to_laserscan_nodelet.cpp src/scan_outlier_removal_filter.cpp)
3333
target_link_libraries(ipa_pointcloud_to_laserscan ${catkin_LIBRARIES})
3434

35-
add_executable(ipa_pointcloud_to_laserscan_node src/ipa_pointcloud_to_laserscan_node.cpp)
35+
add_executable(ipa_pointcloud_to_laserscan_node src/ipa_pointcloud_to_laserscan_node.cpp src/scan_outlier_removal_filter.cpp)
3636
target_link_libraries(ipa_pointcloud_to_laserscan_node ipa_pointcloud_to_laserscan ${catkin_LIBRARIES})
3737

3838
install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node ipa_pointcloud_to_laserscan ipa_pointcloud_to_laserscan_node

‎config/cob4_scan_left.yaml‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
# -----------------Scan construction settings --------------
2+
# The target frame of the generated scan
3+
target_frame: base_link # Leave disabled to output scan in pointcloud frame
4+
5+
# Pointcloud reduction parameters to define which points to use
6+
# scan construction.
7+
# The combination of borders defines a cylinder in the target frame.
8+
# Only points within that cylinder (between all max and min borders)
9+
# are used for the scan creation.
10+
11+
# z axis borders [meters] in target frame
12+
min_height: 0.15
13+
max_height: 1.5
14+
15+
# angle [radians] borders in x-y-plane of target frame
16+
angle_min: 0.03
17+
angle_max: 0.78
18+
angle_increment: 0.008
19+
20+
# range [meters] borders to the z axis of the target frame
21+
range_min: 0.25
22+
range_max: 10.0
23+
24+
# other scan construction settings
25+
transform_tolerance: 0.05
26+
scan_time: 0.003333
27+
use_inf: true
28+
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
29+
# 0 : Detect number of cores
30+
# 1 : Single threaded
31+
# 2->inf : Parallelism level
32+
concurrency_level: 0
33+
34+
35+
# ------------- Scan filter settings -----------------
36+
# Settings for the outlier filter applied to the constructed scan
37+
# flag to use outlier filter
38+
use_outlier_filter: true
39+
# set the max range that the cluster points can have in order for
40+
# the cluster to be counted as noise
41+
max_noise_cluster_distance: 2.5 # [meters]
42+
# The minimum range jump of two naighoring points to cause a
43+
# new cluster
44+
cluster_break_distance: 0.3 # [meters]
45+
# The maximum number of points in a cluster for it to be considered
46+
# a noise cluster
47+
max_noise_cluster_size: 10

‎config/cob4_scan_right.yaml‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
# -----------------Scan construction settings --------------
2+
# The target frame of the generated scan
3+
target_frame: base_link # Leave disabled to output scan in pointcloud frame
4+
5+
# Pointcloud reduction parameters to define which points to use
6+
# scan construction.
7+
# The combination of borders defines a cylinder in the target frame.
8+
# Only points within that cylinder (between all max and min borders)
9+
# are used for the scan creation.
10+
11+
# z axis borders [meters] in target frame
12+
min_height: 0.15
13+
max_height: 1.5
14+
15+
# angle [radians] borders in x-y-plane of target frame
16+
angle_min: -0.78
17+
angle_max: -0.03
18+
angle_increment: 0.008
19+
20+
# range [meters] borders to the z axis of the target frame
21+
range_min: 0.25
22+
range_max: 10.0
23+
24+
# other scan construction settings
25+
transform_tolerance: 0.05
26+
scan_time: 0.003333
27+
use_inf: true
28+
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
29+
# 0 : Detect number of cores
30+
# 1 : Single threaded
31+
# 2->inf : Parallelism level
32+
concurrency_level: 0
33+
34+
35+
# ------------- Scan filter settings -----------------
36+
# Settings for the outlier filter applied to the constructed scan
37+
# flag to use outlier filter
38+
use_outlier_filter: true
39+
# set the max range that the cluster points can have in order for
40+
# the cluster to be counted as noise
41+
max_noise_cluster_distance: 2.5 # [meters]
42+
# The minimum range jump of two naighoring points to cause a
43+
# new cluster
44+
cluster_break_distance: 0.3 # [meters]
45+
# The maximum number of points in a cluster for it to be considered
46+
# a noise cluster
47+
max_noise_cluster_size: 10

‎include/pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,7 @@
5151
#include "tf2_ros/message_filter.h"
5252
#include "message_filters/subscriber.h"
5353
#include "sensor_msgs/PointCloud2.h"
54+
#include <pointcloud_to_laserscan/scan_outlier_removal_filter.h>
5455

5556

5657
namespace pointcloud_to_laserscan
@@ -65,6 +66,7 @@ namespace pointcloud_to_laserscan
6566

6667
public:
6768
IpaPointCloudToLaserScanNodelet();
69+
void configure_filter();
6870

6971
private:
7072
virtual void onInit();
@@ -86,12 +88,14 @@ namespace pointcloud_to_laserscan
8688
message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
8789
boost::shared_ptr<MessageFilter> message_filter_;
8890

91+
scan_outlier_filter::ScanOutlierRemovalFilter outlier_filter_;
8992
// ROS Parameters
9093
unsigned int input_queue_size_;
9194
std::string target_frame_;
9295
double tolerance_;
9396
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
9497
bool use_inf_;
98+
bool use_outlier_filter_;
9599
};
96100

97101
} // pointcloud_to_laserscan

‎include/pointcloud_to_laserscan/scan_outlier_removal_filter.h‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
2+
/*
3+
* Software License Agreement (BSD License)
4+
*
5+
* Copyright (c) 2010-2012, Fraunhofer IPA
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of Willow Garage, Inc. nor the names of its
19+
* contributors may be used to endorse or promote products derived
20+
* from this software without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*
35+
*
36+
*/
37+
38+
/*
39+
* Author: Sofie Nilsson
40+
*/
41+
42+
#ifndef IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
43+
#define IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
44+
45+
#include <sensor_msgs/LaserScan.h>
46+
47+
namespace scan_outlier_filter
48+
{
49+
class ScanOutlierRemovalFilter
50+
{
51+
private:
52+
bool filter_configured_;
53+
double cluster_break_distance_;
54+
int max_noise_cluster_size_;
55+
double max_noise_cluster_distance_;
56+
57+
public:
58+
ScanOutlierRemovalFilter():
59+
filter_configured_(false),
60+
cluster_break_distance_(0.0),
61+
max_noise_cluster_size_(0),
62+
max_noise_cluster_distance_(0.0)
63+
{};
64+
void configure(const double cluster_break_distance, const int max_noise_cluster_size, const double max_noise_cluster_distance);
65+
66+
void remove_outliers(sensor_msgs::LaserScan &scan);
67+
};
68+
}
69+
#endif //IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER

‎launch/nodelet.launch‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
5+
<arg name="nodelet_manager" />
6+
<arg name="pointcloud_topic" />
7+
<arg name="scan_topic" />
8+
9+
<!-- push pointcloud_to_laserscan nodelet into sensor's nodelet manager-->
10+
<node pkg="nodelet" type="nodelet" name="ipa_pointcloud_to_laserscan" args="load pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet $(arg nodelet_manager)">
11+
12+
<remap from="cloud_in" to="$(arg pointcloud_topic)"/>
13+
<remap from="scan" to="$(arg scan_topic)"/>
14+
15+
</node>
16+
</launch>

0 commit comments

Comments
 (0)