Skip to content

Commit da0288e

Browse files
author
Florian Weisshardt
committed
Merge pull request #1 from ipa-svn/indigo-devel
ipa changes moved from navigation package
2 parents 317e389 + 7c5817b commit da0288e

8 files changed

+719
-3
lines changed

‎CMakeLists.txt‎

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,13 @@ find_package(catkin REQUIRED COMPONENTS
99
tf2
1010
tf2_ros
1111
tf2_sensor_msgs
12+
tf2_geometry_msgs
1213
)
1314

1415
catkin_package(
1516
INCLUDE_DIRS include
16-
LIBRARIES pointcloud_to_laserscan
17-
CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros
17+
LIBRARIES pointcloud_to_laserscan ipa_pointcloud_to_laserscan
18+
CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros tf2_sensor_msgs tf2_geometry_msgs
1819
)
1920

2021
include_directories(
@@ -28,7 +29,13 @@ target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
2829
add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp)
2930
target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES})
3031

31-
install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node
32+
add_library(ipa_pointcloud_to_laserscan src/ipa_pointcloud_to_laserscan_nodelet.cpp)
33+
target_link_libraries(ipa_pointcloud_to_laserscan ${catkin_LIBRARIES})
34+
35+
add_executable(ipa_pointcloud_to_laserscan_node src/ipa_pointcloud_to_laserscan_node.cpp)
36+
target_link_libraries(ipa_pointcloud_to_laserscan_node ipa_pointcloud_to_laserscan ${catkin_LIBRARIES})
37+
38+
install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node ipa_pointcloud_to_laserscan ipa_pointcloud_to_laserscan_node
3239
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
3340
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
3441
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

‎include/pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet.h‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2010-2012, Willow Garage, Inc.
5+
* All rights reserved.
6+
*
7+
* Redistribution and use in source and binary forms, with or without
8+
* modification, are permitted provided that the following conditions
9+
* are met:
10+
*
11+
* * Redistributions of source code must retain the above copyright
12+
* notice, this list of conditions and the following disclaimer.
13+
* * Redistributions in binary form must reproduce the above
14+
* copyright notice, this list of conditions and the following
15+
* disclaimer in the documentation and/or other materials provided
16+
* with the distribution.
17+
* * Neither the name of Willow Garage, Inc. nor the names of its
18+
* contributors may be used to endorse or promote products derived
19+
* from this software without specific prior written permission.
20+
*
21+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+
* POSSIBILITY OF SUCH DAMAGE.
33+
*
34+
*
35+
*/
36+
37+
/*
38+
* Author: Paul Bovbel
39+
* Author: Sofie Nilsson
40+
*/
41+
42+
#ifndef IPA_POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
43+
#define IPA_POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
44+
45+
#include "ros/ros.h"
46+
#include "boost/thread/mutex.hpp"
47+
48+
#include "nodelet/nodelet.h"
49+
#include "tf2_ros/buffer.h"
50+
#include "tf2_ros/transform_listener.h"
51+
#include "tf2_ros/message_filter.h"
52+
#include "message_filters/subscriber.h"
53+
#include "sensor_msgs/PointCloud2.h"
54+
55+
56+
namespace pointcloud_to_laserscan
57+
{
58+
typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
59+
/**
60+
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
61+
* pointcloud_to_laserscan implementation.
62+
*/
63+
class IpaPointCloudToLaserScanNodelet : public nodelet::Nodelet
64+
{
65+
66+
public:
67+
IpaPointCloudToLaserScanNodelet();
68+
69+
private:
70+
virtual void onInit();
71+
72+
void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
73+
void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
74+
tf2_ros::filter_failure_reasons::FilterFailureReason reason);
75+
76+
void connectCb();
77+
78+
void disconnectCb();
79+
80+
ros::NodeHandle nh_, private_nh_;
81+
ros::Publisher pub_;
82+
boost::mutex connect_mutex_;
83+
84+
boost::shared_ptr<tf2_ros::Buffer> tf2_;
85+
boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
86+
message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
87+
boost::shared_ptr<MessageFilter> message_filter_;
88+
89+
// ROS Parameters
90+
unsigned int input_queue_size_;
91+
std::string target_frame_;
92+
double tolerance_;
93+
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
94+
bool use_inf_;
95+
};
96+
97+
} // pointcloud_to_laserscan
98+
99+
#endif // IPA_POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET

