From de8a0aca3fbcc175007c29dad938c9c6e936345a Mon Sep 17 00:00:00 2001
From: Paul Bovbel <pbovbel@clearpathrobotics.com>
Date: Thu, 15 Jan 2015 13:41:57 -0500
Subject: [PATCH] Fix regressions

---
 .../launch/sample_node.launch                 |  2 +-
 .../launch/sample_nodelet.launch              |  2 +-
 pointcloud_to_laserscan/package.xml           |  3 +-
 .../src/pointcloud_to_laserscan_nodelet.cpp   | 37 ++++++++++---------
 4 files changed, 23 insertions(+), 21 deletions(-)

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 @@
         <remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
         <remap from="scan" to="$(arg camera)/scan"/>
         <rosparam>
-            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 @@
         <remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
         <remap from="scan" to="$(arg camera)/scan"/>
         <rosparam>
-            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 @@
     <build_depend>roscpp</build_depend>
     <build_depend>roslaunch</build_depend>
     <build_depend>sensor_msgs</build_depend>
+    <build_depend>tf2</build_depend>
 
     <run_depend>dynamic_reconfigure</run_depend>
     <run_depend>libpcl-all-dev</run_depend>
@@ -31,7 +32,7 @@
     <run_depend>pcl_ros</run_depend>
     <run_depend>roscpp</run_depend>
     <run_depend>sensor_msgs</run_depend>
-    <run_depend>openni2_launch</run_depend>
+    <run_depend>tf2</run_depend>
 
     <export>
         <nodelet plugin="${prefix}/nodelets.xml"/>
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<std::string>("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);