feat(slam): add lidar SLAM and pointcloud processing packages
This commit is contained in:
X-lanni
@@ -0,0 +1,18 @@
|
||||
---
|
||||
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
|
||||
@@ -0,0 +1,62 @@
|
||||
---
|
||||
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
|
||||
...
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 1.3 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 85 KiB |
@@ -0,0 +1,12 @@
|
||||
devel/
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
|
||||
.vscode/
|
||||
.cache/
|
||||
__pycache__/
|
||||
.catkin_workspace
|
||||
*.gv
|
||||
*.pdf
|
||||
*.pcd
|
||||
@@ -0,0 +1,127 @@
|
||||
# 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"]
|
||||
@@ -0,0 +1,54 @@
|
||||
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
|
||||
)
|
||||
@@ -0,0 +1,63 @@
|
||||
# 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
|
||||
+202
@@ -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.
|
||||
@@ -0,0 +1,79 @@
|
||||
# pcd2pgm
|
||||
|
||||
基于 ROS2 和 PCL 库，用于将 `.pcd` 点云文件转换为用于 Navigation 的 `pgm` 栅格地图
|
||||
|
||||
|pcd|pgm|
|
||||
|:-:|:-:|
|
||||
|||
|
||||
|
||||
## 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 <YOUR_MAP_NAME>
|
||||
```
|
||||
|
||||
## 三. 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 滤波器）
|
||||
```
|
||||
@@ -0,0 +1,12 @@
|
||||
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
|
||||
@@ -0,0 +1,72 @@
|
||||
// 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 <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<pcl::PointXYZ>::Ptr & pcd_cloud0, double radius, int thre_count);
|
||||
|
||||
void setMapTopicMsg(
|
||||
const pcl::PointCloud<pcl::PointXYZ>::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<double> odom_to_lidar_odom_;
|
||||
|
||||
std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pcd_cloud_;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_pass_through_;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_radius_;
|
||||
nav_msgs::msg::OccupancyGrid map_topic_msg_;
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_publisher_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcd_publisher_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
};
|
||||
} // namespace pcd2pgm
|
||||
|
||||
#endif // PCD2PGM__PCD2PGM_HPP_
|
||||
@@ -0,0 +1,88 @@
|
||||
# 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
|
||||
@@ -0,0 +1,27 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>pcd2pgm</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Convert .pcd files to .pgm grid maps for navigation.</description>
|
||||
<maintainer email="lihanchen2004@163.com">LihanChen</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
<author email="lihanchen2004@163.com">LihanChen</author>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>pcl_ros</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_clang_format</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,194 @@
|
||||
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: <Fixed 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: <Fixed 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: <Fixed 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
|
||||
@@ -0,0 +1,187 @@
|
||||
// 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<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
map_publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(map_topic_name_, map_qos);
|
||||
pcd_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("pcd_cloud", 10);
|
||||
|
||||
if (pcl::io::loadPCDFile<pcl::PointXYZ>(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<double>{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::PointCloud<pcl::PointXYZ>>();
|
||||
pcl::PassThrough<pcl::PointXYZ> 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<pcl::PointXYZ>::Ptr & input_cloud, double radius, int thre_count)
|
||||
{
|
||||
auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
pcl::RadiusOutlierRemoval<pcl::PointXYZ> 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<pcl::PointXYZ>::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<double>::max();
|
||||
double x_max = std::numeric_limits<double>::lowest();
|
||||
double y_min = std::numeric_limits<double>::max();
|
||||
double y_max = std::numeric_limits<double>::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<double>(point.x));
|
||||
x_max = std::max(x_max, static_cast<double>(point.x));
|
||||
y_min = std::min(y_min, static_cast<double>(point.y));
|
||||
y_max = std::max(y_max, static_cast<double>(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)
|
||||
Reference in New Issue
Block a user