‎launch/cob4_test_launch_node.launch‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
5+
<arg name="camera1" default="torso_cam3d_left" />
6+
<arg name="camera2" default="torso_cam3d_right" />
7+
8+
9+
<!-- run pointcloud_to_laserscan node -->
10+
<node pkg="pointcloud_to_laserscan" type="ipa_pointcloud_to_laserscan_node" name="pointcloud_to_laserscan_l">
11+
12+
<remap from="cloud_in" to="$(arg camera1)/depth_registered/points"/>
13+
<remap from="scan" to="$(arg camera1)/scan"/>
14+
<rosparam>
15+
target_frame: base_link # torso_3_link # Leave disabled to output scan in pointcloud frame
16+
transform_tolerance: 0.05
17+
min_height: 0.15 #-0.5 # -0.9
18+
max_height: 1.5 #0.5 # 0.6
19+
20+
angle_min: -0.5708 # -M_PI/2
21+
angle_max: 0.7 # M_PI/2
22+
angle_increment: 0.008 #0.0174 # 1*M_PI/180.0
23+
scan_time: 0.003333
24+
range_min: 0.25
25+
range_max: 10.0
26+
use_inf: true
27+
28+
use_outlier_filter: true
29+
max_noise_cluster_distance: 2.5
30+
cluster_break_distance: 0.3
31+
max_noise_cluster_size: 10
32+
33+
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
34+
# 0 : Detect number of cores
35+
# 1 : Single threaded
36+
# 2->inf : Parallelism level
37+
concurrency_level: 0
38+
</rosparam>
39+
40+
</node>
41+
42+
<!-- run pointcloud_to_laserscan node -->
43+
<node pkg="pointcloud_to_laserscan" type="ipa_pointcloud_to_laserscan_node" name="pointcloud_to_laserscan_r">
44+
45+
<remap from="cloud_in" to="$(arg camera2)/depth_registered/points"/>
46+
<remap from="scan" to="$(arg camera2)/scan"/>
47+
<rosparam>
48+
target_frame: base_link # torso_3_link # Leave disabled to output scan in pointcloud frame
49+
transform_tolerance: 0.05
50+
min_height: 0.15 #-0.5 # -0.9
51+
max_height: 1.5 #0.5 # 0.6
52+
53+
angle_min: -0.7 # -M_PI/2
54+
angle_max: 0.5708 # M_PI/2
55+
angle_increment: 0.008 #0.0174 # 1*M_PI/180.0
56+
scan_time: 0.003333
57+
range_min: 0.25
58+
range_max: 10.0
59+
use_inf: true
60+
61+
use_outlier_filter: true
62+
max_noise_cluster_distance: 2.5
63+
cluster_break_distance: 0.3
64+
max_noise_cluster_size: 10
65+
66+
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
67+
# 0 : Detect number of cores
68+
# 1 : Single threaded
69+
# 2->inf : Parallelism level
70+
concurrency_level: 0
71+
</rosparam>
72+
73+
</node>
74+
75+
</launch>

‎launch/cob4_test_launch_nodelet.launch‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
5+
<arg name="camera" default="torso_cam3d_left" />
6+
7+
<!-- start sensor-->
8+
<!--<include file="$(find openni2_launch)/launch/openni2.launch">
9+
<arg name="camera" default="$(arg camera)"/>
10+
</include>-->
11+
<!-- start nodlet manager -->
12+
<!-- would be nicer to ude the manager started by the openni driver, but this is not available when for example using bagfiles -->
13+
<node pkg="nodelet" type="nodelet" name="pc_left_converter_nodelet_manager" args="manager"/>
14+
15+
<!-- push pointcloud_to_laserscan nodelet into sensor's nodelet manager-->
16+
<node pkg="nodelet" type="nodelet" name="ipa_pointcloud_to_laserscan" args="load pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet pc_left_converter_nodelet_manager">
17+
18+
<remap from="cloud_in" to="$(arg camera)/depth_registered/points"/>
19+
<remap from="scan" to="$(arg camera)/scan"/>
20+
<rosparam>
21+
target_frame: base_link # torso_3_link # Leave disabled to output scan in pointcloud frame
22+
transform_tolerance: 0.05
23+
min_height: 0.15 #-0.5 # -0.9
24+
max_height: 1.5 #0.5 # 0.6
25+
26+
angle_min: -0.5708 # -M_PI/2
27+
angle_max: 0.7 # M_PI/2
28+
angle_increment: 0.008 #0.0174 # 1*M_PI/180.0
29+
scan_time: 0.003333
30+
range_min: 0.25
31+
range_max: 10.0
32+
use_inf: true
33+
34+
use_outlier_filter: true
35+
max_noise_cluster_distance: 2.5
36+
cluster_break_distance: 0.3
37+
max_noise_cluster_size: 10
38+
39+
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
40+
# 0 : Detect number of cores
41+
# 1 : Single threaded
42+
# 2->inf : Parallelism level
43+
concurrency_level: 0
44+
</rosparam>
45+
46+
47+
</node>
48+
49+
50+
51+
</launch>

