From 4aefec2800fbc449b4574fec756d5e08f7820597 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 16 Jul 2024 22:45:04 +0000 Subject: [PATCH] Sort up ROS parameters for filter chains --- src/scan_to_cloud_filter_chain.cpp | 23 ++++++++++++----------- src/scan_to_cloud_filter_chain.hpp | 13 +++++-------- src/scan_to_scan_filter_chain.cpp | 15 +++++++++++---- src/scan_to_scan_filter_chain.hpp | 5 ++++- 4 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index 66a4e89..0ea40fe 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -50,21 +50,22 @@ ScanToCloudFilterChain::ScanToCloudFilterChain( cloud_filter_chain_("sensor_msgs::msg::PointCloud2"), scan_filter_chain_("sensor_msgs::msg::LaserScan") { - this->declare_parameter("high_fidelity", false); - this->declare_parameter("notifier_tolerance", 0.03); - this->declare_parameter("target_frame", std::string("base_link")); - this->declare_parameter("incident_angle_correction", true); - + rcl_interfaces::msg::ParameterDescriptor read_only_desc; + read_only_desc.read_only = true; + + // Declare parameters + this->declare_parameter("high_fidelity", false, read_only_desc); + this->declare_parameter("notifier_tolerance", 0.03, read_only_desc); + this->declare_parameter("target_frame", "base_link", read_only_desc); + this->declare_parameter("incident_angle_correction", true, read_only_desc); + this->declare_parameter("laser_max_range", DBL_MAX, read_only_desc); + + // Get parameters this->get_parameter("high_fidelity", high_fidelity_); this->get_parameter("notifier_tolerance", tf_tolerance_); this->get_parameter("target_frame", target_frame_); this->get_parameter("incident_angle_correction", incident_angle_correction_); - - this->get_parameter_or("filter_window", window_, 2); - this->get_parameter_or("laser_max_range", laser_max_range_, DBL_MAX); - this->get_parameter_or("scan_topic", scan_topic_, std::string("tilt_scan")); - this->get_parameter_or("cloud_topic", cloud_topic_, std::string("tilt_laser_cloud_filtered")); - + this->get_parameter("laser_max_range", laser_max_range_); filter_.setTargetFrame(target_frame_); filter_.registerCallback( diff --git a/src/scan_to_cloud_filter_chain.hpp b/src/scan_to_cloud_filter_chain.hpp index 0b6955c..450848b 100644 --- a/src/scan_to_cloud_filter_chain.hpp +++ b/src/scan_to_cloud_filter_chain.hpp @@ -58,13 +58,6 @@ class ScanToCloudFilterChain : public rclcpp::Node // ROS related laser_geometry::LaserProjection projector_; // Used to project laser scans - double laser_max_range_; // Used in laser scan projection - int window_; - - bool high_fidelity_; // High fidelity (interpolating time across scan) - std::string target_frame_; // Target frame for high fidelity result - std::string scan_topic_, cloud_topic_; - // TF tf2_ros::Buffer buffer_; tf2_ros::TransformListener tf_; @@ -72,12 +65,16 @@ class ScanToCloudFilterChain : public rclcpp::Node message_filters::Subscriber sub_; tf2_ros::MessageFilter filter_; - double tf_tolerance_; filters::FilterChain cloud_filter_chain_; filters::FilterChain scan_filter_chain_; rclcpp::Publisher::SharedPtr cloud_pub_; + // Parameters + bool high_fidelity_; // High fidelity (interpolating time across scan) + double tf_tolerance_; + std::string target_frame_; // Target frame for high fidelity result bool incident_angle_correction_; + double laser_max_range_; // Used in laser scan projection ScanToCloudFilterChain( const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), diff --git a/src/scan_to_scan_filter_chain.cpp b/src/scan_to_scan_filter_chain.cpp index 45fa445..582b670 100644 --- a/src/scan_to_scan_filter_chain.cpp +++ b/src/scan_to_scan_filter_chain.cpp @@ -47,17 +47,24 @@ ScanToScanFilterChain::ScanToScanFilterChain( "", this->get_node_logging_interface(), this->get_node_parameters_interface()); - std::string tf_message_filter_target_frame; - if (this->get_parameter("tf_message_filter_target_frame", tf_message_filter_target_frame)) { + rcl_interfaces::msg::ParameterDescriptor read_only_desc; + read_only_desc.read_only = true; - this->get_parameter_or("tf_message_filter_tolerance", tf_filter_tolerance_, 0.03); + // Declare parameters + this->declare_parameter("tf_message_filter_target_frame", "", read_only_desc); + this->declare_parameter("tf_message_filter_tolerance", 0.03, read_only_desc); + // Get parameters + this->get_parameter("tf_message_filter_target_frame", tf_message_filter_target_frame_); + this->get_parameter("tf_message_filter_tolerance", tf_filter_tolerance_); + + if (!tf_message_filter_target_frame_.empty()) { tf_.reset(new tf2_ros::TransformListener(buffer_)); tf_filter_.reset( new tf2_ros::MessageFilter( scan_sub_, buffer_, "", 50, this->shared_from_this())); - tf_filter_->setTargetFrame(tf_message_filter_target_frame); + tf_filter_->setTargetFrame(tf_message_filter_target_frame_); tf_filter_->setTolerance(std::chrono::duration(tf_filter_tolerance_)); // Setup tf::MessageFilter generates callback diff --git a/src/scan_to_scan_filter_chain.hpp b/src/scan_to_scan_filter_chain.hpp index 711965d..a7501cb 100644 --- a/src/scan_to_scan_filter_chain.hpp +++ b/src/scan_to_scan_filter_chain.hpp @@ -49,7 +49,6 @@ class ScanToScanFilterChain : public rclcpp::Node message_filters::Subscriber scan_sub_; std::shared_ptr> tf_filter_; - double tf_filter_tolerance_; // Filter Chain filters::FilterChain filter_chain_; @@ -58,6 +57,10 @@ class ScanToScanFilterChain : public rclcpp::Node sensor_msgs::msg::LaserScan msg_; rclcpp::Publisher::SharedPtr output_pub_; + // Parameters + std::string tf_message_filter_target_frame_; + double tf_filter_tolerance_; + public: // Constructor ScanToScanFilterChain(