diff --git a/config/pointcloud2_to_laserscan_param.yaml b/config/pointcloud2_to_laserscan_param.yaml
new file mode 100644
index 00000000..71eeff53
--- /dev/null
+++ b/config/pointcloud2_to_laserscan_param.yaml
@@ -0,0 +1,21 @@
+target_frame: hokuyo3d # Leave disabled to output scan in pointcloud frame
+cloud_in: /hokuyo3d/hokuyo_cloud2 # input point_cloud2 topic name
+scan: /hokuyo3d/scan # output scan topic name
+transform_tolerance: 0.01
+min_height: 0.0
+max_height: 1.0
+
+angle_min: -1.5708 # -M_PI/2
+angle_max: 1.5708 # M_PI/2
+angle_increment: 0.0087 # M_PI/360.0
+scan_time: 0.3333
+range_min: 0.45
+range_max: 4.0
+use_inf: true
+inf_epsilon: 1.0
+
+# Concurrency level, affects number of pointclouds queued for processing and number of threads used
+# 0 : Detect number of cores
+# 1 : Single threaded
+# 2->inf : Parallelism level
+concurrency_level: 1
\ No newline at end of file
diff --git a/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h
index 930cebaf..c40fe7e6 100644
--- a/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h
+++ b/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h
@@ -86,6 +86,8 @@ class PointCloudToLaserScanNodelet : public nodelet::Nodelet
// ROS Parameters
unsigned int input_queue_size_;
std::string target_frame_;
+ std::string cloud_in_;
+ std::string scan_;
double tolerance_;
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
bool use_inf_;
diff --git a/launch/sample_node_with_yaml.launch b/launch/sample_node_with_yaml.launch
new file mode 100644
index 00000000..a690a407
--- /dev/null
+++ b/launch/sample_node_with_yaml.launch
@@ -0,0 +1,6 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/src/pointcloud_to_laserscan_nodelet.cpp b/src/pointcloud_to_laserscan_nodelet.cpp
index 184ff7ed..2f87e8df 100644
--- a/src/pointcloud_to_laserscan_nodelet.cpp
+++ b/src/pointcloud_to_laserscan_nodelet.cpp
@@ -69,6 +69,8 @@ void PointCloudToLaserScanNodelet::onInit()
private_nh_.param("range_min", range_min_, 0.0);
private_nh_.param("range_max", range_max_, std::numeric_limits::max());
private_nh_.param("inf_epsilon", inf_epsilon_, 1.0);
+ private_nh_.param("cloud_in", cloud_in_, "/hokuyo3d/hokuyo_cloud2");
+ private_nh_.param("scan", scan_, "/hokuyo3d/scan");
int concurrency_level;
private_nh_.param("concurrency_level", concurrency_level, 1);