‎nodelets.xml‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,3 +9,15 @@
99
</class>
1010

1111
</library>
12+
13+
<library path="lib/libipa_pointcloud_to_laserscan">
14+
15+
<class name="pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet"
16+
type="pointcloud_to_laserscan::IpaPointCloudToLaserScanNodelet"
17+
base_class_type="nodelet::Nodelet">
18+
<description>
19+
Ipa version of Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans.
20+
</description>
21+
</class>
22+
23+
</library>

‎package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
<build_depend>tf2</build_depend>
2424
<build_depend>tf2_ros</build_depend>
2525
<build_depend>tf2_sensor_msgs</build_depend>
26+
<build_depend>tf2_geometry_msgs</build_depend>
2627

2728
<run_depend>message_filters</run_depend>
2829
<run_depend>nodelet</run_depend>
@@ -31,6 +32,7 @@
3132
<run_depend>tf2</run_depend>
3233
<run_depend>tf2_ros</run_depend>
3334
<run_depend>tf2_sensor_msgs</run_depend>
35+
<run_depend>tf2_geometry_msgs</run_depend>
3436

3537
<export>
3638
<nodelet plugin="${prefix}/nodelets.xml"/>

‎src/ipa_pointcloud_to_laserscan_node.cpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2010-2012, Willow Garage, Inc.
5+
* All rights reserved.
6+
*
7+
* Redistribution and use in source and binary forms, with or without
8+
* modification, are permitted provided that the following conditions
9+
* are met:
10+
*
11+
* * Redistributions of source code must retain the above copyright
12+
* notice, this list of conditions and the following disclaimer.
13+
* * Redistributions in binary form must reproduce the above
14+
* copyright notice, this list of conditions and the following
15+
* disclaimer in the documentation and/or other materials provided
16+
* with the distribution.
17+
* * Neither the name of Willow Garage, Inc. nor the names of its
18+
* contributors may be used to endorse or promote products derived
19+
* from this software without specific prior written permission.
20+
*
21+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+
* POSSIBILITY OF SUCH DAMAGE.
33+
*
34+
*
35+
*/
36+
37+
/*
38+
* Author: Paul Bovbel
39+
* Author: Sofie Nilsson
40+
*/
41+
42+
#include <ros/ros.h>
43+
#include <nodelet/loader.h>
44+
45+
int main(int argc, char **argv){
46+
ros::init(argc, argv, "ipa_pointcloud_to_laserscan_node");
47+
ros::NodeHandle private_nh("~");
48+
int concurrency_level;
49+
private_nh.param<int>("concurrency_level", concurrency_level, 0);
50+
51+
nodelet::Loader nodelet;
52+
nodelet::M_string remap(ros::names::getRemappings());
53+
nodelet::V_string nargv;
54+
std::string nodelet_name = ros::this_node::getName();
55+
nodelet.load(nodelet_name, "pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet", remap, nargv);
56+
57+
boost::shared_ptr<ros::MultiThreadedSpinner> spinner;
58+
if(concurrency_level) {
59+
spinner.reset(new ros::MultiThreadedSpinner(concurrency_level));
60+
}else{
61+
spinner.reset(new ros::MultiThreadedSpinner());
62+
}
63+
spinner->spin();
64+
return 0;
65+
66+
}

0 commit comments

Comments
 (0)