From a3606cecae369eac08a8174dd70487c2ff4db23e Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Mon, 27 May 2024 15:38:17 +0200 Subject: [PATCH] final refactor --- .../config/scan_unifier_node.yaml | 1 - .../laser_scan_unifier/scan_unifier_node.hpp | 68 +-------------- laser_scan_unifier/src/scan_unifier_node.cpp | 82 +++++-------------- laser_scan_unifier/src/scan_unifier_node.yaml | 6 -- 4 files changed, 24 insertions(+), 133 deletions(-) diff --git a/laser_scan_unifier/config/scan_unifier_node.yaml b/laser_scan_unifier/config/scan_unifier_node.yaml index f5c56042a..d8d7e0ea4 100644 --- a/laser_scan_unifier/config/scan_unifier_node.yaml +++ b/laser_scan_unifier/config/scan_unifier_node.yaml @@ -7,5 +7,4 @@ scan_unifier_node: - laser_scan_left - laser_scan_back output_frame: base_link - publish_pointcloud: true inter_message_lower_bound: 0.167 diff --git a/laser_scan_unifier/include/laser_scan_unifier/scan_unifier_node.hpp b/laser_scan_unifier/include/laser_scan_unifier/scan_unifier_node.hpp index aafdd7cc3..0b5e7e693 100755 --- a/laser_scan_unifier/include/laser_scan_unifier/scan_unifier_node.hpp +++ b/laser_scan_unifier/include/laser_scan_unifier/scan_unifier_node.hpp @@ -18,12 +18,8 @@ #ifndef LASER_SCAN_UNIFIER__SCAN_UNIFIER_NODE_HPP_ #define LASER_SCAN_UNIFIER__SCAN_UNIFIER_NODE_HPP_ -//################## -//#### includes #### - // standard includes #include -//#include #include #include "scan_unifier_node_parameters.hpp" @@ -44,40 +40,18 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" #include "sensor_msgs/msg/laser_scan.hpp" - typedef message_filters::sync_policies::ApproximateTime Sync2Policy; typedef message_filters::sync_policies::ApproximateTime Sync3Policy; typedef message_filters::sync_policies::ApproximateTime Sync4Policy; -//#################### -//#### node class #### + class ScanUnifierNode : public rclcpp::Node { private: - /** @struct config_struct - * @brief This structure holds configuration parameters - * @var config_struct::number_input_scans - * Member 'number_input_scans' contains number of scanners to subscribe to - * @var config_struct::loop_rate - * Member 'loop_rate' contains the loop rate of the ros node - * @var config_struct::input_scan_topics - * Member 'input_scan_topics' contains the names of the input scan topics - */ - std::shared_ptr param_listener_; scan_unifier_node::Params params_; - struct config_struct{ - size_t number_input_scans; - std::vector input_scan_topics; - bool publish_pointcloud = false; - }; - config_struct config_; - std::string frame_; - std::vector>> message_filter_subscribers_; - - void sync2FilterCallback(const sensor_msgs::msg::LaserScan::SharedPtr& first_scanner, const sensor_msgs::msg::LaserScan::SharedPtr& second_scanner); void sync3FilterCallback(const sensor_msgs::msg::LaserScan::SharedPtr& first_scanner, @@ -88,58 +62,24 @@ class ScanUnifierNode : public rclcpp::Node const sensor_msgs::msg::LaserScan::SharedPtr& third_scanner, const sensor_msgs::msg::LaserScan::SharedPtr& fourth_scanner); - - std::shared_ptr> synchronizer2_; std::shared_ptr> synchronizer3_; std::shared_ptr> synchronizer4_; public: - //constructor ScanUnifierNode(); - /* ----------------------------------- */ - /* --------- ROS Variables ----------- */ - /* ----------------------------------- */ - // create publishers + // publishers & subscribers rclcpp::Publisher::SharedPtr topicPub_LaserUnified_; - rclcpp::Publisher::SharedPtr topicPub_PointCloudUnified_; - - // tf listener std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; - // laser geometry projector + // other variables laser_geometry::LaserProjection projector_; - - std::vector vec_cloud_; - - - /* ----------------------------------- */ - /* ----------- functions ------------- */ - /* ----------------------------------- */ - - /** - * @function getParams - * @brief function to load parameters from ros parameter server - * - * input: - - * output: - - */ - + void getParams(); - - - /** - * @function unifyLaserScans - * @brief unify the scan information from all laser scans in vec_laser_struct_ - * - * input: - - * output: - * @param: a laser scan message containing unified information from all scanners - */ void publish(sensor_msgs::msg::LaserScan& unified_scan); bool unifyLaserScans(const std::vector& current_scans, sensor_msgs::msg::LaserScan& unified_scan); diff --git a/laser_scan_unifier/src/scan_unifier_node.cpp b/laser_scan_unifier/src/scan_unifier_node.cpp index 07d8d5775..f67e1148b 100755 --- a/laser_scan_unifier/src/scan_unifier_node.cpp +++ b/laser_scan_unifier/src/scan_unifier_node.cpp @@ -25,35 +25,29 @@ ScanUnifierNode::ScanUnifierNode() auto logger_ = this->get_logger(); RCLCPP_DEBUG(logger_, "Initializing scan_unifier_node..."); + getParams(); + synchronizer2_ = NULL; synchronizer3_ = NULL; synchronizer4_ = NULL; - getParams(); - // Initialize TF buffer and listener tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); // Publisher topicPub_LaserUnified_ = this->create_publisher("scan_unified", 1); - if(config_.publish_pointcloud) - { - topicPub_PointCloudUnified_ = this->create_publisher("pointcloud_unified", 1); - } // Subscribe to Laserscan topics auto subscriber_options = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); subscriber_options.reliability(rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); - for(size_t i = 0; i < config_.number_input_scans; i++){ - // Store the subscriber shared_ptr in a vector if needed for later access - auto sub = std::make_shared>(this, config_.input_scan_topics[i], subscriber_options.get_rmw_qos_profile()); + for(size_t i = 0; i < params_.number_of_scans; i++){ + auto sub = std::make_shared>(this, params_.input_topic_names[i], subscriber_options.get_rmw_qos_profile()); message_filter_subscribers_.push_back(sub); } - // Initialize message_filters::Synchronizer with the right constructor for the choosen number of inputs. - switch (config_.number_input_scans) + switch (params_.number_of_scans) { case 2: { @@ -92,22 +86,15 @@ ScanUnifierNode::ScanUnifierNode() break; } default: - RCLCPP_ERROR_STREAM(logger_, config_.number_input_scans << " topics have been set as input, but scan_unifier supports between 2 and 4 topics."); + RCLCPP_ERROR_STREAM(logger_, params_.number_of_scans << " topics have been set as input, but scan_unifier supports between 2 and 4 topics."); return; } + RCLCPP_DEBUG(logger_, "Initialization complete."); rclcpp::sleep_for(std::chrono::seconds(1)); } -/** - * @function unifyLaserScans - * @brief unify the scan information from all laser scans in vec_laser_struct_ - * - * input: - - * output: - * @param: a laser scan message containing unified information from all scanners - */ bool ScanUnifierNode::unifyLaserScans(const std::vector& current_scans, sensor_msgs::msg::LaserScan& unified_scan) { @@ -117,26 +104,26 @@ bool ScanUnifierNode::unifyLaserScans(const std::vectorget_logger(); RCLCPP_DEBUG(logger_, "Starting conversion..."); - for(size_t i=0; i < config_.number_input_scans; i++) + for(size_t i=0; i < params_.number_of_scans; i++) { RCLCPP_DEBUG(logger_, " - project to PointCloud2"); projector_.projectLaser(*current_scans[i], vec_cloud_[i]); - // Transform cloud if necessary - if (!frame_.empty() &&vec_cloud_[i].header.frame_id != frame_) { + // Transform cloud if necessary + if (!params_.output_frame.empty() &&vec_cloud_[i].header.frame_id != params_.output_frame) { try { auto cloud = std::make_shared(); - tf_buffer_->transform(vec_cloud_[i], *cloud, frame_, tf2::durationFromSec(0.1)); //make into parameter + tf_buffer_->transform(vec_cloud_[i], *cloud, params_.output_frame, tf2::durationFromSec(0.1)); //make into parameter vec_cloud_[i] = *cloud; } catch (tf2::TransformException & ex) @@ -151,7 +138,7 @@ bool ScanUnifierNode::unifyLaserScans(const std::vectorheader; - unified_scan.header.frame_id = frame_; + unified_scan.header.frame_id = params_.output_frame; unified_scan.angle_increment = std::abs(current_scans.front()->angle_increment); unified_scan.angle_min = static_cast(-M_PI + unified_scan.angle_increment*0.01); unified_scan.angle_max = static_cast(M_PI - unified_scan.angle_increment*0.01); @@ -166,7 +153,7 @@ bool ScanUnifierNode::unifyLaserScans(const std::vector iterX(vec_cloud_[j], "x"); @@ -179,7 +166,6 @@ bool ScanUnifierNode::unifyLaserScans(const std::vectorget_logger(), "Publishing unified scan."); topicPub_LaserUnified_->publish(unified_scan); - /* extract pointcloud transform into a separate function - if(config_.publish_pointcloud) - { - auto unified_pointcloud = sensor_msgs::msg::PointCloud2(); - projector_.transformLaserScanToPointCloud(frame_, unified_scan, unified_pointcloud, tf_buffer_core_); - topicPub_PointCloudUnified_->publish(unified_pointcloud); - } - */ return; } @@ -232,10 +208,7 @@ void ScanUnifierNode::sync2FilterCallback(const sensor_msgs::msg::LaserScan::Sha auto unified_scan = sensor_msgs::msg::LaserScan(); - if (!unifyLaserScans(current_scans, unified_scan)) - { - return; - } + if (!unifyLaserScans(current_scans, unified_scan)) return; publish(unified_scan); return; } @@ -251,10 +224,7 @@ void ScanUnifierNode::sync3FilterCallback(const sensor_msgs::msg::LaserScan::Sha auto unified_scan = sensor_msgs::msg::LaserScan(); - if (!unifyLaserScans(current_scans, unified_scan)) - { - return; - } + if (!unifyLaserScans(current_scans, unified_scan)) return; publish(unified_scan); return; } @@ -266,21 +236,13 @@ void ScanUnifierNode::sync4FilterCallback(const sensor_msgs::msg::LaserScan::Sha { std::vector current_scans; current_scans.push_back(scan1); - RCLCPP_DEBUG(this->get_logger(), "Scan 1 frame_id: %s", scan1->header.frame_id.c_str()); current_scans.push_back(scan2); - RCLCPP_DEBUG(this->get_logger(), "Scan 2 frame_id: %s", scan2->header.frame_id.c_str()); current_scans.push_back(scan3); - RCLCPP_DEBUG(this->get_logger(), "Scan 3 frame_id: %s", scan3->header.frame_id.c_str()); current_scans.push_back(scan4); - RCLCPP_DEBUG(this->get_logger(), "Scan 4 frame_id: %s", scan4->header.frame_id.c_str()); auto unified_scan = sensor_msgs::msg::LaserScan(); - if (!unifyLaserScans(current_scans, unified_scan)) - { - RCLCPP_DEBUG(this->get_logger(), "returning..." ); - return; - } + if (!unifyLaserScans(current_scans, unified_scan)) return; publish(unified_scan); return; } @@ -291,16 +253,12 @@ void ScanUnifierNode::getParams() param_listener_ = std::make_shared(this->get_node_parameters_interface()); params_ = param_listener_->get_params(); - RCLCPP_DEBUG(logger_, "Parameters read."); - config_.number_input_scans=static_cast(params_.number_of_scans); - config_.input_scan_topics = params_.input_topic_names; - RCLCPP_DEBUG(logger_, "Number of input scans: %ld", config_.number_input_scans); + RCLCPP_DEBUG(logger_, "Parameters read. Number of input scans: %ld", params_.number_of_scans); RCLCPP_DEBUG(logger_, "Topic names:"); - for (auto &&topic : config_.input_scan_topics) + for (auto &&topic : params_.input_topic_names) { RCLCPP_DEBUG(logger_, " /%s", topic.c_str()); } - frame_ = params_.output_frame; } diff --git a/laser_scan_unifier/src/scan_unifier_node.yaml b/laser_scan_unifier/src/scan_unifier_node.yaml index d9aabb00f..e8b0ece01 100644 --- a/laser_scan_unifier/src/scan_unifier_node.yaml +++ b/laser_scan_unifier/src/scan_unifier_node.yaml @@ -33,12 +33,6 @@ scan_unifier_node: description: "Frame in which the output LaserScan is published", read_only: true, } - publish_pointcloud: { - type: bool, - default_value: true, - description: "If true, publishes PointCloud2 msg from which the final, unified scan is converted into.", - read_only: true, - } inter_message_lower_bound: { type: double, default_value: 0.167,