From 709c9a27922d4fc97a8b22d12b6d2890995c11d8 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 21 May 2019 23:11:04 +0200 Subject: [PATCH 1/3] Added nodelet version of scan_to_cloud_filter_chain . --- CMakeLists.txt | 9 ++++- nodelets.xml | 7 ++++ package.xml | 3 ++ src/scan_to_cloud_filter_chain.cpp | 61 ++++++++++++++++++++++-------- 4 files changed, 62 insertions(+), 18 deletions(-) create mode 100644 nodelets.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 9af5eefd..abb72fac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ project(laser_filters) ############################################################################## set(THIS_PACKAGE_ROS_DEPS sensor_msgs roscpp tf filters message_filters - laser_geometry pluginlib angles) + laser_geometry pluginlib angles nodelet) find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS}) find_package(Boost REQUIRED COMPONENTS system signals) @@ -36,6 +36,10 @@ target_link_libraries(laser_scan_filters ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(scan_to_cloud_filter_chain src/scan_to_cloud_filter_chain.cpp) target_link_libraries(scan_to_cloud_filter_chain ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_library(scan_to_cloud_filter_chain_nodelet src/scan_to_cloud_filter_chain.cpp) +target_link_libraries(scan_to_cloud_filter_chain_nodelet ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_compile_definitions(scan_to_cloud_filter_chain_nodelet PRIVATE BUILDING_NODELET=1) + add_executable(scan_to_scan_filter_chain src/scan_to_scan_filter_chain.cpp) target_link_libraries(scan_to_scan_filter_chain ${catkin_LIBRARIES} ${Boost_LIBRARIES}) @@ -60,6 +64,7 @@ endif() install(TARGETS pointcloud_filters laser_scan_filters scan_to_cloud_filter_chain + scan_to_cloud_filter_chain_nodelet scan_to_scan_filter_chain generic_laser_filter_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -71,6 +76,6 @@ install(TARGETS pointcloud_filters laser_scan_filters install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES laser_filters_plugins.xml +install(FILES laser_filters_plugins.xml nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/nodelets.xml b/nodelets.xml new file mode 100644 index 00000000..36184af7 --- /dev/null +++ b/nodelets.xml @@ -0,0 +1,7 @@ + + + + Convert laser scans to pointclouds allowing to filter both as a scan and as a point cloud. + + + \ No newline at end of file diff --git a/package.xml b/package.xml index 82ff7da7..3b4f20a5 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ pluginlib rostest angles + nodelet sensor_msgs roscpp @@ -31,9 +32,11 @@ laser_geometry pluginlib angles + nodelet + diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index 52a0fc5b..f974400c 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -51,6 +51,17 @@ //Filters #include "filters/filter_chain.h" +#if BUILDING_NODELET +#include +#include + +#define LASER_INFO NODELET_INFO +#define LASER_WARN NODELET_WARN +#else +#define LASER_INFO ROS_INFO +#define LASER_WARN ROS_WARN +#endif + /** @b ScanShadowsFilter is a simple node that filters shadow points in a laser scan line and publishes the results in a cloud. */ class ScanToCloudFilterChain @@ -69,6 +80,7 @@ class ScanToCloudFilterChain ros::NodeHandle nh; ros::NodeHandle private_nh; + std::string name_; // TF tf::TransformListener tf_; @@ -95,8 +107,9 @@ class ScanToCloudFilterChain bool incident_angle_correction_; //////////////////////////////////////////////////////////////////////////////// - ScanToCloudFilterChain () : laser_max_range_ (DBL_MAX), private_nh("~"), filter_(tf_, "", 50), - cloud_filter_chain_("sensor_msgs::PointCloud2"), scan_filter_chain_("sensor_msgs::LaserScan") + ScanToCloudFilterChain (ros::NodeHandle& nh, const std::string& name) : + laser_max_range_ (DBL_MAX), private_nh(nh), name_(name), filter_(tf_, "", 50), + cloud_filter_chain_("sensor_msgs::PointCloud2"), scan_filter_chain_("sensor_msgs::LaserScan") { private_nh.param("high_fidelity", high_fidelity_, false); private_nh.param("notifier_tolerance", tf_tolerance_, 0.03); @@ -155,40 +168,46 @@ class ScanToCloudFilterChain scan_filter_chain_.configure("scan_filter_chain", private_nh); deprecation_timer_ = nh.createTimer(ros::Duration(5.0), boost::bind(&ScanToCloudFilterChain::deprecation_warn, this, _1)); + + LASER_INFO("Scan to cloud filter initialized."); } // We use a deprecation warning on a timer to avoid warnings getting lost in the noise void deprecation_warn(const ros::TimerEvent& e) { if (using_scan_topic_deprecated_) - ROS_WARN("Use of '~scan_topic' parameter in scan_to_cloud_filter_chain has been deprecated."); + LASER_WARN("Use of '~scan_topic' parameter in scan_to_cloud_filter_chain has been deprecated."); if (using_cloud_topic_deprecated_) - ROS_WARN("Use of '~cloud_topic' parameter in scan_to_cloud_filter_chain has been deprecated."); + LASER_WARN("Use of '~cloud_topic' parameter in scan_to_cloud_filter_chain has been deprecated."); if (using_laser_max_range_deprecated_) - ROS_WARN("Use of '~laser_max_range' parameter in scan_to_cloud_filter_chain has been deprecated."); + LASER_WARN("Use of '~laser_max_range' parameter in scan_to_cloud_filter_chain has been deprecated."); if (using_filter_window_deprecated_) - ROS_WARN("Use of '~filter_window' parameter in scan_to_cloud_filter_chain has been deprecated."); + LASER_WARN("Use of '~filter_window' parameter in scan_to_cloud_filter_chain has been deprecated."); if (using_default_target_frame_deprecated_) - ROS_WARN("Use of default '~target_frame' parameter in scan_to_cloud_filter_chain has been deprecated. Default currently set to 'base_link' please set explicitly as appropriate."); + LASER_WARN("Use of default '~target_frame' parameter in scan_to_cloud_filter_chain has been deprecated. Default currently set to 'base_link' please set explicitly as appropriate."); if (using_cloud_filters_deprecated_) - ROS_WARN("Use of '~cloud_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~cloud_filter_chain'"); + LASER_WARN("Use of '~cloud_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~cloud_filter_chain'"); if (using_scan_filters_deprecated_) - ROS_WARN("Use of '~scan_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~scan_filter_chain'"); + LASER_WARN("Use of '~scan_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~scan_filter_chain'"); if (using_cloud_filters_wrong_deprecated_) - ROS_WARN("Use of '~cloud_filters/cloud_filter_chain' parameter in scan_to_cloud_filter_chain is incorrect. Please Replace with '~cloud_filter_chain'"); + LASER_WARN("Use of '~cloud_filters/cloud_filter_chain' parameter in scan_to_cloud_filter_chain is incorrect. Please Replace with '~cloud_filter_chain'"); if (using_scan_filters_wrong_deprecated_) - ROS_WARN("Use of '~scan_filters/scan_filter_chain' parameter in scan_to_scan_filter_chain is incorrect. Please Replace with '~scan_filter_chain'"); + LASER_WARN("Use of '~scan_filters/scan_filter_chain' parameter in scan_to_scan_filter_chain is incorrect. Please Replace with '~scan_filter_chain'"); } + std::string getName() const { + return this->name_; + } + //////////////////////////////////////////////////////////////////////////////// void @@ -227,7 +246,7 @@ class ScanToCloudFilterChain } catch (tf::TransformException &ex) { - ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str (), ex.what ()); + LASER_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str (), ex.what ()); return; //projector_.projectLaser (filtered_scan, scan_cloud, laser_max_range_, preservative_, mask); } @@ -245,18 +264,28 @@ class ScanToCloudFilterChain } ; +#if BUILDING_NODELET +class ScanToCloudFilterChainNodelet : public nodelet::Nodelet +{ + std::unique_ptr chain_; + void onInit() override { + chain_ = std::make_unique(getPrivateNodeHandle(), getName()); + } +}; + +PLUGINLIB_EXPORT_CLASS(ScanToCloudFilterChainNodelet, nodelet::Nodelet) +#else int main (int argc, char** argv) { ros::init (argc, argv, "scan_to_cloud_filter_chain"); - ros::NodeHandle nh; - ScanToCloudFilterChain f; + ros::NodeHandle nh, pnh("~"); + ScanToCloudFilterChain f(pnh, ""); ros::spin(); return (0); } - - +#endif \ No newline at end of file From 3fb957dbbb477cfe438775166976d6682054545b Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 23 May 2019 23:30:02 +0200 Subject: [PATCH 2/3] Style nitpicks. --- nodelets.xml | 2 +- src/scan_to_cloud_filter_chain.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nodelets.xml b/nodelets.xml index 36184af7..6d554f2c 100644 --- a/nodelets.xml +++ b/nodelets.xml @@ -4,4 +4,4 @@ Convert laser scans to pointclouds allowing to filter both as a scan and as a point cloud. - \ No newline at end of file + diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index f974400c..f2ef8042 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -107,8 +107,8 @@ class ScanToCloudFilterChain bool incident_angle_correction_; //////////////////////////////////////////////////////////////////////////////// - ScanToCloudFilterChain (ros::NodeHandle& nh, const std::string& name) : - laser_max_range_ (DBL_MAX), private_nh(nh), name_(name), filter_(tf_, "", 50), + ScanToCloudFilterChain (ros::NodeHandle& pnh, const std::string& name) : + laser_max_range_ (DBL_MAX), private_nh(pnh), name_(name), filter_(tf_, "", 50), cloud_filter_chain_("sensor_msgs::PointCloud2"), scan_filter_chain_("sensor_msgs::LaserScan") { private_nh.param("high_fidelity", high_fidelity_, false); @@ -281,11 +281,11 @@ int main (int argc, char** argv) { ros::init (argc, argv, "scan_to_cloud_filter_chain"); - ros::NodeHandle nh, pnh("~"); + ros::NodeHandle pnh("~"); ScanToCloudFilterChain f(pnh, ""); ros::spin(); return (0); } -#endif \ No newline at end of file +#endif From 3f31382b5c671a32e256d480193cf818169a0484 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 29 Jun 2021 12:41:18 +0200 Subject: [PATCH 3/3] Lowered C++ standard to C++11 --- src/scan_to_cloud_filter_chain.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index b07a6d71..6aeac38e 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -287,7 +287,7 @@ class ScanToCloudFilterChainNodelet : public nodelet::Nodelet std::unique_ptr chain_; void onInit() override { - chain_ = std::make_unique(getPrivateNodeHandle(), getName()); + chain_ = std::unique_ptr(new ScanToCloudFilterChain(getPrivateNodeHandle(), getName())); } };