From eb81f4c3032eee17b7170e6543d48222e215bd4e Mon Sep 17 00:00:00 2001 From: yukikitsukawa Date: Fri, 4 Nov 2016 22:38:39 +0900 Subject: [PATCH] Rename variables. --- .../points_downsampler/CMakeLists.txt | 4 +- ...filter.launch => points_downsample.launch} | 2 +- ...lterInfo.msg => PointsDownsamplerInfo.msg} | 0 .../nodes/distance_filter/distance_filter.cpp | 42 +++++++++--------- .../nodes/random_filter/random_filter.cpp | 44 +++++++++---------- .../nodes/ring_filter/ring_filter.cpp | 44 +++++++++---------- .../voxel_grid_filter/voxel_grid_filter.cpp | 44 +++++++++---------- .../packages/points_downsampler/package.xml | 4 +- .../runtime_manager/scripts/sensing.yaml | 12 ++--- 9 files changed, 98 insertions(+), 98 deletions(-) rename ros/src/sensing/filters/packages/points_downsampler/launch/{filter.launch => points_downsample.launch} (78%) rename ros/src/sensing/filters/packages/points_downsampler/msg/{PointsFilterInfo.msg => PointsDownsamplerInfo.msg} (100%) diff --git a/ros/src/sensing/filters/packages/points_downsampler/CMakeLists.txt b/ros/src/sensing/filters/packages/points_downsampler/CMakeLists.txt index a6917aa2dac..b120921dae0 100644 --- a/ros/src/sensing/filters/packages/points_downsampler/CMakeLists.txt +++ b/ros/src/sensing/filters/packages/points_downsampler/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(points_filter) +project(points_downsampler) find_package(catkin REQUIRED COMPONENTS roscpp @@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES - PointsFilterInfo.msg + PointsDownsamplerInfo.msg ) generate_messages( diff --git a/ros/src/sensing/filters/packages/points_downsampler/launch/filter.launch b/ros/src/sensing/filters/packages/points_downsampler/launch/points_downsample.launch similarity index 78% rename from ros/src/sensing/filters/packages/points_downsampler/launch/filter.launch rename to ros/src/sensing/filters/packages/points_downsampler/launch/points_downsample.launch index cd8dbdb140f..27eea80bbf0 100644 --- a/ros/src/sensing/filters/packages/points_downsampler/launch/filter.launch +++ b/ros/src/sensing/filters/packages/points_downsampler/launch/points_downsample.launch @@ -3,7 +3,7 @@ - + diff --git a/ros/src/sensing/filters/packages/points_downsampler/msg/PointsFilterInfo.msg b/ros/src/sensing/filters/packages/points_downsampler/msg/PointsDownsamplerInfo.msg similarity index 100% rename from ros/src/sensing/filters/packages/points_downsampler/msg/PointsFilterInfo.msg rename to ros/src/sensing/filters/packages/points_downsampler/msg/PointsDownsamplerInfo.msg diff --git a/ros/src/sensing/filters/packages/points_downsampler/nodes/distance_filter/distance_filter.cpp b/ros/src/sensing/filters/packages/points_downsampler/nodes/distance_filter/distance_filter.cpp index 8739ebab733..71c7ae8e557 100644 --- a/ros/src/sensing/filters/packages/points_downsampler/nodes/distance_filter/distance_filter.cpp +++ b/ros/src/sensing/filters/packages/points_downsampler/nodes/distance_filter/distance_filter.cpp @@ -37,7 +37,7 @@ #include -#include +#include #include @@ -45,8 +45,8 @@ ros::Publisher filtered_points_pub; static int sample_num = 1000; -static ros::Publisher points_filter_info_pub; -static points_filter::PointsFilterInfo points_filter_info_msg; +static ros::Publisher points_downsampler_info_pub; +static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; static std::chrono::time_point filter_start, filter_end; @@ -106,29 +106,29 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) filtered_msg.header = input->header; filtered_points_pub.publish(filtered_msg); - points_filter_info_msg.header = input->header; - points_filter_info_msg.filter_name = "distance_filter"; - points_filter_info_msg.original_points_size = points_num; - points_filter_info_msg.filtered_points_size = filtered_scan_ptr->size(); - points_filter_info_msg.original_ring_size = 0; - points_filter_info_msg.original_ring_size = 0; - points_filter_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_filter_info_pub.publish(points_filter_info_msg); + points_downsampler_info_msg.header = input->header; + points_downsampler_info_msg.filter_name = "distance_filter"; + points_downsampler_info_msg.original_points_size = points_num; + points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); + points_downsampler_info_msg.original_ring_size = 0; + points_downsampler_info_msg.original_ring_size = 0; + points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; + points_downsampler_info_pub.publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } - ofs << points_filter_info_msg.header.seq << "," - << points_filter_info_msg.header.stamp << "," - << points_filter_info_msg.header.frame_id << "," - << points_filter_info_msg.filter_name << "," - << points_filter_info_msg.original_points_size << "," - << points_filter_info_msg.filtered_points_size << "," - << points_filter_info_msg.original_ring_size << "," - << points_filter_info_msg.filtered_ring_size << "," - << points_filter_info_msg.exe_time << "," + ofs << points_downsampler_info_msg.header.seq << "," + << points_downsampler_info_msg.header.stamp << "," + << points_downsampler_info_msg.header.frame_id << "," + << points_downsampler_info_msg.filter_name << "," + << points_downsampler_info_msg.original_points_size << "," + << points_downsampler_info_msg.filtered_points_size << "," + << points_downsampler_info_msg.original_ring_size << "," + << points_downsampler_info_msg.filtered_ring_size << "," + << points_downsampler_info_msg.exe_time << "," << std::endl; } @@ -153,7 +153,7 @@ int main(int argc, char** argv) // Publishers filtered_points_pub = nh.advertise("/filtered_points", 10); - points_filter_info_pub = nh.advertise("/points_filter_info", 1000); + points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); // Subscribers ros::Subscriber config_sub = nh.subscribe("config/distance_filter", 10, config_callback); diff --git a/ros/src/sensing/filters/packages/points_downsampler/nodes/random_filter/random_filter.cpp b/ros/src/sensing/filters/packages/points_downsampler/nodes/random_filter/random_filter.cpp index 9e93a84d4ac..295662fbd38 100644 --- a/ros/src/sensing/filters/packages/points_downsampler/nodes/random_filter/random_filter.cpp +++ b/ros/src/sensing/filters/packages/points_downsampler/nodes/random_filter/random_filter.cpp @@ -36,7 +36,7 @@ #include -#include +#include #include @@ -44,8 +44,8 @@ ros::Publisher filtered_points_pub; static int sample_num = 1000; -static ros::Publisher points_filter_info_pub; -static points_filter::PointsFilterInfo points_filter_info_msg; +static ros::Publisher points_downsampler_info_pub; +static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; static std::chrono::time_point filter_start, filter_end; @@ -75,7 +75,7 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) for (int i = 0; i < points_num; i++) { - if (filtered_scan_ptr->size() < sample_num && i % step == 0) + if ((int)filtered_scan_ptr->size() < sample_num && i % step == 0) { filtered_scan_ptr->points.push_back(scan.at(i)); } @@ -89,29 +89,29 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) filtered_msg.header = input->header; filtered_points_pub.publish(filtered_msg); - points_filter_info_msg.header = input->header; - points_filter_info_msg.filter_name = "random_filter"; - points_filter_info_msg.original_points_size = points_num; - points_filter_info_msg.filtered_points_size = filtered_scan_ptr->size(); - points_filter_info_msg.original_ring_size = 0; - points_filter_info_msg.filtered_ring_size = 0; - points_filter_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_filter_info_pub.publish(points_filter_info_msg); + points_downsampler_info_msg.header = input->header; + points_downsampler_info_msg.filter_name = "random_filter"; + points_downsampler_info_msg.original_points_size = points_num; + points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); + points_downsampler_info_msg.original_ring_size = 0; + points_downsampler_info_msg.filtered_ring_size = 0; + points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; + points_downsampler_info_pub.publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } - ofs << points_filter_info_msg.header.seq << "," - << points_filter_info_msg.header.stamp << "," - << points_filter_info_msg.header.frame_id << "," - << points_filter_info_msg.filter_name << "," - << points_filter_info_msg.original_points_size << "," - << points_filter_info_msg.filtered_points_size << "," - << points_filter_info_msg.original_ring_size << "," - << points_filter_info_msg.filtered_ring_size << "," - << points_filter_info_msg.exe_time << "," + ofs << points_downsampler_info_msg.header.seq << "," + << points_downsampler_info_msg.header.stamp << "," + << points_downsampler_info_msg.header.frame_id << "," + << points_downsampler_info_msg.filter_name << "," + << points_downsampler_info_msg.original_points_size << "," + << points_downsampler_info_msg.filtered_points_size << "," + << points_downsampler_info_msg.original_ring_size << "," + << points_downsampler_info_msg.filtered_ring_size << "," + << points_downsampler_info_msg.exe_time << "," << std::endl; } @@ -136,7 +136,7 @@ int main(int argc, char** argv) // Publishers filtered_points_pub = nh.advertise("/filtered_points", 10); - points_filter_info_pub = nh.advertise("/points_filter_info", 1000); + points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); // Subscribers ros::Subscriber config_sub = nh.subscribe("config/random_filter", 10, config_callback); diff --git a/ros/src/sensing/filters/packages/points_downsampler/nodes/ring_filter/ring_filter.cpp b/ros/src/sensing/filters/packages/points_downsampler/nodes/ring_filter/ring_filter.cpp index 65ff55cdfea..639dcf78de0 100644 --- a/ros/src/sensing/filters/packages/points_downsampler/nodes/ring_filter/ring_filter.cpp +++ b/ros/src/sensing/filters/packages/points_downsampler/nodes/ring_filter/ring_filter.cpp @@ -39,7 +39,7 @@ #include -#include +#include #include @@ -51,8 +51,8 @@ static double voxel_leaf_size = 2.0; int ring_max = 0; int ring_div = 3; -static ros::Publisher points_filter_info_pub; -static points_filter::PointsFilterInfo points_filter_info_msg; +static ros::Publisher points_downsampler_info_pub; +static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; static std::chrono::time_point filter_start, filter_end; @@ -120,36 +120,36 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) filtered_msg.header = input->header; filtered_points_pub.publish(filtered_msg); - points_filter_info_msg.header = input->header; - points_filter_info_msg.filter_name = "ring_filter"; - points_filter_info_msg.original_points_size = scan.size(); + points_downsampler_info_msg.header = input->header; + points_downsampler_info_msg.filter_name = "ring_filter"; + points_downsampler_info_msg.original_points_size = scan.size(); if (voxel_leaf_size >= 0.1) { - points_filter_info_msg.filtered_points_size = filtered_scan_ptr->size(); + points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); } else { - points_filter_info_msg.filtered_points_size = scan_ptr->size(); + points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); } - points_filter_info_msg.original_ring_size = ring_max; - points_filter_info_msg.filtered_ring_size = ring_max / ring_div; - points_filter_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_filter_info_pub.publish(points_filter_info_msg); + points_downsampler_info_msg.original_ring_size = ring_max; + points_downsampler_info_msg.filtered_ring_size = ring_max / ring_div; + points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; + points_downsampler_info_pub.publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } - ofs << points_filter_info_msg.header.seq << "," - << points_filter_info_msg.header.stamp << "," - << points_filter_info_msg.header.frame_id << "," - << points_filter_info_msg.filter_name << "," - << points_filter_info_msg.original_points_size << "," - << points_filter_info_msg.filtered_points_size << "," - << points_filter_info_msg.original_ring_size << "," - << points_filter_info_msg.filtered_ring_size << "," - << points_filter_info_msg.exe_time << "," + ofs << points_downsampler_info_msg.header.seq << "," + << points_downsampler_info_msg.header.stamp << "," + << points_downsampler_info_msg.header.frame_id << "," + << points_downsampler_info_msg.filter_name << "," + << points_downsampler_info_msg.original_points_size << "," + << points_downsampler_info_msg.filtered_points_size << "," + << points_downsampler_info_msg.original_ring_size << "," + << points_downsampler_info_msg.filtered_ring_size << "," + << points_downsampler_info_msg.exe_time << "," << std::endl; } @@ -174,7 +174,7 @@ int main(int argc, char** argv) // Publishers filtered_points_pub = nh.advertise("/filtered_points", 10); - points_filter_info_pub = nh.advertise("/points_filter_info", 1000); + points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); // Subscribers ros::Subscriber config_sub = nh.subscribe("config/ring_filter", 10, config_callback); diff --git a/ros/src/sensing/filters/packages/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp b/ros/src/sensing/filters/packages/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp index d1e229e8b0e..9afefcfa390 100644 --- a/ros/src/sensing/filters/packages/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp +++ b/ros/src/sensing/filters/packages/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp @@ -37,7 +37,7 @@ #include -#include +#include #include @@ -46,8 +46,8 @@ ros::Publisher filtered_points_pub; // Leaf size of VoxelGrid filter. static double voxel_leaf_size = 2.0; -static ros::Publisher points_filter_info_pub; -static points_filter::PointsFilterInfo points_filter_info_msg; +static ros::Publisher points_downsampler_info_pub; +static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; static std::chrono::time_point filter_start, filter_end; @@ -92,36 +92,36 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) filtered_msg.header = input->header; filtered_points_pub.publish(filtered_msg); - points_filter_info_msg.header = input->header; - points_filter_info_msg.filter_name = "voxel_grid_filter"; - points_filter_info_msg.original_points_size = scan.size(); + points_downsampler_info_msg.header = input->header; + points_downsampler_info_msg.filter_name = "voxel_grid_filter"; + points_downsampler_info_msg.original_points_size = scan.size(); if (voxel_leaf_size >= 0.1) { - points_filter_info_msg.filtered_points_size = filtered_scan_ptr->size(); + points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); } else { - points_filter_info_msg.filtered_points_size = scan_ptr->size(); + points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); } - points_filter_info_msg.original_ring_size = 0; - points_filter_info_msg.filtered_ring_size = 0; - points_filter_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_filter_info_pub.publish(points_filter_info_msg); + points_downsampler_info_msg.original_ring_size = 0; + points_downsampler_info_msg.filtered_ring_size = 0; + points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; + points_downsampler_info_pub.publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } - ofs << points_filter_info_msg.header.seq << "," - << points_filter_info_msg.header.stamp << "," - << points_filter_info_msg.header.frame_id << "," - << points_filter_info_msg.filter_name << "," - << points_filter_info_msg.original_points_size << "," - << points_filter_info_msg.filtered_points_size << "," - << points_filter_info_msg.original_ring_size << "," - << points_filter_info_msg.filtered_ring_size << "," - << points_filter_info_msg.exe_time << "," + ofs << points_downsampler_info_msg.header.seq << "," + << points_downsampler_info_msg.header.stamp << "," + << points_downsampler_info_msg.header.frame_id << "," + << points_downsampler_info_msg.filter_name << "," + << points_downsampler_info_msg.original_points_size << "," + << points_downsampler_info_msg.filtered_points_size << "," + << points_downsampler_info_msg.original_ring_size << "," + << points_downsampler_info_msg.filtered_ring_size << "," + << points_downsampler_info_msg.exe_time << "," << std::endl; } @@ -146,7 +146,7 @@ int main(int argc, char** argv) // Publishers filtered_points_pub = nh.advertise("/filtered_points", 10); - points_filter_info_pub = nh.advertise("/points_filter_info", 1000); + points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); // Subscribers ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback); diff --git a/ros/src/sensing/filters/packages/points_downsampler/package.xml b/ros/src/sensing/filters/packages/points_downsampler/package.xml index 58de0058dac..0b49d447140 100644 --- a/ros/src/sensing/filters/packages/points_downsampler/package.xml +++ b/ros/src/sensing/filters/packages/points_downsampler/package.xml @@ -1,8 +1,8 @@ - points_filter + points_downsampler 0.0.0 - The points_filter package + The points_downsampler package Yuki KITSUKAWA BSD diff --git a/ros/src/util/packages/runtime_manager/scripts/sensing.yaml b/ros/src/util/packages/runtime_manager/scripts/sensing.yaml index 47d6f42a204..5234dee1404 100644 --- a/ros/src/util/packages/runtime_manager/scripts/sensing.yaml +++ b/ros/src/util/packages/runtime_manager/scripts/sensing.yaml @@ -150,33 +150,33 @@ subs : cmds: subs : - - name : Points Filter - desc : Points Filter desc sample + - name : Points Downsampler + desc : Points Downsampler desc sample subs : - name : voxel_grid_filter desc : voxel_grid_filter desc sample - cmd : roslaunch points_filter filter.launch node_name:=voxel_grid_filter + cmd : roslaunch points_downsampler points_downsample.launch node_name:=voxel_grid_filter param: voxel_grid_filter gui : sync : func : self.button_synchronization.GetValue() - name : ring_filter desc : ring_filter desc sample - cmd : roslaunch points_filter filter.launch node_name:=ring_filter + cmd : roslaunch points_downsampler points_downsample.launch node_name:=ring_filter param: ring_filter gui : sync : func : self.button_synchronization.GetValue() - name : distance_filter desc : distance_filter desc sample - cmd : roslaunch points_filter filter.launch node_name:=distance_filter + cmd : roslaunch points_downsampler points_downsample.launch node_name:=distance_filter param: distance_filter gui : sync : func : self.button_synchronization.GetValue() - name : random_filter desc : random_filter desc sample - cmd : roslaunch points_filter filter.launch node_name:=random_filter + cmd : roslaunch points_downsampler points_downsample.launch node_name:=random_filter param: random_filter gui : sync :