diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch
index 48c0ef47..9c077fca 100644
--- a/pointcloud_to_laserscan/launch/sample_node.launch
+++ b/pointcloud_to_laserscan/launch/sample_node.launch
@@ -15,7 +15,7 @@
- target_frame: base_link
+ target_frame: base_link # Leave empty for no transform
min_height: 0.0
max_height: 1.0
diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch
index 84943f55..45027f8a 100644
--- a/pointcloud_to_laserscan/launch/sample_nodelet.launch
+++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch
@@ -15,7 +15,7 @@
- target_frame: base_link
+ target_frame: base_link # Leave empty for no transform
min_height: 0.0
max_height: 1.0
diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml
index 63420f17..cb6cb0b8 100644
--- a/pointcloud_to_laserscan/package.xml
+++ b/pointcloud_to_laserscan/package.xml
@@ -23,6 +23,7 @@
roscpp
roslaunch
sensor_msgs
+ tf2
dynamic_reconfigure
libpcl-all-dev
@@ -31,7 +32,7 @@
pcl_ros
roscpp
sensor_msgs
- openni2_launch
+ tf2
diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
index de9ec102..0c63065f 100644
--- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
+++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
@@ -50,7 +50,7 @@ namespace pointcloud_to_laserscan
void PointCloudToLaserScanNodelet::onInit()
{
- nh_ = getMTPrivateNodeHandle();
+ nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle();
private_nh_.param("target_frame", target_frame_, "");
@@ -83,6 +83,24 @@ namespace pointcloud_to_laserscan
boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));
}
+ void PointCloudToLaserScanNodelet::connectCb()
+ {
+ boost::mutex::scoped_lock lock(connect_mutex_);
+ if (!sub_ && pub_.getNumSubscribers() > 0) {
+ NODELET_DEBUG("Connecting to depth topic.");
+ sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this);
+ }
+ }
+
+ void PointCloudToLaserScanNodelet::disconnectCb()
+ {
+ boost::mutex::scoped_lock lock(connect_mutex_);
+ if (pub_.getNumSubscribers() == 0) {
+ NODELET_DEBUG("Unsubscribing from depth topic.");
+ sub_.shutdown();
+ }
+ }
+
void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg)
{
//pointer to pointcloud data to transform to laserscan
@@ -164,23 +182,6 @@ namespace pointcloud_to_laserscan
pub_.publish(output);
}
- void PointCloudToLaserScanNodelet::connectCb()
- {
- boost::mutex::scoped_lock lock(connect_mutex_);
- if (!sub_ && pub_.getNumSubscribers() > 0) {
- NODELET_DEBUG("Connecting to depth topic.");
- sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this);
- }
- }
-
- void PointCloudToLaserScanNodelet::disconnectCb()
- {
- boost::mutex::scoped_lock lock(connect_mutex_);
- if (pub_.getNumSubscribers() == 0) {
- NODELET_DEBUG("Unsubscribing from depth topic.");
- sub_.shutdown();
- }
- }
}
PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet);