diff --git a/laser_scan_unifier/CHANGELOG.rst b/laser_scan_unifier/CHANGELOG.rst new file mode 100644 index 000000000..cb05e7e1a --- /dev/null +++ b/laser_scan_unifier/CHANGELOG.rst @@ -0,0 +1,205 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cob_scan_unifier +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.7.17 (2024-04-18) +------------------- + +0.8.17 (2024-04-30) +------------------- +* 0.7.17 +* update changelogs +* Contributors: fmessmer + +0.7.16 (2024-02-20) +------------------- +* Merge pull request `#443 `_ from mbeutelspacher/fix/scan_unifier_negative_angle_increment + fix(cob_scan_unifier): fix handeling of negative angle_increments +* fix(cob_scan_unifier): fix handeling of negative angle_increments + laser scanners that are upside down often have negative + angle_increments (and also angle_max < angle_min). + Since we use angle_max=M_PI angle_min=-M_PI in the unified scan here, we + can't just copy the angle increment but need to use the absolute value + of angle_increments +* Contributors: Felix Messmer, Max Beutelspacher + +0.7.15 (2023-11-06) +------------------- +* Merge pull request `#440 `_ from benmaidel/feature/angle_increment_independend + remove hardcoded angle_increment value +* make cob_scan_unifier angle_increment independet by using the info from the input scan +* Contributors: Benjamin Maidel + +0.7.14 (2022-11-17) +------------------- + +0.7.13 (2022-07-29) +------------------- + +0.7.12 (2022-03-15) +------------------- + +0.7.11 (2022-01-12) +------------------- + +0.7.10 (2021-12-23) +------------------- + +0.7.9 (2021-11-26) +------------------ + +0.7.8 (2021-10-19) +------------------ +* Merge pull request `#424 `_ from lpk1950/publish_unified_points + Publish unified points +* final touches +* rename point_cloud to pointcloud +* Changes based on the review +* Changes on review: Separate publisher +* Format +* Format +* check for the point_cloud param and set to false by default +* publish pointcloud only when the parameter is set +* change in topic name +* publish unified scan as unified pointcloud +* Contributors: Felix Messmer, fmessmer, karthik + +0.7.7 (2021-08-02) +------------------ + +0.7.6 (2021-05-10) +------------------ + +0.7.5 (2021-04-06) +------------------ +* Merge pull request `#420 `_ from lindemeier/feature/improve_performance + performance boost +* performance boost +* Merge pull request `#418 `_ from fmessmer/fix_catkin_lint + fix catkin_lint +* fix catkin_lint +* Contributors: Felix Messmer, fmessmer, tsl + +0.7.4 (2020-10-14) +------------------ +* Merge pull request `#417 `_ from fmessmer/test_noetic + test noetic +* Bump CMake version to avoid CMP0048 warning +* Contributors: Felix Messmer, fmessmer + +0.7.3 (2020-03-18) +------------------ + +0.7.2 (2020-03-18) +------------------ +* Merge pull request `#408 `_ from fmessmer/ci_updates + [travis] ci updates +* catkin_lint fixes +* Contributors: Felix Messmer, fmessmer + +0.7.1 (2019-11-07) +------------------ + +0.7.0 (2019-08-06) +------------------ +* Merge pull request `#396 `_ from HannesBachter/indigo_dev + 0.6.15 +* Contributors: Felix Messmer + +0.6.15 (2019-07-17) +------------------- + +0.6.14 (2019-06-07) +------------------- + +0.6.13 (2019-03-14) +------------------- +* Merge pull request `#385 `_ from fmessmer/scan_unifier_range_initialization + initialize ranges with max_range rather than zero +* add explanatory comment +* initialize ranges with max_range rather than zero +* Contributors: Felix Messmer, fmessmer + +0.6.12 (2018-07-21) +------------------- +* update maintainer +* Merge pull request `#366 `_ from ipa-bnm/feature/scan_unifier + merge up to 4 laserscans +* merge up to 4 laserscans +* Contributors: Benjamin Maidel, Richard Bormann, fmessmer + +0.6.11 (2018-01-07) +------------------- +* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev +* Merge pull request `#353 `_ from ipa-fxm/update_maintainer + update maintainer +* update maintainer +* Merge pull request `#341 `_ from ipa-fxm/APACHE_license + use license apache 2.0 +* use license apache 2.0 +* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk + +0.6.10 (2017-07-24) +------------------- + +0.6.9 (2017-07-18) +------------------ +* remove commented line +* Added sleep in constructor, new topic parameter parsing, better error handling. +* Some small fixes +* Cleanup +* Use message_filter::Synchronizer (there is still a bug) +* manually fix changelog +* Contributors: Elias Marks, Matthias Gruhler, ipa-fxm + +0.6.8 (2016-10-10) +------------------ + +0.6.7 (2016-04-02) +------------------ + +0.6.6 (2016-04-01) +------------------ +* changed scan_unifier maintainer +* filtered out scan unifier and moved to new directory +* Contributors: Benjamin Maidel + +0.6.3 (2015-08-31) +------------------ +* remove trailing whitespace +* migration to package format v2, indentation fixes +* Merge remote-tracking branch 'origin-ipa320/hydro_dev' into indigo_dev +* reduced MAGIC NUMBER +* check range values +* round index +* Contributors: ipa-josh, ipa-mig + +0.6.2 (2015-06-17) +------------------ +* cob_scan_unifier: get rid of exported but uninstalled include path +* cob_scan_unifier: fix include folder stuff +* Contributors: ipa-mig + +0.6.1 (2014-09-18) +------------------ + +0.6.0 (2014-09-10) +------------------ + +0.5.2 (2014-08-28) +------------------ +* add changelog +* cob_scan_unifier: fix laser projection. wrong parameter +* cob_scan_unifier: added intensities to unified scan and use the nearest range measurement from all incoming scan +* adjusted license header in cob_scan_unifier +* updated license tag in cob_scan_unifier +* another indentation-fix-attempt +* merge +* removed start_delay from scan-unifier configs and intendation-fix +* Update scan_unifier_node.h + fixed intendation +* renamed ipa_navigation_scan_uniffier to cob_scan_unifier +* Contributors: Florian Mirus + +0.5.1 (2014-03-24) +------------------ diff --git a/laser_scan_unifier/CMakeLists.txt b/laser_scan_unifier/CMakeLists.txt new file mode 100755 index 000000000..a6976015c --- /dev/null +++ b/laser_scan_unifier/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(laser_scan_unifier) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra + -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow + -Wno-float-conversion -Wno-sign-compare) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + laser_geometry + rclcpp + sensor_msgs + tf2_ros + tf2_sensor_msgs + tf2_geometry_msgs + tf2 +) +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# Add library for the header file +add_library( + laser_scan_unifier + SHARED + src/scan_unifier_node.cpp +) +# Add executable +add_executable(scan_unifier_node src/scan_unifier_node.cpp) + +target_include_directories( + laser_scan_unifier + PUBLIC + $ + $ +) +ament_target_dependencies( + laser_scan_unifier PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +# Link the library to the executable +target_link_libraries(scan_unifier_node laser_scan_unifier) + + +install( + DIRECTORY include/ + DESTINATION include +) + +# Install +install(TARGETS + scan_unifier_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() + + diff --git a/laser_scan_unifier/LICENSE b/laser_scan_unifier/LICENSE new file mode 100644 index 000000000..d64569567 --- /dev/null +++ b/laser_scan_unifier/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/laser_scan_unifier/README.md b/laser_scan_unifier/README.md new file mode 100644 index 000000000..718927e28 --- /dev/null +++ b/laser_scan_unifier/README.md @@ -0,0 +1,31 @@ +laser\_scan\_unifier +==================== + +General description +--------------------- +This package implements a node that unifies scan messages from a given numer of laser scanners + +Node: scan\_unifier\_node +--------------------- + +The actual node that unifies a given number of laser scans +#### Parameters +**input\_scans** *(List of std::string)* + The names of the scan topics to subscribe to as list of strings. + +**loop\_rate** *(double, default: 100.0 [hz])* + The loop rate of the ros node. + +#### Published Topics +**scan\_unified** *(sensor_msgs::LaserScan)* + Publishes the unified scans. + +#### Subscribed Topics +**input\_scan\_name** *(sensor_msgs::LaserScan)* + The current scan message from the laser scanner with topic name specified via the parameter **input\_scan\_topics** + + +#### Services + + +#### Services called 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 new file mode 100755 index 000000000..ce52d0e90 --- /dev/null +++ b/laser_scan_unifier/include/laser_scan_unifier/scan_unifier_node.hpp @@ -0,0 +1,144 @@ +/* + * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef LASER_SCAN_UNIFIER__SCAN_UNIFIER_NODE_HPP_ +#define LASER_SCAN_UNIFIER__SCAN_UNIFIER_NODE_HPP_ + +//################## +//#### includes #### + +// standard includes +#include +//#include +#include + +// ROS includes +#include "rclcpp/rclcpp.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "laser_geometry/laser_geometry.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "message_filters/sync_policies/approximate_time.h" + +// ROS message includes +#include "sensor_msgs/msg/point_cloud2.hpp" +#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 + */ + 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, + const sensor_msgs::msg::LaserScan::SharedPtr& second_scanner, + const sensor_msgs::msg::LaserScan::SharedPtr& third_scanner); + void sync4FilterCallback(const sensor_msgs::msg::LaserScan::SharedPtr& first_scanner, + const sensor_msgs::msg::LaserScan::SharedPtr& second_scanner, + 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 + 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 + 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); + +}; + +#endif // LASER_SCAN_UNIFIER__SCAN_UNIFIER_NODE_HPP_ diff --git a/laser_scan_unifier/package.xml b/laser_scan_unifier/package.xml new file mode 100755 index 000000000..cad6e182e --- /dev/null +++ b/laser_scan_unifier/package.xml @@ -0,0 +1,30 @@ + + + + laser_scan_unifier + 0.9.0 + The laser_scan_unifier package holds code to unify two or more laser-scans to one unified scan-message + + + Felix Messmer + Benjamin Maidel + Florian Mirus + + Nikola Banović + + Apache 2.0 + + ament_cmake + + laser_geometry + rclcpp + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + tf2_sensor_msgs + + + ament_cmake + + diff --git a/laser_scan_unifier/src/scan_unifier_node.cpp b/laser_scan_unifier/src/scan_unifier_node.cpp new file mode 100755 index 000000000..792de6f7b --- /dev/null +++ b/laser_scan_unifier/src/scan_unifier_node.cpp @@ -0,0 +1,310 @@ +/* + * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "laser_scan_unifier/scan_unifier_node.hpp" + + +// Constructor +ScanUnifierNode::ScanUnifierNode() + : Node("scan_unifier_node") +{ + auto logger_ = this->get_logger(); + RCLCPP_DEBUG(logger_, "Init scan_unifier"); + + synchronizer2_ = NULL; + synchronizer3_ = NULL; + synchronizer4_ = NULL; + + //########################## PARAMETERS + //getParams(); + // GET PARAMS SOMEHOW + config_.number_input_scans=4; + config_.input_scan_topics.push_back("laser_scan_right"); + config_.input_scan_topics.push_back("laser_scan_front"); + config_.input_scan_topics.push_back("laser_scan_left"); + config_.input_scan_topics.push_back("laser_scan_back"); + frame_ = "base_link"; + // Minimum possible time between messages on the same topic + auto inter_message_lower_bound = rclcpp::Duration::from_seconds(0.167); + + //########################## PARAMETERS + + // 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()); + 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) + { + case 2: + { + synchronizer2_ = std::make_shared>(Sync2Policy(2), + *message_filter_subscribers_[0], + *message_filter_subscribers_[1]); + synchronizer2_->setInterMessageLowerBound(0, inter_message_lower_bound); + synchronizer2_->setInterMessageLowerBound(1, inter_message_lower_bound); + synchronizer2_->registerCallback(&ScanUnifierNode::sync2FilterCallback, this); + break; + } + case 3: + { + synchronizer3_ = std::make_shared>(Sync3Policy(3), + *message_filter_subscribers_[0], + *message_filter_subscribers_[1], + *message_filter_subscribers_[2]); + synchronizer3_->setInterMessageLowerBound(0, inter_message_lower_bound); + synchronizer3_->setInterMessageLowerBound(1, inter_message_lower_bound); + synchronizer3_->setInterMessageLowerBound(2, inter_message_lower_bound); + synchronizer3_->registerCallback(&ScanUnifierNode::sync3FilterCallback, this); + break; + } + case 4: + { + synchronizer4_ = std::make_shared>(Sync4Policy(10), + *message_filter_subscribers_[0], + *message_filter_subscribers_[1], + *message_filter_subscribers_[2], + *message_filter_subscribers_[3]); + synchronizer4_->setInterMessageLowerBound(0, inter_message_lower_bound); + synchronizer4_->setInterMessageLowerBound(1, inter_message_lower_bound); + synchronizer4_->setInterMessageLowerBound(2, inter_message_lower_bound); + synchronizer4_->setInterMessageLowerBound(3, inter_message_lower_bound); + synchronizer4_->registerCallback(&ScanUnifierNode::sync4FilterCallback, this); + 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."); + return; + } + + 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) +{ + if(current_scans.empty()) + { + RCLCPP_DEBUG(this->get_logger(), "No scans to unify."); + return false; + } + + if (vec_cloud_.size() != config_.number_input_scans) + { + vec_cloud_.resize(config_.number_input_scans); + } + auto logger_ = this->get_logger(); + + + RCLCPP_DEBUG(logger_, "Starting conversion..."); + + for(size_t i=0; i < config_.number_input_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_) { + try + { + auto cloud = std::make_shared(); + tf_buffer_->transform(vec_cloud_[i], *cloud, frame_, tf2::durationFromSec(0.1)); //make into parameter + vec_cloud_[i] = *cloud; + } + catch (tf2::TransformException & ex) + { + RCLCPP_DEBUG(logger_, " - PointCloud2 transform failure: %s", ex.what()); + return false; + } + } + + } + + RCLCPP_DEBUG(logger_, "... Complete! Unifying scans..."); + RCLCPP_DEBUG(logger_, " - Creating message header"); + unified_scan.header = current_scans.front()->header; + unified_scan.header.frame_id = 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); + unified_scan.time_increment = current_scans.front()->time_increment; + unified_scan.scan_time = current_scans.front()->scan_time; + unified_scan.range_min = current_scans.front()->range_min; + unified_scan.range_max = current_scans.front()->range_max; + //default values (ranges: range_max, intensities: 0) are used to better reflect the driver behavior + //there "phantom" data has values > range_max + //but those values are removed during projection to pointcloud + unified_scan.ranges.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1.0, unified_scan.range_max); + unified_scan.intensities.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment+ 1.0), 0.0); + + // now unify all Scans + for(size_t j = 0; j < config_.number_input_scans; j++) + { + // Iterating over PointCloud2 point xyz values + sensor_msgs::PointCloud2Iterator iterX(vec_cloud_[j], "x"); + sensor_msgs::PointCloud2Iterator iterY(vec_cloud_[j], "y"); + sensor_msgs::PointCloud2Iterator iterZ(vec_cloud_[j], "z"); + + for (; iterX != iterX.end(); ++iterX, ++iterY, ++iterZ) + { + const float& x = *iterX; + const float& y = *iterY; + const float& z = *iterZ; + + //RCLCPP_INFO("Point %f %f %f", x, y, z); + if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ) + { + RCLCPP_DEBUG(logger_, " - Rejected for nan in point(%f, %f, %f)\n", x, y, z); + continue; + } + double angle = atan2(y, x);// + M_PI/2; + if (angle < unified_scan.angle_min || angle > unified_scan.angle_max) + { + RCLCPP_DEBUG(logger_, " - Rejected for angle %f not in range (%f, %f)\n", angle, unified_scan.angle_min, unified_scan.angle_max); + continue; + } + int index = std::floor(0.5 + (angle - unified_scan.angle_min) / unified_scan.angle_increment); + if(index<0 || index>=unified_scan.ranges.size()) continue; + double range_sq = y*y+x*x; + //printf ("index xyz( %f %f %f) angle %f index %d range %f\n", x, y, z, angle, index, sqrt(range_sq)); + if ((sqrt(range_sq) <= unified_scan.ranges[index])) + { + // use the nearest reflection point of all scans for unified scan + unified_scan.ranges[index] = sqrt(range_sq); + // get respective intensity from point cloud intensity-channel (index 0) + // unified_scan.intensities[index] = vec_cloud_[j].channels.front().values[i]; // intensities may not be necessary, and I'm not sure how to access them + } + } + } + + return true; +} + +void ScanUnifierNode::publish(sensor_msgs::msg::LaserScan& unified_scan) +{ + RCLCPP_DEBUG(this->get_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; +} + +void ScanUnifierNode::sync2FilterCallback(const sensor_msgs::msg::LaserScan::SharedPtr& scan1, + const sensor_msgs::msg::LaserScan::SharedPtr& scan2) +{ + std::vector current_scans; + current_scans.push_back(scan1); + current_scans.push_back(scan2); + + auto unified_scan = sensor_msgs::msg::LaserScan(); + + if (!unifyLaserScans(current_scans, unified_scan)) + { + return; + } + publish(unified_scan); + return; +} + +void ScanUnifierNode::sync3FilterCallback(const sensor_msgs::msg::LaserScan::SharedPtr& scan1, + const sensor_msgs::msg::LaserScan::SharedPtr& scan2, + const sensor_msgs::msg::LaserScan::SharedPtr& scan3) +{ + std::vector current_scans; + current_scans.push_back(scan1); + current_scans.push_back(scan2); + current_scans.push_back(scan3); + + auto unified_scan = sensor_msgs::msg::LaserScan(); + + if (!unifyLaserScans(current_scans, unified_scan)) + { + return; + } + publish(unified_scan); + return; +} + +void ScanUnifierNode::sync4FilterCallback(const sensor_msgs::msg::LaserScan::SharedPtr& scan1, + const sensor_msgs::msg::LaserScan::SharedPtr& scan2, + const sensor_msgs::msg::LaserScan::SharedPtr& scan3, + const sensor_msgs::msg::LaserScan::SharedPtr& scan4) +{ + 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; + } + publish(unified_scan); + return; +} + + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} + +