diff --git a/pcl_ros/cfg/SACSegmentation_common.py b/pcl_ros/cfg/SACSegmentation_common.py index 9bf8da1c..0bef192b 100644 --- a/pcl_ros/cfg/SACSegmentation_common.py +++ b/pcl_ros/cfg/SACSegmentation_common.py @@ -1,8 +1,8 @@ #! /usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import int_t -from dynamic_reconfigure.parameter_generator_catkin import double_t from dynamic_reconfigure.parameter_generator_catkin import bool_t +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import int_t from dynamic_reconfigure.parameter_generator_catkin import str_t # set up parameters that we care about @@ -11,37 +11,37 @@ PACKAGE = 'pcl_ros' def add_common_parameters(gen): # add(self, name, paramtype, level, description, default = None, min = None, - # max = None, edit_method = "") - gen.add("max_iterations", int_t, 0, - "The maximum number of iterations the algorithm will run for", + # max = None, edit_method = '') + gen.add('max_iterations', int_t, 0, + 'The maximum number of iterations the algorithm will run for', 50, 0, 100000) - gen.add("probability", double_t, 0, - "The desired probability of choosing at least one sample free from outliers", + gen.add('probability', double_t, 0, + 'The desired probability of choosing at least one sample free from outliers', 0.99, 0.5, 0.99) - gen.add("distance_threshold", double_t, 0, - "The distance to model threshold", + gen.add('distance_threshold', double_t, 0, + 'The distance to model threshold', 0.02, 0, 1.0) - gen.add("optimize_coefficients", bool_t, 0, - "Model coefficient refinement", + gen.add('optimize_coefficients', bool_t, 0, + 'Model coefficient refinement', True) - gen.add("radius_min", double_t, 0, - "The minimum allowed model radius (where applicable)", + gen.add('radius_min', double_t, 0, + 'The minimum allowed model radius (where applicable)', 0.0, 0, 1.0) - gen.add("radius_max", double_t, 0, - "The maximum allowed model radius (where applicable)", + gen.add('radius_max', double_t, 0, + 'The maximum allowed model radius (where applicable)', 0.05, 0, 1.0) - gen.add("eps_angle", double_t, 0, - ("The maximum allowed difference between the model normal " - "and the given axis in radians."), + gen.add('eps_angle', double_t, 0, + ('The maximum allowed difference between the model normal ' + 'and the given axis in radians.'), 0.17, 0.0, 1.5707) - gen.add("min_inliers", int_t, 0, - "The minimum number of inliers a model must have in order to be considered valid.", + gen.add('min_inliers', int_t, 0, + 'The minimum number of inliers a model must have in order to be considered valid.', 0, 0, 100000) - gen.add("input_frame", str_t, 0, - ("The input TF frame the data should be transformed into, " - "if input.header.frame_id is different."), - "") - gen.add("output_frame", str_t, 0, - ("The output TF frame the data should be transformed into, " - "if input.header.frame_id is different."), - "") + gen.add('input_frame', str_t, 0, + ('The input TF frame the data should be transformed into, ' + 'if input.header.frame_id is different.'), + '') + gen.add('output_frame', str_t, 0, + ('The output TF frame the data should be transformed into, ' + 'if input.header.frame_id is different.'), + '') diff --git a/pcl_ros/cfg/common.py b/pcl_ros/cfg/common.py index 43c49554..27e47107 100644 --- a/pcl_ros/cfg/common.py +++ b/pcl_ros/cfg/common.py @@ -1,8 +1,8 @@ #! /usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import str_t -from dynamic_reconfigure.parameter_generator_catkin import double_t from dynamic_reconfigure.parameter_generator_catkin import bool_t +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import str_t # set up parameters that we care about PACKAGE = 'pcl_ros' @@ -10,27 +10,27 @@ PACKAGE = 'pcl_ros' def add_common_parameters(gen): # def add (self, name, paramtype, level, description, default = None, min = None, - # max = None, edit_method = ""): - gen.add("filter_field_name", str_t, 0, "The field name used for filtering", "z") - gen.add("filter_limit_min", double_t, 0, - "The minimum allowed field value a point will be considered from", + # max = None, edit_method = ''): + gen.add('filter_field_name', str_t, 0, 'The field name used for filtering', 'z') + gen.add('filter_limit_min', double_t, 0, + 'The minimum allowed field value a point will be considered from', 0.0, -100000.0, 100000.0) - gen.add("filter_limit_max", double_t, 0, - "The maximum allowed field value a point will be considered from", + gen.add('filter_limit_max', double_t, 0, + 'The maximum allowed field value a point will be considered from', 1.0, -100000.0, 100000.0) - gen.add("filter_limit_negative", bool_t, 0, - ("Set to true if we want to return the data outside " - "[filter_limit_min; filter_limit_max]."), + gen.add('filter_limit_negative', bool_t, 0, + ('Set to true if we want to return the data outside ' + '[filter_limit_min; filter_limit_max].'), False) - gen.add("keep_organized", bool_t, 0, - ("Set whether the filtered points should be kept and set to NaN, " - "or removed from the PointCloud, thus potentially breaking its organized structure."), + gen.add('keep_organized', bool_t, 0, + ('Set whether the filtered points should be kept and set to NaN, ' + 'or removed from the PointCloud, thus potentially breaking its organized structure.'), False) - gen.add("input_frame", str_t, 0, - ("The input TF frame the data should be transformed into before processing, " - "if input.header.frame_id is different."), - "") - gen.add("output_frame", str_t, 0, - ("The output TF frame the data should be transformed into after processing, " - "if input.header.frame_id is different."), - "") + gen.add('input_frame', str_t, 0, + ('The input TF frame the data should be transformed into before processing, ' + 'if input.header.frame_id is different.'), + '') + gen.add('output_frame', str_t, 0, + ('The output TF frame the data should be transformed into after processing, ' + 'if input.header.frame_id is different.'), + '') diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 31c0e1fe..1f92e648 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -37,7 +37,6 @@ #ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_ #define PCL_ROS__IMPL__TRANSFORMS_HPP_ -#include "pcl_ros/transforms.hpp" #include #include #include @@ -45,14 +44,18 @@ #include #include #include -#include #include #include + +#include +#include + +#include #include #include #include -#include -#include + +#include "pcl_ros/transforms.hpp" using pcl_conversions::fromPCL; using pcl_conversions::toPCL; diff --git a/pcl_ros/include/pcl_ros/io/bag_io.hpp b/pcl_ros/include/pcl_ros/io/bag_io.hpp index 6f5add21..bad98c7d 100644 --- a/pcl_ros/include/pcl_ros/io/bag_io.hpp +++ b/pcl_ros/include/pcl_ros/io/bag_io.hpp @@ -38,11 +38,11 @@ #ifndef PCL_ROS__IO__BAG_IO_HPP_ #define PCL_ROS__IO__BAG_IO_HPP_ -#include #include #include #include #include +#include namespace pcl_ros { diff --git a/pcl_ros/include/pcl_ros/pcl_node.hpp b/pcl_ros/include/pcl_ros/pcl_node.hpp index d80cb6f4..31be9944 100644 --- a/pcl_ros/include/pcl_ros/pcl_node.hpp +++ b/pcl_ros/include/pcl_ros/pcl_node.hpp @@ -44,30 +44,25 @@ #ifndef PCL_ROS__PCL_NODE_HPP_ #define PCL_ROS__PCL_NODE_HPP_ -#include -// PCL includes -#include -#include -#include -#include -#include -// ROS Nodelet includes -// #include // TODO(daisukes): lazy subscription #include #include #include #include - -// Include TF +#include +#include +#include #include #include +// #include // TODO(daisukes): lazy subscription -// STL #include #include #include -// ROS2 includes + #include +#include +#include +#include // #include "pcl_ros/point_cloud.hpp" @@ -199,9 +194,9 @@ protected: bool approximate_sync_; /** \brief TF listener object. */ - tf2_ros::TransformListener tf_listener_; tf2_ros::Buffer tf_buffer_; - + tf2_ros::TransformListener tf_listener_; + /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). * \param cloud the point cloud to test * \param topic_name an optional topic name (only used for printing, defaults to "input") @@ -249,7 +244,9 @@ protected: * \param topic_name an optional topic name (only used for printing, defaults to "indices") */ inline bool - isValid(const PointIndicesConstPtr & indices, const std::string & topic_name = "indices") + isValid( + const PointIndicesConstPtr & /*indices*/, + const std::string & /*topic_name*/ = "indices") { /*if (indices->indices.empty ()) { @@ -269,7 +266,9 @@ protected: * \param topic_name an optional topic name (only used for printing, defaults to "model") */ inline bool - isValid(const ModelCoefficientsConstPtr & model, const std::string & topic_name = "model") + isValid( + const ModelCoefficientsConstPtr & /*model*/, + const std::string & /*topic_name*/ = "model") { /*if (model->values.empty ()) { diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index b25697e1..b1da6b0a 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -44,12 +44,12 @@ #include #include -#include // for PCL_VERSION_COMPARE +#include // for PCL_VERSION_COMPARE #if PCL_VERSION_COMPARE(>=, 1, 11, 0) #include #else #include -#endif // PCL_VERSION_COMPARE(>=, 1, 11, 0) +#endif // PCL_VERSION_COMPARE(>=, 1, 11, 0) #include #include #if ROS_POINTER_COMPATIBILITY_IMPLEMENTED @@ -61,9 +61,6 @@ #endif #include #include -#include // for BOOST_FOREACH -#include -#include #include #include #include @@ -71,6 +68,9 @@ #include #include #endif +#include // for BOOST_FOREACH +#include +#include namespace pcl { diff --git a/pcl_ros/include/pcl_ros/transforms.hpp b/pcl_ros/include/pcl_ros/transforms.hpp index 1b5312f3..e1bd4a6b 100644 --- a/pcl_ros/include/pcl_ros/transforms.hpp +++ b/pcl_ros/include/pcl_ros/transforms.hpp @@ -41,11 +41,11 @@ #include #include #include +#include +#include #include #include #include -#include -#include namespace pcl_ros { diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index 5f379342..33985cbb 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -36,8 +36,8 @@ */ #include -#include #include +#include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void diff --git a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp index 980a7d6b..a32d7511 100644 --- a/pcl_ros/src/test/test_tf_message_filter_pcl.cpp +++ b/pcl_ros/src/test/test_tf_message_filter_pcl.cpp @@ -38,10 +38,6 @@ #include #include -#include -#include - -#include #include #include @@ -52,6 +48,11 @@ #include #include +#include +#include + +#include + // using a random point type, as we want to make sure that it does work with // other points than just XYZ typedef pcl::PointCloud PCDType; diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index 2ca2462a..d733708d 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -36,6 +36,7 @@ #include "pcl_ros/transforms.hpp" #include "pcl_ros/impl/transforms.hpp" + #include #include #include @@ -43,18 +44,20 @@ #include #include #include -#include #include #include -#include -#include -#include -#include + #include #include #include #include +#include +#include +#include +#include +#include + namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/tests/filters/test_filter_component.py b/pcl_ros/tests/filters/test_filter_component.py index 2c31983e..2e01e0e8 100644 --- a/pcl_ros/tests/filters/test_filter_component.py +++ b/pcl_ros/tests/filters/test_filter_component.py @@ -36,9 +36,8 @@ import launch.actions import launch_ros.actions import launch_testing.actions import launch_testing.markers -import pytest - from launch_testing_ros import WaitForTopics +import pytest from sensor_msgs.msg import PointCloud2 @@ -78,6 +77,7 @@ def generate_test_description(): class TestFilter(unittest.TestCase): + def test_filter_output(self): wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0) assert wait_for_topics.wait() diff --git a/pcl_ros/tests/filters/test_filter_executable.py b/pcl_ros/tests/filters/test_filter_executable.py index 11bcdb6d..b5e8e7c9 100644 --- a/pcl_ros/tests/filters/test_filter_executable.py +++ b/pcl_ros/tests/filters/test_filter_executable.py @@ -36,9 +36,8 @@ import launch.actions import launch_ros.actions import launch_testing.actions import launch_testing.markers -import pytest - from launch_testing_ros import WaitForTopics +import pytest from sensor_msgs.msg import PointCloud2 @@ -49,9 +48,9 @@ def generate_test_description(): filter_executable = os.getenv('FILTER_EXECUTABLE') parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {} - ros_arguments = ["-r", "input:=point_cloud2"] + ros_arguments = ['-r', 'input:=point_cloud2'] for key in parameters.keys(): - ros_arguments.extend(["-p", "{}:={}".format(key, parameters[key])]) + ros_arguments.extend(['-p', '{}:={}'.format(key, parameters[key])]) return launch.LaunchDescription([ @@ -72,7 +71,7 @@ def generate_test_description(): launch_ros.actions.Node( package='pcl_ros', executable=filter_executable, - output="screen", + output='screen', ros_arguments=ros_arguments ), launch_testing.actions.ReadyToTest() @@ -80,6 +79,7 @@ def generate_test_description(): class TestFilter(unittest.TestCase): + def test_filter_output(self): wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0) assert wait_for_topics.wait() diff --git a/pcl_ros/tools/bag_to_pcd.cpp b/pcl_ros/tools/bag_to_pcd.cpp index c895b801..a06d7305 100644 --- a/pcl_ros/tools/bag_to_pcd.cpp +++ b/pcl_ros/tools/bag_to_pcd.cpp @@ -49,11 +49,13 @@ Cloud Data) format. #include #include #include + #include #include -#include #include #include + +#include #include "pcl_ros/transforms.hpp" typedef sensor_msgs::PointCloud2 PointCloud; diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp index f8368f3d..cb21a24d 100644 --- a/pcl_ros/tools/pcd_to_pointcloud.cpp +++ b/pcl_ros/tools/pcd_to_pointcloud.cpp @@ -43,21 +43,18 @@ **/ -// STL +#include +#include +#include +#include + #include #include #include -// PCL -#include -#include -#include - -// ROS core #include #include "rclcpp_components/register_node_macro.hpp" #include -#include namespace pcl_ros {