diff --git a/pcd2pgm/.clang-format b/pcd2pgm/.clang-format deleted file mode 100644 index aea802f..0000000 --- a/pcd2pgm/.clang-format +++ /dev/null @@ -1,18 +0,0 @@ ---- -Language: Cpp -BasedOnStyle: Google - -AccessModifierOffset: -2 -AlignAfterOpenBracket: AlwaysBreak -BraceWrapping: - AfterClass: true - AfterFunction: true - AfterNamespace: true - AfterStruct: true -BreakBeforeBraces: Custom -ColumnLimit: 100 -ConstructorInitializerIndentWidth: 0 -ContinuationIndentWidth: 2 -DerivePointerAlignment: false -PointerAlignment: Middle -ReflowComments: false diff --git a/pcd2pgm/.clang-tidy b/pcd2pgm/.clang-tidy deleted file mode 100644 index d805018..0000000 --- a/pcd2pgm/.clang-tidy +++ /dev/null @@ -1,62 +0,0 @@ ---- -Checks: '-*, - performance-*, - -performance-unnecessary-value-param, - llvm-namespace-comment, - modernize-redundant-void-arg, - modernize-use-nullptr, - modernize-use-default, - modernize-use-override, - modernize-loop-convert, - modernize-make-shared, - modernize-make-unique, - misc-unused-parameters, - readability-named-parameter, - readability-redundant-smartptr-get, - readability-redundant-string-cstr, - readability-simplify-boolean-expr, - readability-container-size-empty, - readability-identifier-naming, - ' -HeaderFilterRegex: '' -CheckOptions: - - key: llvm-namespace-comment.ShortNamespaceLines - value: '10' - - key: llvm-namespace-comment.SpacesBeforeComments - value: '2' - - key: misc-unused-parameters.StrictMode - value: '1' - - key: readability-braces-around-statements.ShortStatementLines - value: '2' - # type names - - key: readability-identifier-naming.ClassCase - value: CamelCase - - key: readability-identifier-naming.EnumCase - value: CamelCase - - key: readability-identifier-naming.UnionCase - value: CamelCase - # method names - - key: readability-identifier-naming.MethodCase - value: camelBack - # variable names - - key: readability-identifier-naming.VariableCase - value: lower_case - # class member names - - key: readability-identifier-naming.PrivateMemberCase - value: lower_case - - key: readability-identifier-naming.PrivateMemberSuffix - value: '_' - - key: readability-identifier-naming.ProtectedMemberCase - value: lower_case - - key: readability-identifier-naming.ProtectedMemberSuffix - value: '_' - # const static or global variables are UPPER_CASE - - key: readability-identifier-naming.EnumConstantCase - value: UPPER_CASE - - key: readability-identifier-naming.StaticConstantCase - value: UPPER_CASE - - key: readability-identifier-naming.ClassConstantCase - value: UPPER_CASE - - key: readability-identifier-naming.GlobalVariableCase - value: UPPER_CASE -... diff --git a/pcd2pgm/.docs/pcd.png b/pcd2pgm/.docs/pcd.png deleted file mode 100644 index 2ac39fa..0000000 Binary files a/pcd2pgm/.docs/pcd.png and /dev/null differ diff --git a/pcd2pgm/.docs/pgm.png b/pcd2pgm/.docs/pgm.png deleted file mode 100644 index d2f6f5b..0000000 Binary files a/pcd2pgm/.docs/pgm.png and /dev/null differ diff --git a/pcd2pgm/.gitignore b/pcd2pgm/.gitignore deleted file mode 100644 index b0a66c2..0000000 --- a/pcd2pgm/.gitignore +++ /dev/null @@ -1,12 +0,0 @@ -devel/ -build/ -install/ -log/ - -.vscode/ -.cache/ -__pycache__/ -.catkin_workspace -*.gv -*.pdf -*.pcd diff --git a/pcd2pgm/.pre-commit-config.yaml b/pcd2pgm/.pre-commit-config.yaml deleted file mode 100644 index 2badd49..0000000 --- a/pcd2pgm/.pre-commit-config.yaml +++ /dev/null @@ -1,127 +0,0 @@ -# To use: -# -# pre-commit run -a -# -# Or: -# -# pre-commit install # (runs every time you commit in git) -# -# To update this file: -# -# pre-commit autoupdate -# -# See https://github.com/pre-commit/pre-commit - -repos: - # Standard hooks - - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 - hooks: - - id: check-added-large-files - - id: check-ast - - id: check-case-conflict - - id: check-docstring-first - - id: check-merge-conflict - - id: check-symlinks - - id: check-xml - - id: check-yaml - - id: debug-statements - - id: end-of-file-fixer - - id: mixed-line-ending - - id: trailing-whitespace - exclude_types: [rst] - - id: fix-byte-order-marker - - # Python hooks - - repo: https://github.com/asottile/pyupgrade - rev: v3.19.1 - hooks: - - id: pyupgrade - args: [--py36-plus] - - - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.8.4 - hooks: - - id: ruff - args: [ --fix ] - - id: ruff-format - - # CPP hooks - - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.3 - hooks: - - id: clang-format - args: ['-fallback-style=none', '-i'] - - - repo: local - hooks: - - id: ament_cppcheck - name: ament_cppcheck - description: Static code analysis of C/C++ files. - entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - - - repo: local - hooks: - - id: ament_cpplint - name: ament_cpplint - description: Static code analysis of C/C++ files. - entry: ament_cpplint - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - args: ["--linelength=100", "--filter=-whitespace/newline"] - - # Cmake hooks - - repo: local - hooks: - - id: ament_lint_cmake - name: ament_lint_cmake - description: Check format of CMakeLists.txt files. - entry: ament_lint_cmake - language: system - files: CMakeLists\.txt$ - - # Copyright - - repo: local - hooks: - - id: ament_copyright - name: ament_copyright - description: Check if copyright notice is available in all files. - entry: ament_copyright - language: system - - # Docs - RestructuredText hooks - - repo: https://github.com/PyCQA/doc8 - rev: v1.1.2 - hooks: - - id: doc8 - args: ['--max-line-length=100', '--ignore=D001'] - exclude: CHANGELOG\.rst$ - - - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.10.0 - hooks: - - id: rst-backticks - exclude: CHANGELOG\.rst$ - - id: rst-directive-colons - - id: rst-inline-touching-normal - - # Spellcheck in comments and docs - # skipping of *.svg files is not working... - - repo: https://github.com/codespell-project/codespell - rev: v2.3.0 - hooks: - - id: codespell - args: ['--write-changes'] - exclude: CHANGELOG\.rst|\.(svg|pyc)$ - - - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.30.0 - hooks: - - id: check-github-workflows - args: ["--verbose"] - - id: check-github-actions - args: ["--verbose"] - - id: check-dependabot - args: ["--verbose"] diff --git a/pcd2pgm/CMakeLists.txt b/pcd2pgm/CMakeLists.txt deleted file mode 100644 index f07d404..0000000 --- a/pcd2pgm/CMakeLists.txt +++ /dev/null @@ -1,54 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(pcd2pgm) - -## Use C++14 -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -## Export compile commands for clangd -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -####################### -## Find dependencies ## -####################### - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - - -########### -## Build ## -########### - -ament_auto_add_library(${PROJECT_NAME} SHARED - DIRECTORY src -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN pcd2pgm::Pcd2PgmNode - EXECUTABLE ${PROJECT_NAME}_node -) - -############# -## Testing ## -############# - -if(BUILD_TESTING) -set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_SOURCE_DIR}/.clang-format") -find_package(ament_lint_auto REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_uncrustify - ament_cmake_flake8 - ) - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# - -ament_auto_package(INSTALL_TO_SHARE - config - launch - rviz -) diff --git a/pcd2pgm/CONTRIBUTING.md b/pcd2pgm/CONTRIBUTING.md deleted file mode 100644 index 331a805..0000000 --- a/pcd2pgm/CONTRIBUTING.md +++ /dev/null @@ -1,63 +0,0 @@ -# Contributing Guidelines - -Thank you for your interest in contributing to `pcd2pgm`. -Whether it's a bug report, new feature, correction, or additional -documentation, we greatly value feedback and contributions from our community. - -Please read through this document before submitting any issues or pull requests to ensure we have all the necessary -information to effectively respond to your bug report or contribution. - -## Reporting Bugs/Feature Requests - -We welcome you to use the GitHub issue tracker to report bugs or suggest features. - -When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure - somebody else hasn't already reported the issue. -Please try to include as much information as you can. Details like these are incredibly useful: - -* A reproducible test case or series of steps -* The version of our code being used -* Any modifications you've made relevant to the bug -* Anything unusual about your environment or deployment - -## Contributing via Pull Requests - -To send us a pull request, please: - -1. Fork the repository. -2. Modify the source; please focus on the specific change you are contributing. - If you also reformat all the code, it will be hard for us to focus on your change. -3. Ensure local tests pass. (`colcon test` and `pre-commit run` (requires you to install pre-commit by `pip3 install pre-commit`) -4. Commit to your fork using clear commit messages. -5. Send a pull request, answering any default questions in the pull request interface. -6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation. - -GitHub provides additional documentation on [forking a repository](https://help.github.com/articles/fork-a-repo/) and -[creating a pull request](https://help.github.com/articles/creating-a-pull-request/). - -## Finding contributions to work on - -Looking at the existing issues is a great way to find something to contribute on. -As this project, by default, uses the default GitHub issue labels - (enhancement/bug/duplicate/help wanted/invalid/question/wontfix), looking at any ['help wanted'][help-wanted] issues - is a great place to start. - -## Licensing - -Any contribution that you make to this repository will -be under the Apache 2 License, as dictated by that -[license](http://www.apache.org/licenses/LICENSE-2.0.html): - -~~~ -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. -~~~ - -[issues]:https://github.com/LihanChen2004/pcd2pgm/issues -[closed-issues]:https://github.com/LihanChen2004/pcd2pgm/issues?utf8=%E2%9C%93&q=is%3Aissue%20is%3Aclosed%20 -[help-wanted]:https://github.com/LihanChen2004/pcd2pgm/issues?q=is%3Aopen+is%3Aissue+label%3A%22help+wanted%22 diff --git a/pcd2pgm/LICENSE b/pcd2pgm/LICENSE deleted file mode 100644 index d645695..0000000 --- a/pcd2pgm/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - 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/pcd2pgm/README.md b/pcd2pgm/README.md deleted file mode 100644 index 4c2b978..0000000 --- a/pcd2pgm/README.md +++ /dev/null @@ -1,79 +0,0 @@ -# pcd2pgm - -基于 ROS2 和 PCL 库，用于将 `.pcd` 点云文件转换为用于 Navigation 的 `pgm` 栅格地图 - -|pcd|pgm| -|:-:|:-:| -|![pcd](.docs/pcd.png)|![pgm](.docs/pgm.png)| - -## 1. Overview - -- 读取指定的 `.pcd` 文件 - -- 使用 Pass Through 滤波器过滤点云 - -- 使用 Radius Outlier 滤波器进一步处理点云 - -- 将处理后的点云转换为占据栅格地图（Occupancy Grid Map） - -- 将转换后的地图发布到指定 ROS 话题上 - -## 2. Quick Start - -### 2.1 Setup Environment - -- Ubuntu 22.04 -- ROS: [Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html) - -### 2.2 Create Workspace - -```bash -mkdir -p ~/ros_ws -cd ~/ros_ws -``` - -```bash -git clone https://github.com/LihanChen2004/pcd2pgm.git -``` - -### 2.3 Build - -```bash -rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y -``` - -```bash -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=release -``` - -### 2.4 Running - -启动 pcd2pgm 节点，可在 RViz 中预览滤波后点云和栅格地图： - -```sh -ros2 launch pcd2pgm pcd2pgm_launch.py -``` - -保存栅格地图： - -```sh -ros2 run nav2_map_server map_saver_cli -f -``` - -## 三. Node Parameters - -可以通过修改 `pcd2pgm/pcd2pgm.yaml` 文件来配置节点的参数。 - - ```yaml - pcd2pgm: - ros__parameters: - pcd_file: /home/lihanchen/NAVIGATION_WS/pcd2pgm/rmuc_2025.pcd # pcd 文件所在目录 - odom_to_lidar_odom: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [x, y, z, r, p, y] 里程计到激光雷达的坐标变换（用于变换点云） - flag_pass_through: false # 是否使用 Pass Through 滤波器 - map_resolution: 0.05 # 地图分辨率 - map_topic_name: map # 发布地图的 ROS 话题名 - thre_radius: 0.1 # Radius Outlier 滤波器半径 - thre_z_max: 2.0 # Z轴最大值（用于 Pass Through 滤波器） - thre_z_min: 0.1 # Z轴最小值（用于 Pass Through 滤波器） - thres_point_count: 10 # 最小点数阈值（用于 Radius Outlier 滤波器） - ``` diff --git a/pcd2pgm/config/pcd2pgm.yaml b/pcd2pgm/config/pcd2pgm.yaml deleted file mode 100644 index df02def..0000000 --- a/pcd2pgm/config/pcd2pgm.yaml +++ /dev/null @@ -1,12 +0,0 @@ -pcd2pgm: - ros__parameters: - pcd_file: /home/elephant/agv_pro_ros2/src/point_lio_ros2/PCD/scans.pcd - odom_to_lidar_odom: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - # odom_to_lidar_odom: [-0.16, 0.0, -0.18, -0.523599, 0.0, -1.5707963] - flag_pass_through: false - map_resolution: 0.04 - map_topic_name: map - thre_radius: 0.1 - thre_z_max: 1.3 - thre_z_min: 0.3 - thres_point_count: 10 diff --git a/pcd2pgm/include/pcd2pgm/pcd2pgm.hpp b/pcd2pgm/include/pcd2pgm/pcd2pgm.hpp deleted file mode 100644 index a35f515..0000000 --- a/pcd2pgm/include/pcd2pgm/pcd2pgm.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 2025 Lihan Chen -// -// 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 PCD2PGM__PCD2PGM_HPP_ -#define PCD2PGM__PCD2PGM_HPP_ - -#include -#include -#include - -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "pcl/filters/passthrough.h" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -namespace pcd2pgm -{ -class Pcd2PgmNode : public rclcpp::Node -{ -public: - explicit Pcd2PgmNode(const rclcpp::NodeOptions & options); - -private: - void declareParameters(); - - void getParameters(); - - void passThroughFilter(double thre_low, double thre_high, bool flag_in); - - void radiusOutlierFilter( - const pcl::PointCloud::Ptr & pcd_cloud0, double radius, int thre_count); - - void setMapTopicMsg( - const pcl::PointCloud::Ptr cloud, nav_msgs::msg::OccupancyGrid & msg); - - void publishCallback(); - - void applyTransform(); - - float thre_z_min_; - float thre_z_max_; - float thre_radius_; - bool flag_pass_through_; - float map_resolution_; - int thres_point_count_; - std::string pcd_file_; - std::string map_topic_name_; - std::vector odom_to_lidar_odom_; - - std::shared_ptr> pcd_cloud_; - pcl::PointCloud::Ptr cloud_after_pass_through_; - pcl::PointCloud::Ptr cloud_after_radius_; - nav_msgs::msg::OccupancyGrid map_topic_msg_; - - rclcpp::Publisher::SharedPtr map_publisher_; - rclcpp::Publisher::SharedPtr pcd_publisher_; - rclcpp::TimerBase::SharedPtr timer_; -}; -} // namespace pcd2pgm - -#endif // PCD2PGM__PCD2PGM_HPP_ diff --git a/pcd2pgm/launch/pcd2pgm_launch.py b/pcd2pgm/launch/pcd2pgm_launch.py deleted file mode 100644 index f378ccf..0000000 --- a/pcd2pgm/launch/pcd2pgm_launch.py +++ /dev/null @@ -1,88 +0,0 @@ -# Copyright 2025 Lihan Chen -# -# 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. - -import os - -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - SetEnvironmentVariable, -) -from launch.substitutions import LaunchConfiguration - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory("pcd2pgm") - - # Create the launch configuration variables`` - params_file = LaunchConfiguration("params_file") - rviz_config_file = LaunchConfiguration("rviz_config_file") - - stdout_linebuf_envvar = SetEnvironmentVariable( - "RCUTILS_LOGGING_BUFFERED_STREAM", "1" - ) - - colorized_output_envvar = SetEnvironmentVariable("RCUTILS_COLORIZED_OUTPUT", "1") - - declare_params_file_cmd = DeclareLaunchArgument( - "params_file", - default_value=os.path.join(bringup_dir, "config", "pcd2pgm.yaml"), - description="Full path to the ROS2 parameters file to use for all launched nodes", - ) - - declare_rviz_config_file_cmd = DeclareLaunchArgument( - "rviz_config_file", - default_value=os.path.join(bringup_dir, "rviz", "pcd2pgm.rviz"), - description="Full path to the RVIZ config file to use", - ) - - start_pcd2pgm_cmd = Node( - package="pcd2pgm", - executable="pcd2pgm_node", - name="pcd2pgm", - output="screen", - parameters=[params_file], - ) - - start_rviz_cmd = Node( - package="rviz2", - executable="rviz2", - arguments=["-d", rviz_config_file], - output="screen", - remappings=[ - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set environment variables - ld.add_action(stdout_linebuf_envvar) - ld.add_action(colorized_output_envvar) - - # Declare the launch options - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_rviz_config_file_cmd) - - # Add the actions to launch all of the navigation nodes - ld.add_action(start_pcd2pgm_cmd) - ld.add_action(start_rviz_cmd) - - return ld diff --git a/pcd2pgm/package.xml b/pcd2pgm/package.xml deleted file mode 100644 index f0d2f7b..0000000 --- a/pcd2pgm/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - pcd2pgm - 0.0.0 - Convert .pcd files to .pgm grid maps for navigation. - LihanChen - Apache-2.0 - LihanChen - - ament_cmake - - rclcpp - rclcpp_components - nav_msgs - sensor_msgs - pcl_ros - pcl_conversions - - ament_lint_auto - ament_lint_common - ament_cmake_clang_format - - - ament_cmake - - diff --git a/pcd2pgm/rviz/pcd2pgm.rviz b/pcd2pgm/rviz/pcd2pgm.rviz deleted file mode 100644 index 2f2d96f..0000000 --- a/pcd2pgm/rviz/pcd2pgm.rviz +++ /dev/null @@ -1,194 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 716 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: cloud_after_radius -Visualization Manager: - Class: "" - Displays: - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates - Use Timestamp: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: cloud_after_radius - Position Transformer: XYZ - Selectable: true - Size (Pixels): 1 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /pcd_cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 17.899526596069336 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.028435707092285156 - Y: 0.07508468627929688 - Z: 0.047394752502441406 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6574800610542297 - Target Frame: - Value: Orbit (rviz) - Yaw: 2.4183316230773926 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 982 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000033a0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000022f000000b60000000000000000fb0000001800440065006200750067002000430061006d00650072006100000002b8000000f00000000000000000000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c20000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004380000003efc0100000002fb0000000800540069006d00650000000000000004380000058100fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1740 - X: 1233 - Y: 54 diff --git a/pcd2pgm/src/pcd2pgm.cpp b/pcd2pgm/src/pcd2pgm.cpp deleted file mode 100644 index 666190d..0000000 --- a/pcd2pgm/src/pcd2pgm.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2025 Lihan Chen -// -// 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 "pcd2pgm/pcd2pgm.hpp" - -#include "pcl/common/transforms.h" -#include "pcl/filters/radius_outlier_removal.h" -#include "pcl/io/pcd_io.h" -#include "pcl_conversions/pcl_conversions.h" - -namespace pcd2pgm -{ -Pcd2PgmNode::Pcd2PgmNode(const rclcpp::NodeOptions & options) : Node("pcd2pgm", options) -{ - declareParameters(); - getParameters(); - - rclcpp::QoS map_qos(10); - map_qos.transient_local(); - map_qos.reliable(); - map_qos.keep_last(1); - - pcd_cloud_ = std::make_shared>(); - map_publisher_ = this->create_publisher(map_topic_name_, map_qos); - pcd_publisher_ = this->create_publisher("pcd_cloud", 10); - - if (pcl::io::loadPCDFile(pcd_file_, *pcd_cloud_) == -1) { - RCLCPP_ERROR(get_logger(), "Couldn't read file: %s", pcd_file_.c_str()); - return; - } - - RCLCPP_INFO(get_logger(), "Initial point cloud size: %lu", pcd_cloud_->points.size()); - - applyTransform(); - - passThroughFilter(thre_z_min_, thre_z_max_, flag_pass_through_); - radiusOutlierFilter(cloud_after_pass_through_, thre_radius_, thres_point_count_); - setMapTopicMsg(cloud_after_radius_, map_topic_msg_); - - timer_ = - create_wall_timer(std::chrono::seconds(1), std::bind(&Pcd2PgmNode::publishCallback, this)); -} - -void Pcd2PgmNode::publishCallback() -{ - sensor_msgs::msg::PointCloud2 output; - pcl::toROSMsg(*cloud_after_radius_, output); - output.header.frame_id = "map"; - pcd_publisher_->publish(output); - map_publisher_->publish(map_topic_msg_); -} - -void Pcd2PgmNode::declareParameters() -{ - declare_parameter("pcd_file", ""); - declare_parameter("thre_z_min", 0.5); - declare_parameter("thre_z_max", 2.0); - declare_parameter("flag_pass_through", false); - declare_parameter("thre_radius", 0.5); - declare_parameter("map_resolution", 0.05); - declare_parameter("thres_point_count", 10); - declare_parameter("map_topic_name", "map"); - declare_parameter( - "odom_to_lidar_odom", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); // 新增的参数 -} - -void Pcd2PgmNode::getParameters() -{ - get_parameter("pcd_file", pcd_file_); - get_parameter("thre_z_min", thre_z_min_); - get_parameter("thre_z_max", thre_z_max_); - get_parameter("flag_pass_through", flag_pass_through_); - get_parameter("thre_radius", thre_radius_); - get_parameter("map_resolution", map_resolution_); - get_parameter("thres_point_count", thres_point_count_); - get_parameter("map_topic_name", map_topic_name_); - get_parameter("odom_to_lidar_odom", odom_to_lidar_odom_); // 获取新的参数 -} - -void Pcd2PgmNode::passThroughFilter(double thre_low, double thre_high, bool flag_in) -{ - auto filtered_cloud = std::make_shared>(); - pcl::PassThrough passthrough; - passthrough.setInputCloud(pcd_cloud_); - passthrough.setFilterFieldName("z"); - passthrough.setFilterLimits(thre_low, thre_high); - passthrough.setNegative(flag_in); - passthrough.filter(*filtered_cloud); - - cloud_after_pass_through_ = filtered_cloud; - RCLCPP_INFO( - get_logger(), "After PassThrough filtering: %lu points", - cloud_after_pass_through_->points.size()); -} - -void Pcd2PgmNode::radiusOutlierFilter( - const pcl::PointCloud::Ptr & input_cloud, double radius, int thre_count) -{ - auto filtered_cloud = std::make_shared>(); - pcl::RadiusOutlierRemoval radius_outlier; - radius_outlier.setInputCloud(input_cloud); - radius_outlier.setRadiusSearch(radius); - radius_outlier.setMinNeighborsInRadius(thre_count); - radius_outlier.filter(*filtered_cloud); - - cloud_after_radius_ = filtered_cloud; - RCLCPP_INFO( - get_logger(), "After RadiusOutlier filtering: %lu points", cloud_after_radius_->points.size()); -} - -void Pcd2PgmNode::setMapTopicMsg( - const pcl::PointCloud::Ptr cloud, nav_msgs::msg::OccupancyGrid & msg) -{ - msg.header.stamp = now(); - msg.header.frame_id = "map"; - - msg.info.map_load_time = now(); - msg.info.resolution = map_resolution_; - - double x_min = std::numeric_limits::max(); - double x_max = std::numeric_limits::lowest(); - double y_min = std::numeric_limits::max(); - double y_max = std::numeric_limits::lowest(); - - if (cloud->points.empty()) { - RCLCPP_WARN(get_logger(), "Point cloud is empty!"); - return; - } - - for (const auto & point : cloud->points) { - x_min = std::min(x_min, static_cast(point.x)); - x_max = std::max(x_max, static_cast(point.x)); - y_min = std::min(y_min, static_cast(point.y)); - y_max = std::max(y_max, static_cast(point.y)); - } - - msg.info.origin.position.x = x_min; - msg.info.origin.position.y = y_min; - msg.info.origin.position.z = 0.0; - msg.info.origin.orientation.x = 0.0; - msg.info.origin.orientation.y = 0.0; - msg.info.origin.orientation.z = 0.0; - msg.info.origin.orientation.w = 1.0; - - msg.info.width = std::ceil((x_max - x_min) / map_resolution_); - msg.info.height = std::ceil((y_max - y_min) / map_resolution_); - msg.data.assign(msg.info.width * msg.info.height, 0); - - for (const auto & point : cloud->points) { - int i = std::floor((point.x - x_min) / map_resolution_); - int j = std::floor((point.y - y_min) / map_resolution_); - - if (i >= 0 && i < msg.info.width && j >= 0 && j < msg.info.height) { - msg.data[i + j * msg.info.width] = 100; - } - } - - RCLCPP_INFO(get_logger(), "Map data size: %lu", msg.data.size()); -} - -void Pcd2PgmNode::applyTransform() -{ - Eigen::Affine3f transform = Eigen::Affine3f::Identity(); - - transform.translation() << odom_to_lidar_odom_[0], odom_to_lidar_odom_[1], odom_to_lidar_odom_[2]; - transform.rotate(Eigen::AngleAxisf(odom_to_lidar_odom_[3], Eigen::Vector3f::UnitX())); - transform.rotate(Eigen::AngleAxisf(odom_to_lidar_odom_[4], Eigen::Vector3f::UnitY())); - transform.rotate(Eigen::AngleAxisf(odom_to_lidar_odom_[5], Eigen::Vector3f::UnitZ())); - - pcl::transformPointCloud(*pcd_cloud_, *pcd_cloud_, transform.inverse()); -} - -} // namespace pcd2pgm - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(pcd2pgm::Pcd2PgmNode) diff --git a/upstream.jazzy.repos b/upstream.jazzy.repos index 2ea13e7..1832f61 100644 --- a/upstream.jazzy.repos +++ b/upstream.jazzy.repos @@ -11,6 +11,10 @@ repositories: type: git url: https://github.com/Lslidar/Lslidar_ROS2_driver.git version: LS-S1_V1.0 + pcd2pgm: + type: git + url: https://github.com/LihanChen2004/pcd2pgm.git + version: main point_lio_ros2: type: git url: https://github.com/dfloreaa/point_lio_ros2.git