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);