Initial import of FaRui Autoware Dual-Orin Stack

2026-06-05 13:34:38 +08:00
commit 45e3325700
6548 changed files with 1335203 additions and 0 deletions
@@ -0,0 +1,9 @@
+build
+install
+log
+
+.cache
+.vscode
+.idea
+
+COLCON_IGNORE
@@ -0,0 +1,48 @@
+# 海康威视工业相机ROS2驱动
+
+支持列表：
+
+* GIGE相机（测试相机：MV-CS016-10GC）
+
+* USB相机（未测试）
+
+---
+
+依赖项安装：
+
+1. 在官网下载并安装MVS驱动包。
+
+2. 运行MVS可视化工具，配置相机参数。设置相机名称(user_name, device_id)，本项目将根据相机名称寻找对应设备。
+
+---
+
+独立节点（standalone）运行方式：
+
+```bash
+ros2 launch hikvision_ros2_driver standalone.launch.yaml camera_name:=<camera_name>
+```
+
+进程内通信（component）运行方式：
+
+```bash
+ros2 launch hikvision_ros2_driver component.launch.yaml camera_name:=<camera_name> container_name:=<container_name>
+```
+
+* 需要将camera_name替换成对应相机的名称。
+
+* 对于进程内通信，需要提前启动`component_container`，并将container_name设置为对应container的名称。
+
+---
+
+发布话题：
+
+* raw/image [sensor_msgs/msg/Image]: 相机原始数据，格式可以在MVS工具中修改，默认情况下应该是bayer格式。
+* info [hikvision_interface/msg/HikImageInfo]: 图像元数据，包括时间戳、曝光长度、增益大小、白平衡参数等。
+
+---
+
+补充:
+
+* 不建议对bayer格式的原始数据使用jpeg压缩。即当相机原始数据格式为bayer时，不建议订阅`raw/image/compressed`话题。
+
+* rosbag录制时建议开启zstd压缩以节省空间。(ros2 bag record -s mcap --storage-preset-profile zstd_fast)
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(hikvision_interface)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/HikImageInfo.msg"
+  DEPENDENCIES std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_dependencies(std_msgs rosidl_default_runtime)
+ament_package()
@@ -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,12 @@
+# header
+std_msgs/Header header
+# sensor stamp
+builtin_interfaces/Time dev_stamp
+uint32 frame_num
+# acqusistion
+float32 gain
+float32 exposure
+# white balance
+uint32 red
+uint32 green
+uint32 blue
@@ -0,0 +1,24 @@
+<?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>hikvision_interface</name>
+  <version>0.0.0</version>
+  <description>ros2 interface for hikvision camera</description>
+  <maintainer email="41963211+xinyang-go@users.noreply.github.com">loc</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,3 @@
+BasedOnStyle: Google
+IndentWidth: 4
+ColumnLimit: 120
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.8)
+project(hikvision_ros2_driver)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_EXTENSIONS ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+add_compile_options(-Wall)
+
+# find ros dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# find hikvision sdk
+list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+find_package(MVS REQUIRED)
+
+# build components
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/hikvision_ros2_driver.cpp
+)
+target_include_directories(${PROJECT_NAME} PUBLIC include)
+target_include_directories(${PROJECT_NAME} PRIVATE ${MVS_INCLUDE_DIR})
+ament_target_dependencies(${PROJECT_NAME} driver_timestamp_sync)
+target_link_libraries(${PROJECT_NAME} ${MVS_LIBRARIES})
+
+# register components
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "hikvision_ros2_driver::HikvisionDriver"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
@@ -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,37 @@
+if(DEFINED ENV{MVCAM_SDK_PATH})
+    set(MVCAM_SDK_PATH $ENV{MVCAM_SDK_PATH} CACHE PATH "hikvision sdk path")
+else()
+    set(MVCAM_SDK_PATH /opt/MVS CACHE PATH "hikvision sdk path")
+endif()
+
+# find header
+find_path(MVS_INCLUDE_DIR 
+    NAMES MvCameraControl.h
+    PATHS ${MVCAM_SDK_PATH}/include
+)
+
+# find libraries
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
+    find_library(MVS_LIBRARIES
+        NAMES MvCameraControl
+        PATHS ${MVCAM_SDK_PATH}/lib/64
+    )
+elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+    find_library(MVS_LIBRARIES
+        NAMES MvCameraControl
+        PATHS ${MVCAM_SDK_PATH}/lib/aarch64
+    )    
+endif()
+
+
+message(VERBOSE "MVCAM_SDK_PATH: ${MVCAM_SDK_PATH}")
+message(VERBOSE "MVS_INCLUDE_DIR: ${MVS_INCLUDE_DIR}")
+message(VERBOSE "MVS_LIBRARIES: ${MVS_LIBRARIES}")
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(MVS
+  FOUND_VAR MVS_FOUND
+  REQUIRED_VARS
+    MVS_INCLUDE_DIR
+    MVS_LIBRARIES
+)
@@ -0,0 +1,21 @@
+#pragma once
+#ifndef HIKVISION_ROS2_DRIVER_HPP
+#define HIKVISION_ROS2_DRIVER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace hikvision_ros2_driver {
+
+class HikvisionDriver : public rclcpp::Node {
+   public:
+    HikvisionDriver(const rclcpp::NodeOptions &options);
+    ~HikvisionDriver();
+
+   private:
+    struct Impl;
+    std::unique_ptr<Impl> pImpl;
+};
+
+}  // namespace hikvision_ros2_driver
+
+#endif  // HIKVISION_ROS2_DRIVER_HPP
@@ -0,0 +1,74 @@
+launch:
+  - arg:
+      name: camera_name
+  - arg:
+      name: container_name
+  - arg:
+      name: sync_mode
+      default: freerun
+  - arg:
+      name: trigger_activation
+      default: rising_edge
+  - arg:
+      name: trigger_delay_us
+      default: '0.0'
+  - arg:
+      name: device_timestamp_tick_hz
+      default: '0.0'
+  - arg:
+      name: use_timestamp_sync
+      default: 'true'
+  - arg:
+      name: time_offset_sec
+      default: '0.0'
+  - arg:
+      name: bias_alpha
+      default: '0.01'
+  - arg:
+      name: jump_warn_threshold_ms
+      default: '1000'
+  - arg:
+      name: clamp_backward_time
+      default: 'true'
+  - arg:
+      name: timestamp_diagnostics
+      default: 'false'
+  - arg:
+      name: timestamp_diagnostics_throttle_ms
+      default: '2000'
+
+  - load_composable_node:
+      target: $(var container_name)
+      composable_node:
+        - pkg: hikvision_ros2_driver
+          plugin: hikvision_ros2_driver::HikvisionDriver
+          name: hikvision_ros2_driver_node
+          namespace: /driver/hikvision/$(var camera_name)
+          param:
+            - name: camera_name
+              value: $(var camera_name)
+            - name: sync_mode
+              value: $(var sync_mode)
+            - name: trigger_activation
+              value: $(var trigger_activation)
+            - name: trigger_delay_us
+              value: $(var trigger_delay_us)
+            - name: device_timestamp_tick_hz
+              value: $(var device_timestamp_tick_hz)
+            - name: use_timestamp_sync
+              value: $(var use_timestamp_sync)
+            - name: time_offset_sec
+              value: $(var time_offset_sec)
+            - name: bias_alpha
+              value: $(var bias_alpha)
+            - name: jump_warn_threshold_ms
+              value: $(var jump_warn_threshold_ms)
+            - name: clamp_backward_time
+              value: $(var clamp_backward_time)
+            - name: timestamp_diagnostics
+              value: $(var timestamp_diagnostics)
+            - name: timestamp_diagnostics_throttle_ms
+              value: $(var timestamp_diagnostics_throttle_ms)
+          extra_arg:
+            - name: use_intra_process_comms
+              value: 'true'
@@ -0,0 +1,66 @@
+launch:
+  - arg:
+      name: camera_name
+  - arg:
+      name: sync_mode
+      default: freerun
+  - arg:
+      name: trigger_activation
+      default: rising_edge
+  - arg:
+      name: trigger_delay_us
+      default: '0.0'
+  - arg:
+      name: device_timestamp_tick_hz
+      default: '0.0'
+  - arg:
+      name: use_timestamp_sync
+      default: 'true'
+  - arg:
+      name: time_offset_sec
+      default: '0.0'
+  - arg:
+      name: bias_alpha
+      default: '0.01'
+  - arg:
+      name: jump_warn_threshold_ms
+      default: '1000'
+  - arg:
+      name: clamp_backward_time
+      default: 'true'
+  - arg:
+      name: timestamp_diagnostics
+      default: 'false'
+  - arg:
+      name: timestamp_diagnostics_throttle_ms
+      default: '2000'
+
+  - node:
+      pkg: hikvision_ros2_driver
+      exec: hikvision_ros2_driver_node
+      namespace: /driver/hikvision/$(var camera_name)
+      param:
+        - name: camera_name
+          value: $(var camera_name)
+        - name: sync_mode
+          value: $(var sync_mode)
+        - name: trigger_activation
+          value: $(var trigger_activation)
+        - name: trigger_delay_us
+          value: $(var trigger_delay_us)
+        - name: device_timestamp_tick_hz
+          value: $(var device_timestamp_tick_hz)
+        - name: use_timestamp_sync
+          value: $(var use_timestamp_sync)
+        - name: time_offset_sec
+          value: $(var time_offset_sec)
+        - name: bias_alpha
+          value: $(var bias_alpha)
+        - name: jump_warn_threshold_ms
+          value: $(var jump_warn_threshold_ms)
+        - name: clamp_backward_time
+          value: $(var clamp_backward_time)
+        - name: timestamp_diagnostics
+          value: $(var timestamp_diagnostics)
+        - name: timestamp_diagnostics_throttle_ms
+          value: $(var timestamp_diagnostics_throttle_ms)
@@ -0,0 +1,26 @@
+<?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>hikvision_ros2_driver</name>
+  <version>0.0.0</version>
+  <description>ros2 driver for hikvision camera</description>
+  <maintainer email="41963211+xinyang-go@users.noreply.github.com">xinyang</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>image_transport_plugins</depend>
+  <depend>hikvision_interface</depend>
+  <depend>driver_timestamp_sync</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,505 @@
+#include "hikvision_ros2_driver/hikvision_ros2_driver.hpp"
+
+// ros
+#include <hikvision_interface/msg/hik_image_info.hpp>
+#include <image_transport/image_transport.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <builtin_interfaces/msg/time.hpp>
+#include <driver_timestamp_sync/timestamp_sync.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <memory>
+#include <optional>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+// hikvision sdk
+#include <MvCameraControl.h>
+
+#define MV_CHECK_THROW(logger, func, ...)                                            \
+    do {                                                                              \
+        int nRet = func(__VA_ARGS__);                                                 \
+        if (MV_OK != nRet) {                                                          \
+            RCLCPP_ERROR(logger, "hikvision sdk error: " #func " = %d", nRet);    \
+            throw std::runtime_error("hikvision sdk call failed: " #func);          \
+        }                                                                             \
+    } while (0)
+
+using hikvision_interface::msg::HikImageInfo;
+
+namespace hikvision_ros2_driver {
+namespace {
+
+builtin_interfaces::msg::Time ToBuiltinTime(const rclcpp::Time & stamp)
+{
+    builtin_interfaces::msg::Time builtin;
+    const auto total_ns = stamp.nanoseconds();
+    builtin.sec = static_cast<int32_t>(total_ns / 1000000000ll);
+    builtin.nanosec = static_cast<uint32_t>(total_ns % 1000000000ll);
+    return builtin;
+}
+
+}  // namespace
+
+struct HikvisionDriver::Impl {
+    std::unique_ptr<rclcpp::Logger> logger;
+
+    void *handle{nullptr};
+    std::string camera_name;
+    std::string sync_mode{"freerun"};
+    std::string trigger_activation{"rising_edge"};
+    double trigger_delay_us{0.0};
+    double device_timestamp_tick_hz{0.0};
+    bool use_timestamp_sync{true};
+    bool timestamp_diagnostics{false};
+    int timestamp_diagnostics_throttle_ms{2000};
+    driver_timestamp_sync::TimestampSync timestamp_sync;
+    image_transport::Publisher img_pub;
+    std::shared_ptr<rclcpp::Publisher<HikImageInfo>> p_info_pub;
+
+    void configure_trigger(HikvisionDriver * node);
+    double resolve_device_timestamp_tick_hz(HikvisionDriver * node, MV_CC_DEVICE_INFO * device_info);
+    std::optional<rclcpp::Time> convert_device_stamp_to_ros_time(uint64_t raw_dev_tick) const;
+    uint32_t device_tick_ms(uint64_t raw_dev_tick) const;
+    rclcpp::Time compute_header_stamp(HikvisionDriver * node, const MV_FRAME_OUT_INFO_EX & frame_info,
+        const rclcpp::Time & host_rx_time, uint32_t converted_device_tick_ms, uint64_t raw_dev_tick);
+    static void image_callback_ex(unsigned char *pData, MV_FRAME_OUT_INFO_EX *pFrameInfo, void *pUser);
+};
+
+void HikvisionDriver::Impl::configure_trigger(HikvisionDriver * node)
+{
+    auto logger = node->get_logger();
+
+    if (sync_mode == "freerun") {
+        MV_CHECK_THROW(logger, MV_CC_SetEnumValue, handle, "TriggerMode", MV_TRIGGER_MODE_OFF);
+        RCLCPP_INFO(logger, "hikvision sync_mode=freerun");
+        return;
+    }
+
+    unsigned int trigger_source = 0;
+    if (sync_mode == "external_line0") {
+        trigger_source = MV_TRIGGER_SOURCE_LINE0;
+    } else if (sync_mode == "external_line2") {
+        trigger_source = MV_TRIGGER_SOURCE_LINE2;
+    } else {
+        throw std::runtime_error("unsupported sync_mode: " + sync_mode);
+    }
+
+    MV_CHECK_THROW(logger, MV_CC_SetEnumValueByString, handle, "TriggerSelector", "FrameStart");
+    MV_CHECK_THROW(logger, MV_CC_SetEnumValue, handle, "TriggerMode", MV_TRIGGER_MODE_ON);
+    MV_CHECK_THROW(logger, MV_CC_SetEnumValue, handle, "TriggerSource", trigger_source);
+
+    if (trigger_activation == "rising_edge") {
+        MV_CHECK_THROW(logger, MV_CC_SetEnumValueByString, handle, "TriggerActivation", "RisingEdge");
+    } else if (trigger_activation == "falling_edge") {
+        MV_CHECK_THROW(logger, MV_CC_SetEnumValueByString, handle, "TriggerActivation", "FallingEdge");
+    } else {
+        throw std::runtime_error("unsupported trigger_activation: " + trigger_activation);
+    }
+
+    if (trigger_delay_us > 0.0) {
+        MV_CHECK_THROW(logger, MV_CC_SetFloatValue, handle, "TriggerDelay", static_cast<float>(trigger_delay_us));
+    }
+
+    RCLCPP_INFO(
+        logger,
+        "hikvision sync_mode=%s trigger_activation=%s trigger_delay_us=%.3f",
+        sync_mode.c_str(),
+        trigger_activation.c_str(),
+        trigger_delay_us);
+}
+
+double HikvisionDriver::Impl::resolve_device_timestamp_tick_hz(
+    HikvisionDriver * node,
+    MV_CC_DEVICE_INFO * device_info)
+{
+    auto logger = node->get_logger();
+    MVCC_INTVALUE int_value;
+    memset(&int_value, 0, sizeof(int_value));
+
+    if (device_info->nTLayerType == MV_GIGE_DEVICE &&
+        MV_CC_GetIntValue(handle, "GevTimestampTickFrequency", &int_value) == MV_OK &&
+        int_value.nCurValue > 0)
+    {
+        const auto resolved = static_cast<double>(int_value.nCurValue);
+        RCLCPP_INFO(logger, "resolved GevTimestampTickFrequency=%.3f Hz", resolved);
+        return resolved;
+    }
+
+    if (device_timestamp_tick_hz > 0.0) {
+        RCLCPP_WARN(
+            logger,
+            "falling back to configured device_timestamp_tick_hz=%.3f Hz",
+            device_timestamp_tick_hz);
+        return device_timestamp_tick_hz;
+    }
+
+    throw std::runtime_error("failed to resolve device timestamp tick frequency");
+}
+
+std::optional<rclcpp::Time> HikvisionDriver::Impl::convert_device_stamp_to_ros_time(uint64_t raw_dev_tick) const
+{
+    if (device_timestamp_tick_hz <= 0.0) {
+        return std::nullopt;
+    }
+
+    const long double stamp_ns =
+        static_cast<long double>(raw_dev_tick) * 1000000000.0L / static_cast<long double>(device_timestamp_tick_hz);
+    return rclcpp::Time(static_cast<int64_t>(std::llround(stamp_ns)), RCL_ROS_TIME);
+}
+
+uint32_t HikvisionDriver::Impl::device_tick_ms(uint64_t raw_dev_tick) const
+{
+    if (device_timestamp_tick_hz <= 0.0) {
+        throw std::runtime_error("device timestamp tick frequency must be positive");
+    }
+
+    const long double tick_ms =
+        static_cast<long double>(raw_dev_tick) * 1000.0L / static_cast<long double>(device_timestamp_tick_hz);
+    return static_cast<uint32_t>(std::llround(tick_ms));
+}
+
+rclcpp::Time HikvisionDriver::Impl::compute_header_stamp(
+    HikvisionDriver * node,
+    const MV_FRAME_OUT_INFO_EX & frame_info,
+    const rclcpp::Time & host_rx_time,
+    uint32_t converted_device_tick_ms,
+    uint64_t raw_dev_tick)
+{
+    if (!use_timestamp_sync) {
+        return host_rx_time;
+    }
+
+    auto sync_result = timestamp_sync.reconstruct(converted_device_tick_ms, host_rx_time);
+
+    if (sync_result.large_jump_detected) {
+        RCLCPP_WARN_THROTTLE(
+            node->get_logger(), *node->get_clock(), 2000,
+            "hikvision timestamp jump detected: frame=%u raw_tick=%llu delta_ms=%u unwrapped_ms=%llu",
+            frame_info.nFrameNum,
+            static_cast<unsigned long long>(raw_dev_tick),
+            sync_result.delta_tick_ms,
+            static_cast<unsigned long long>(sync_result.unwrapped_device_tick_ms));
+    }
+
+    if (sync_result.backward_clamped) {
+        RCLCPP_WARN_THROTTLE(
+            node->get_logger(), *node->get_clock(), 2000,
+            "hikvision backward timestamp clamped: frame=%u stamp_ns=%lld",
+            frame_info.nFrameNum,
+            static_cast<long long>(sync_result.stamp.nanoseconds()));
+    }
+
+    if (timestamp_diagnostics) {
+        RCLCPP_INFO_THROTTLE(
+            node->get_logger(), *node->get_clock(), timestamp_diagnostics_throttle_ms,
+            "hikvision timestamp: frame=%u raw_tick=%llu tick_ms=%u host_rx_ns=%lld stamp_ns=%lld bias_ns=%lld init=%s",
+            frame_info.nFrameNum,
+            static_cast<unsigned long long>(raw_dev_tick),
+            converted_device_tick_ms,
+            static_cast<long long>(host_rx_time.nanoseconds()),
+            static_cast<long long>(sync_result.stamp.nanoseconds()),
+            static_cast<long long>(sync_result.filtered_bias_ns),
+            sync_result.initialized_this_call ? "true" : "false");
+    }
+
+    return sync_result.stamp;
+}
+
+void HikvisionDriver::Impl::image_callback_ex(
+    unsigned char *pData,
+    MV_FRAME_OUT_INFO_EX *pFrameInfo,
+    void *pUser) {
+    auto node = reinterpret_cast<HikvisionDriver *>(pUser);
+
+    const auto host_rx_time = node->get_clock()->now();
+    const uint64_t raw_dev_tick =
+        (static_cast<uint64_t>(pFrameInfo->nDevTimeStampHigh) << 32ull) |
+        static_cast<uint64_t>(pFrameInfo->nDevTimeStampLow);
+    const uint32_t converted_device_tick_ms = node->pImpl->device_tick_ms(raw_dev_tick);
+    const auto header_stamp =
+        node->pImpl->compute_header_stamp(node, *pFrameInfo, host_rx_time, converted_device_tick_ms, raw_dev_tick);
+
+    auto p_img_msg = std::make_unique<sensor_msgs::msg::Image>();
+    p_img_msg->header.frame_id = node->pImpl->camera_name;
+    p_img_msg->header.stamp = ToBuiltinTime(header_stamp);
+    p_img_msg->is_bigendian = false;
+    p_img_msg->width = pFrameInfo->nWidth;
+    p_img_msg->height = pFrameInfo->nHeight;
+
+    RCLCPP_INFO_THROTTLE(
+        node->get_logger(),
+        *node->get_clock(),
+        2000,
+        "pixel format=%d (0x%08X), width=%u, height=%u, frame_len=%u",
+        static_cast<int>(pFrameInfo->enPixelType),
+        static_cast<unsigned int>(pFrameInfo->enPixelType),
+        pFrameInfo->nWidth,
+        pFrameInfo->nHeight,
+        pFrameInfo->nFrameLen);
+
+    auto direct_copy = [&](const std::string &encoding, size_t bytes_per_pixel) -> bool {
+        const size_t expected_size =
+            static_cast<size_t>(pFrameInfo->nWidth) *
+            static_cast<size_t>(pFrameInfo->nHeight) *
+            bytes_per_pixel;
+
+        if (pFrameInfo->nFrameLen < expected_size) {
+            RCLCPP_ERROR(
+                node->get_logger(),
+                "frame too small: nFrameLen=%u < expected=%zu",
+                pFrameInfo->nFrameLen,
+                expected_size);
+            return false;
+        }
+
+        p_img_msg->encoding = encoding;
+        p_img_msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(
+            pFrameInfo->nWidth * bytes_per_pixel);
+        p_img_msg->data.resize(expected_size);
+        std::copy_n(pData, expected_size, p_img_msg->data.data());
+        return true;
+    };
+
+    auto convert_to_bgr8 = [&]() -> bool {
+        const size_t dst_size =
+            static_cast<size_t>(pFrameInfo->nWidth) *
+            static_cast<size_t>(pFrameInfo->nHeight) * 3u;
+
+        std::vector<uint8_t> bgr_buf(dst_size);
+
+        MV_CC_PIXEL_CONVERT_PARAM stConvertParam;
+        memset(&stConvertParam, 0, sizeof(stConvertParam));
+        stConvertParam.nWidth = pFrameInfo->nWidth;
+        stConvertParam.nHeight = pFrameInfo->nHeight;
+        stConvertParam.pSrcData = pData;
+        stConvertParam.nSrcDataLen = pFrameInfo->nFrameLen;
+        stConvertParam.enSrcPixelType = pFrameInfo->enPixelType;
+        stConvertParam.enDstPixelType = PixelType_Gvsp_BGR8_Packed;
+        stConvertParam.pDstBuffer = bgr_buf.data();
+        stConvertParam.nDstBufferSize = static_cast<unsigned int>(bgr_buf.size());
+
+        const int nRet = MV_CC_ConvertPixelType(node->pImpl->handle, &stConvertParam);
+        if (nRet != MV_OK) {
+            RCLCPP_ERROR(
+                node->get_logger(),
+                "MV_CC_ConvertPixelType failed: %d, src pixel=%d (0x%08X)",
+                nRet,
+                static_cast<int>(pFrameInfo->enPixelType),
+                static_cast<unsigned int>(pFrameInfo->enPixelType));
+            return false;
+        }
+
+        p_img_msg->encoding = sensor_msgs::image_encodings::BGR8;
+        p_img_msg->step =
+            static_cast<sensor_msgs::msg::Image::_step_type>(pFrameInfo->nWidth * 3u);
+        p_img_msg->data = std::move(bgr_buf);
+        return true;
+    };
+
+    bool ok = false;
+    switch (pFrameInfo->enPixelType) {
+        case PixelType_Gvsp_Mono8:
+            ok = direct_copy(sensor_msgs::image_encodings::MONO8, 1u);
+            break;
+
+        case PixelType_Gvsp_RGB8_Packed:
+            ok = direct_copy(sensor_msgs::image_encodings::RGB8, 3u);
+            break;
+
+        case PixelType_Gvsp_BGR8_Packed:
+            ok = direct_copy(sensor_msgs::image_encodings::BGR8, 3u);
+            break;
+
+        // 8-bit Bayer：直接转 BGR8，避免下游再 debayer
+        case PixelType_Gvsp_BayerRG8:
+        case PixelType_Gvsp_BayerBG8:
+        case PixelType_Gvsp_BayerGR8:
+        case PixelType_Gvsp_BayerGB8:
+            ok = convert_to_bgr8();
+            break;
+
+        // 10-bit packed Bayer：也统一转 BGR8
+        case PixelType_Gvsp_BayerRG10_Packed:
+        case PixelType_Gvsp_BayerBG10_Packed:
+        case PixelType_Gvsp_BayerGR10_Packed:
+        case PixelType_Gvsp_BayerGB10_Packed:
+            ok = convert_to_bgr8();
+            break;
+
+        default:
+            RCLCPP_ERROR_ONCE(
+                node->get_logger(),
+                "unsupported pixel format: %d (0x%08X)",
+                static_cast<int>(pFrameInfo->enPixelType),
+                static_cast<unsigned int>(pFrameInfo->enPixelType));
+            return;
+    }
+
+    if (!ok) {
+        return;
+    }
+
+    auto p_info_msg = std::make_unique<hikvision_interface::msg::HikImageInfo>();
+    p_info_msg->header.frame_id = node->pImpl->camera_name;
+    p_info_msg->header.stamp = ToBuiltinTime(header_stamp);
+    if (const auto device_stamp = node->pImpl->convert_device_stamp_to_ros_time(raw_dev_tick)) {
+        p_info_msg->dev_stamp = ToBuiltinTime(*device_stamp);
+    }
+    p_info_msg->frame_num = pFrameInfo->nFrameNum;
+    p_info_msg->gain = pFrameInfo->fGain;
+    p_info_msg->exposure = pFrameInfo->fExposureTime;
+    p_info_msg->red = pFrameInfo->nRed;
+    p_info_msg->green = pFrameInfo->nGreen;
+    p_info_msg->blue = pFrameInfo->nBlue;
+
+    node->pImpl->img_pub.publish(std::move(p_img_msg));
+    node->pImpl->p_info_pub->publish(std::move(p_info_msg));
+}
+
+HikvisionDriver::HikvisionDriver(const rclcpp::NodeOptions &options)
+    : rclcpp::Node("hikvision_ros2_driver_node", options), pImpl(std::make_unique<Impl>()) {
+    auto logger = get_logger();
+    pImpl->logger = std::make_unique<rclcpp::Logger>(logger);
+
+    declare_parameter<std::string>("camera_name", "");
+    declare_parameter<std::string>("sync_mode", "freerun");
+    declare_parameter<std::string>("trigger_activation", "rising_edge");
+    declare_parameter<double>("trigger_delay_us", 0.0);
+    declare_parameter<double>("device_timestamp_tick_hz", 0.0);
+    declare_parameter<bool>("use_timestamp_sync", true);
+    declare_parameter<double>("time_offset_sec", 0.0);
+    declare_parameter<double>("bias_alpha", 0.01);
+    declare_parameter<int>("jump_warn_threshold_ms", 1000);
+    declare_parameter<bool>("clamp_backward_time", true);
+    declare_parameter<bool>("timestamp_diagnostics", false);
+    declare_parameter<int>("timestamp_diagnostics_throttle_ms", 2000);
+
+    pImpl->camera_name = get_parameter("camera_name").as_string();
+    pImpl->sync_mode = get_parameter("sync_mode").as_string();
+    pImpl->trigger_activation = get_parameter("trigger_activation").as_string();
+    pImpl->trigger_delay_us = get_parameter("trigger_delay_us").as_double();
+    pImpl->device_timestamp_tick_hz = get_parameter("device_timestamp_tick_hz").as_double();
+    pImpl->use_timestamp_sync = get_parameter("use_timestamp_sync").as_bool();
+    pImpl->timestamp_diagnostics = get_parameter("timestamp_diagnostics").as_bool();
+    pImpl->timestamp_diagnostics_throttle_ms = get_parameter("timestamp_diagnostics_throttle_ms").as_int();
+
+    driver_timestamp_sync::TimestampSyncConfig config;
+    config.time_offset_sec = get_parameter("time_offset_sec").as_double();
+    config.bias_alpha = get_parameter("bias_alpha").as_double();
+    config.jump_warn_threshold_ms = static_cast<uint32_t>(get_parameter("jump_warn_threshold_ms").as_int());
+    config.clamp_backward_time = get_parameter("clamp_backward_time").as_bool();
+    pImpl->timestamp_sync.set_config(config);
+
+    if (pImpl->camera_name.empty()) {
+        throw std::runtime_error("camera_name parameter must not be empty");
+    }
+
+    RCLCPP_INFO(logger, "trying to open camera: '%s'", pImpl->camera_name.c_str());
+    RCLCPP_INFO(
+        logger,
+        "timestamp_sync: enabled=%s offset=%.6f alpha=%.3f jump_warn_ms=%u clamp_backward=%s diagnostics=%s throttle_ms=%d",
+        pImpl->use_timestamp_sync ? "true" : "false",
+        config.time_offset_sec,
+        config.bias_alpha,
+        config.jump_warn_threshold_ms,
+        config.clamp_backward_time ? "true" : "false",
+        pImpl->timestamp_diagnostics ? "true" : "false",
+        pImpl->timestamp_diagnostics_throttle_ms);
+
+    rclcpp::QoS qos(1);
+    pImpl->img_pub = image_transport::create_publisher(this, "raw/image", qos.get_rmw_qos_profile());
+    pImpl->p_info_pub = create_publisher<HikImageInfo>("info", qos);
+
+    MV_CC_DEVICE_INFO_LIST stDeviceList;
+    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));
+    MV_CHECK_THROW(logger, MV_CC_EnumDevices, MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
+
+    MV_CC_DEVICE_INFO * matched_device = nullptr;
+    for (uint32_t nDeviceId = 0; nDeviceId < stDeviceList.nDeviceNum; nDeviceId++) {
+        auto *pDeviceInfo = stDeviceList.pDeviceInfo[nDeviceId];
+        std::string user_defined_name;
+        if (pDeviceInfo->nTLayerType == MV_GIGE_DEVICE) {
+            user_defined_name = reinterpret_cast<const char *>(pDeviceInfo->SpecialInfo.stGigEInfo.chUserDefinedName);
+            if (user_defined_name == pImpl->camera_name) {
+                int nIp1 = ((pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24);
+                int nIp2 = ((pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16);
+                int nIp3 = ((pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8);
+                int nIp4 = (pDeviceInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff);
+                RCLCPP_INFO(
+                    logger,
+                    "[%s]: GIGE, %s, %d.%d.%d.%d",
+                    user_defined_name.c_str(),
+                    pDeviceInfo->SpecialInfo.stGigEInfo.chModelName,
+                    nIp1,
+                    nIp2,
+                    nIp3,
+                    nIp4);
+                matched_device = pDeviceInfo;
+                break;
+            }
+        } else if (pDeviceInfo->nTLayerType == MV_USB_DEVICE) {
+            user_defined_name = reinterpret_cast<const char *>(pDeviceInfo->SpecialInfo.stUsb3VInfo.chUserDefinedName);
+            if (user_defined_name == pImpl->camera_name) {
+                RCLCPP_INFO(
+                    logger,
+                    "[%s]: USB, %s",
+                    user_defined_name.c_str(),
+                    pDeviceInfo->SpecialInfo.stUsb3VInfo.chModelName);
+                matched_device = pDeviceInfo;
+                break;
+            }
+        } else {
+            RCLCPP_WARN(logger, "type(%d) not support", pDeviceInfo->nTLayerType);
+        }
+    }
+
+    if (matched_device == nullptr) {
+        throw std::runtime_error("camera not found: " + pImpl->camera_name);
+    }
+
+    MV_CHECK_THROW(logger, MV_CC_CreateHandle, &pImpl->handle, matched_device);
+    try {
+        MV_CHECK_THROW(logger, MV_CC_OpenDevice, pImpl->handle);
+        pImpl->device_timestamp_tick_hz = pImpl->resolve_device_timestamp_tick_hz(this, matched_device);
+        pImpl->configure_trigger(this);
+        MV_CHECK_THROW(logger, MV_CC_RegisterImageCallBackEx, pImpl->handle, &HikvisionDriver::Impl::image_callback_ex,
+            this);
+        MV_CHECK_THROW(logger, MV_CC_StartGrabbing, pImpl->handle);
+    } catch (...) {
+        if (pImpl->handle != nullptr) {
+            MV_CC_CloseDevice(pImpl->handle);
+            MV_CC_DestroyHandle(pImpl->handle);
+            pImpl->handle = nullptr;
+        }
+        throw;
+    }
+}
+
+HikvisionDriver::~HikvisionDriver() {
+    if (pImpl->handle == nullptr) return;
+    auto logger = get_logger();
+
+    int nRet = MV_CC_StopGrabbing(pImpl->handle);
+    if (MV_OK != nRet) {
+        RCLCPP_ERROR(logger, "hikvision sdk error: MV_CC_StopGrabbing = %d", nRet);
+    }
+    nRet = MV_CC_CloseDevice(pImpl->handle);
+    if (MV_OK != nRet) {
+        RCLCPP_ERROR(logger, "hikvision sdk error: MV_CC_CloseDevice = %d", nRet);
+    }
+    nRet = MV_CC_DestroyHandle(pImpl->handle);
+    if (MV_OK != nRet) {
+        RCLCPP_ERROR(logger, "hikvision sdk error: MV_CC_DestroyHandle = %d", nRet);
+    }
+    pImpl->handle = nullptr;
+}
+
+}  // namespace hikvision_ros2_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(hikvision_ros2_driver::HikvisionDriver);
@@ -0,0 +1,118 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nmea_navsat_driver
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.0.0 (2022-06-25)
+------------------
+
+* Replace dependency on transforms3d pip package to tf_transformations. (`#147 <https://github.com/evenator/nmea_navsat_driver/issues/147>`_)
+* Added log for successful connection. (`#149 <https://github.com/evenator/nmea_navsat_driver/issues/149>`_)
+* Removed printing every time the checksum is checked. (`#148 <https://github.com/evenator/nmea_navsat_driver/issues/148>`_)
+* nmea_socket_driver fixes for ROS2 (`#127 <https://github.com/evenator/nmea_navsat_driver/issues/127>`_)
+  * Fix white space for PEP8 compliance
+  * Fix socket driver for ROS2 Foxy
+  * Add config file for socket driver
+  * Decode bytes as ASCII
+* Fix bug where ROS node is not assigned correctly (`#114 <https://github.com/evenator/nmea_navsat_driver/issues/114>`_)
+  Fixes `#71 <https://github.com/evenator/nmea_navsat_driver/issues/71>`_
+* Update launch file for ROS2 Foxy (`#110 <https://github.com/evenator/nmea_navsat_driver/issues/110>`_)
+  * Remove get_default_launch_description, which no longer exists.
+  * Update the parameters of the Node initializer to the Foxy API.
+* Update pyserial dependency to make it findable with rosdep (`#109 <https://github.com/evenator/nmea_navsat_driver/issues/109>`_)
+  This allows rosdep to find the correct serial package and install it.
+* ROS2: Eloquent changes for python packages (`#101 <https://github.com/evenator/nmea_navsat_driver/issues/101>`_)
+  Fix some warnings when installing packages that first appear in ROS2 Eloquent:
+  - A missing the resource folder
+  - Not explicitly installing package.xml in the share folder.
+* ROS2 updates for Dashing (`#80 <https://github.com/evenator/nmea_navsat_driver/issues/80>`_)
+  Change subscriptions and parameters to conform to API changes in ROS 2 Dashing.
+* Remove scripts directory in favor of nodes subpackage (`#77 <https://github.com/evenator/nmea_navsat_driver/issues/77>`_)
+  Since these modules will no longer be called as scripts in ROS 2,
+  move them to a nodes subpackage, and remove the option to call them
+  as executables.
+  Addresses `#75 <https://github.com/evenator/nmea_navsat_driver/issues/75>`_ for ROS 2.
+* Fix nmea_topic_serial_reader name (`#74 <https://github.com/evenator/nmea_navsat_driver/issues/74>`_)
+  - Fix `#72 <https://github.com/evenator/nmea_navsat_driver/issues/72>`_ (no module name scripts.nmea_topic_serial_driver)
+  - Fix exception handling in nmea_topic_serial_reader ('rclpy' has no attribute 'ROSInterruptException')
+* Clean up launch file and make it runnable with ROS2 launch (`#73 <https://github.com/evenator/nmea_navsat_driver/issues/73>`_)
+  Fixes `#70 <https://github.com/evenator/nmea_navsat_driver/issues/70>`_
+  - Rename config file and put it into a config directory.
+  - Make setup.py install the launch and config files.
+  - Rename the launch file with the .launch.py suffix used by OSRF
+  packages.
+  - Refactor the launch file so that it works with `ros2 launch`.
+* Fix PEP8 Violations and update setup.cfg file for pycodestyle. (`#69 <https://github.com/evenator/nmea_navsat_driver/issues/69>`_)
+* Port to ROS 2 (`#64 <https://github.com/evenator/nmea_navsat_driver/issues/64>`_)
+  Initial work to port nmea_navsat_driver to ROS2 by @klintan.
+* Add nmea_serial_driver launch file (`#60 <https://github.com/evenator/nmea_navsat_driver/issues/60>`_)
+  Add example nmea_serial_driver launch file.
+* Remove automatic prefixing of forward slash to frame_id. (`#33 <https://github.com/evenator/nmea_navsat_driver/issues/33>`_/`#57 <https://github.com/evenator/nmea_navsat_driver/issues/57>`_)
+  To be consistent with the current default behavior, the default frame_id has been set to /gps with the prepended forward slash.
+* Add support for IMU aided GPS systems (`#30 <https://github.com/evenator/nmea_navsat_driver/issues/30>`_/`#58 <https://github.com/evenator/nmea_navsat_driver/issues/58>`_)
+  * Add support for IMU aided GPS systems like the Applanix POS/MV, whose NMEA strings typically begin '$IN'. (e.g. $INGGA).
+  * Add support for VTG messages, which contain Course Over Ground and Speed Made Good. These are useful when not using RMC messages and you don't have a heading sensor.
+* Add support for publishing heading from GPHDT as a QuaternionStamped message on the topic /heading (`#25 <https://github.com/evenator/nmea_navsat_driver/issues/25>`_)
+* Improve Covariance Estimation (`#46 <https://github.com/evenator/nmea_navsat_driver/issues/46>`_)
+  Use GST covariance where available, otherwise uses default covariances estimated from fix type.
+  The previous implementation set covariance to HDOP^2. Instead, it should multiply that by the measurement variance. HDOP should be greater than 1.0.
+* Add Socket Driver (`#32 <https://github.com/evenator/nmea_navsat_driver/issues/32>`_)
+  Add a NMEA socket driver node, which is like the existing serial driver node, but instead of attaching to a TTY handle from a serial port, it listens to a UDP port for NMEA sentences.
+* Add code to handle serial exception to allow node to exit cleanly (`#52 <https://github.com/evenator/nmea_navsat_driver/issues/52>`_)
+  - Catch Serial exceptions and exit cleanly, instead of printing Python stack trace.
+  - Catch Serial exception when opening the serial port, log a FATAL message, and exit instead of printing Python stack trace.
+* Remove MSL compensation (`#36 <https://github.com/evenator/nmea_navsat_driver/issues/36>`_)
+  Fix for `#29 <https://github.com/evenator/nmea_navsat_driver/issues/29>`_ Altitude vs Elipsoid Height.
+* Add GLONASS support
+  GLONASS capable devices send different NMEA sentences, which are
+  basically identical to the GPS sentences, but with other prefixes.
+* Updated driver to accept status of 9 which some novatel receivers report for a WAAS (SBAS) fix.
+  See http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
+
+0.5.0 (2015-04-23)
+------------------
+* Release to Jade.
+
+0.4.2 (2015-04-23)
+------------------
+* Fix remaining parse problem with NovAtel receivers (empty field specified for num_satellite).
+
+0.4.1 (2014-08-03)
+------------------
+* Add debug logging output to the parser (PR #8, Mike Purvis)
+* Add queue size argument to publishers to fix warning on Indigo (PR #9, Mike Purvis)
+* Add support for roslint and some related cleanup (PR #10, Mike Purvis)
+ 
+0.4.0 (2014-05-04)
+-------------------
+* Initial release for Indigo
+* Fix #5: Empty fields spam rosout with warnings. Driver now outputs sensor_msgs/NavSatFix messages that may contain NaNs in position and covariance when receiving invalid fixes from the device.
+
+0.3.3 (2013-10-08)
+-------------------
+* Allow the driver to output velocity information anytime an RMC message is received
+
+0.3.2 (2013-07-21)
+-------------------
+* Moved to nmea_navsat_driver package
+* Removed .py extensions from new-in-Hydro scripts
+* Now uses nmea_msgs/Sentence instead of custom sentence type
+* nmea_topic_driver reads the `frame_id` parameter from the sentence, not from the parameter server
+
+0.3.1 (2013-05-07)
+-------------------
+* Removed incorrect find_package dependencies
+
+0.3.0 (2013-05-05)
+-------------------
+* Initial release for Hydro
+* Converted to Catkin
+* nmea_gps_driver.py is now deprecated and will be removed in I-Turtle. Replacement node is nmea_serial_driver.py .
+* Refactored code into NMEA parser, common ROS driver and separate nodes for reading directly from serial or from topic.
+* Bugs fixed:
+  - nmea_gps_driver crashes when a sentence doesn't have a checksum * character ( http://kforge.ros.org/gpsdrivers/trac/ticket/4 )
+  - Add ability for nmea_gps_driver to support reading from string topic ( https://github.com/ros-drivers/nmea_gps_driver/issues/1 ). Use the nmea_topic_driver.py node to get this support.
+
+0.2.0 (2012-03-15)
+------------------
+* Initial version (released into Fuerte)
+* Supports GGA or RMC+GSA sentences to generate sensor_msgs/NavSatFix messages
@@ -0,0 +1,11 @@
+nmea_navsat_driver
+===============
+
+ROS driver to parse NMEA strings and publish standard ROS NavSat message types. Does not require the GPSD daemon to be running.
+
+API
+---
+
+This package has no released Code API.
+
+The ROS API documentation and other information can be found at http://ros.org/wiki/nmea_navsat_driver
@@ -0,0 +1,8 @@
+nmea_navsat_driver:
+  ros__parameters:
+    port: "/dev/wheeltec_gnss"
+    baud: 115200
+    frame_id: "navsat_link"
+    time_ref_source: "gps"
+    useRMC: False
+
@@ -0,0 +1,55 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+""" A simple launch file for the nmea_serial_driver node. """
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription, LaunchIntrospector, LaunchService
+from launch_ros import actions
+
+
+def generate_launch_description():
+    """Generate a launch description for a single serial driver."""
+    config_file = os.path.join(get_package_share_directory("nmea_navsat_driver"), "config", "nmea_serial_driver.yaml")
+    driver_node = actions.Node(
+        package='nmea_navsat_driver',
+        executable='nmea_serial_driver',
+        output='screen',
+        parameters=[config_file])
+
+    return LaunchDescription([driver_node])
+
+
+def main(argv):
+    ld = generate_launch_description()
+
+    print('Starting introspection of launch description...')
+    print('')
+
+    print(LaunchIntrospector().format_launch_description(ld))
+
+    print('')
+    print('Starting launch of launch description...')
+    print('')
+
+    ls = LaunchService()
+    ls.include_launch_description(ld)
+    return ls.run()
+
+
+if __name__ == '__main__':
+    main(sys.argv)
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+    <name>nmea_navsat_driver</name>
+    <version>2.0.0</version>
+    <description>
+        Package to parse NMEA strings and publish a very simple GPS message. Does not
+        require or use the GPSD deamon.
+    </description>
+
+    <maintainer email="evenator@gmail.com">Ed Venator</maintainer>
+
+    <license>BSD</license>
+
+    <url type="website">http://ros.org/wiki/nmea_navsat_driver</url>
+
+    <author email="eric@ericperko.com">Eric Perko</author>
+    <author>Steven Martin</author>
+
+    <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>nmea_msgs</exec_depend>
+    <exec_depend>rclpy</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>python3-numpy</exec_depend>
+    <exec_depend>python3-serial</exec_depend>
+    <exec_depend>tf_transformations</exec_depend>
+
+    <test_depend>python3-pytest</test_depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>
@@ -0,0 +1,10 @@
+[develop]
+script_dir=$base/lib/nmea_navsat_driver
+
+[install]
+install_scripts=$base/lib/nmea_navsat_driver
+
+[pycodestyle]
+max-line-length = 120
+statistics = True
+show-pep8 = True
@@ -0,0 +1,38 @@
+from glob import glob
+import os
+from setuptools import setup
+
+PACKAGE_NAME = "nmea_navsat_driver"
+SHARE_DIR = os.path.join("share", PACKAGE_NAME)
+
+setup(
+    name=PACKAGE_NAME,
+    version='2.0.0',
+    packages=["libnmea_navsat_driver", "libnmea_navsat_driver.nodes"],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+         ['resource/' + PACKAGE_NAME]),
+        ('share/' + PACKAGE_NAME, ['package.xml']),
+        (os.path.join(SHARE_DIR, "launch"), glob(os.path.join("launch", "*.launch.py"))),
+        (os.path.join(SHARE_DIR, "config"), glob(os.path.join("config", "*.yaml")))],
+    package_dir={'': 'src', },
+    py_modules=[],
+    zip_safe=True,
+    install_requires=['setuptools',
+                      'pyserial',
+                      'numpy',
+                      'pyyaml'],
+    author='Eric Perko',
+    maintainer='Ed Venator',
+    keywords=['ROS2'],
+    description='Package to parse NMEA strings and publish a very simple GPS message.',
+    license='BSD',
+    entry_points={
+        'console_scripts': ['nmea_serial_driver = libnmea_navsat_driver.nodes.nmea_serial_driver:main',
+                            'nmea_socket_driver = libnmea_navsat_driver.nodes.nmea_socket_driver:main',
+                            'nmea_tcpclient_driver = libnmea_navsat_driver.nodes.nmea_tcpclient_driver:main',
+                            'nmea_topic_driver = libnmea_navsat_driver.nodes.nmea_topic_driver:main',
+                            'nmea_topic_serial_reader = libnmea_navsat_driver.nodes.nmea_topic_serial_reader:main',
+                            ],
+    }
+)
@@ -0,0 +1,48 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Eric Perko
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the names of the authors nor the names of their
+#    affiliated organizations may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+# Check the NMEA sentence checksum. Return True if passes and False if failed
+def check_nmea_checksum(nmea_sentence):
+    split_sentence = nmea_sentence.split('*')
+    if len(split_sentence) != 2:
+        # No checksum bytes were found... improperly formatted/incomplete NMEA data?
+        return False
+    transmitted_checksum = split_sentence[1].strip()
+
+    # Remove the $ at the front
+    data_to_checksum = split_sentence[0][1:]
+    checksum = 0
+    for c in data_to_checksum:
+        checksum ^= ord(c)
+
+    return ("%02X" % checksum) == transmitted_checksum.upper()
@@ -0,0 +1,394 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Eric Perko
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the names of the authors nor the names of their
+#    affiliated organizations may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import math
+
+import rclpy
+
+from rclpy.node import Node
+from sensor_msgs.msg import NavSatFix, NavSatStatus, TimeReference
+from geometry_msgs.msg import TwistStamped, QuaternionStamped
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import TwistWithCovarianceStamped
+from tf_transformations import quaternion_from_euler
+from libnmea_navsat_driver.checksum_utils import check_nmea_checksum
+from libnmea_navsat_driver import parser
+
+from nmea_msgs.msg import Sentence
+from nmea_msgs.msg import Gpgga   #wheeltec tues
+from std_msgs.msg import Float32
+
+class Ros2NMEADriver(Node):
+    def __init__(self):
+        super().__init__('nmea_navsat_driver')
+
+        self.fix_pub = self.create_publisher(NavSatFix, '/gps/fix', 10)
+        self.vel_pub = self.create_publisher(TwistStamped, '/gps/vel', 10)
+
+        self.eagleye_vel_pub = self.create_publisher(TwistWithCovarianceStamped, '/gps/nmea/vel', 10)
+
+        self.pose_pub = self.create_publisher(PoseStamped, '/gps/pose', 10)
+        self.nmea_pub = self.create_publisher(Sentence, 'nmea_sentence', 10)
+        self.gpgga_pub = self.create_publisher(Gpgga, '/gnss/gpgga', 10)
+        self.heading_pub = self.create_publisher(QuaternionStamped, 'heading', 10)
+        self.heading_deg_pub = self.create_publisher(Float32, 'heading_deg', 10)
+
+        self.time_ref_pub = self.create_publisher(TimeReference, 'time_reference', 10)
+
+        self.time_ref_source = self.declare_parameter('time_ref_source', 'gps').value
+        self.use_RMC = self.declare_parameter('useRMC', False).value
+        self.valid_fix = False
+        self.rotation_data=0.0
+
+        self.rotation_data = self.declare_parameter('rotation', 0.0).value
+        print(self.rotation_data)
+        # epe = estimated position error
+        self.default_epe_quality0 = self.declare_parameter('epe_quality0', 1000000).value
+        self.default_epe_quality1 = self.declare_parameter('epe_quality1', 4.0).value
+        self.default_epe_quality2 = self.declare_parameter('epe_quality2', 0.1).value
+        self.default_epe_quality4 = self.declare_parameter('epe_quality4', 0.02).value
+        self.default_epe_quality5 = self.declare_parameter('epe_quality5', 4.0).value
+        self.default_epe_quality9 = self.declare_parameter('epe_quality9', 3.0).value
+
+        self.using_receiver_epe = False
+
+        self.lon_std_dev = float("nan")
+        self.lat_std_dev = float("nan")
+        self.alt_std_dev = float("nan")
+
+        """Format for this dictionary is the fix type from a GGA message as the key, with
+        each entry containing a tuple consisting of a default estimated
+        position error, a NavSatStatus value, and a NavSatFix covariance value."""
+        self.gps_qualities = {
+            # Unknown
+            -1: [
+                self.default_epe_quality0,
+                NavSatStatus.STATUS_NO_FIX,
+                NavSatFix.COVARIANCE_TYPE_UNKNOWN
+            ],
+            # Invalid
+            0: [
+                self.default_epe_quality0,
+                NavSatStatus.STATUS_NO_FIX,
+                NavSatFix.COVARIANCE_TYPE_UNKNOWN
+            ],
+            # SPS
+            1: [
+                self.default_epe_quality1,
+                NavSatStatus.STATUS_FIX,
+                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+            ],
+            # DGPS
+            2: [
+                self.default_epe_quality2,
+                NavSatStatus.STATUS_SBAS_FIX,
+                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+            ],
+            # RTK Fix
+            4: [
+                self.default_epe_quality4,
+                NavSatStatus.STATUS_GBAS_FIX,
+                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+            ],
+            # RTK Float
+            5: [
+                self.default_epe_quality5,
+                NavSatStatus.STATUS_GBAS_FIX,
+                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+            ],
+            # WAAS
+            9: [
+                self.default_epe_quality9,
+                NavSatStatus.STATUS_GBAS_FIX,
+                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+            ]
+        }
+
+    # Returns True if we successfully did something with the passed in
+    # nmea_string
+    def add_sentence(self, nmea_string, frame_id, timestamp=None):
+        if not check_nmea_checksum(nmea_string):
+            #self.get_logger().warn("Received a sentence with an invalid checksum. " +
+            #                       "Sentence was: %s" % nmea_string)
+            return False
+
+        parsed_sentence = parser.parse_nmea_sentence(nmea_string)
+        if not parsed_sentence:
+            #self.get_logger().debug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string)
+            return False
+
+        if timestamp:
+            current_time = timestamp
+        else:
+            current_time = self.get_clock().now().to_msg()
+
+        current_fix = NavSatFix()
+        current_fix.header.stamp = current_time
+        current_fix.header.frame_id = frame_id
+        current_time_ref = TimeReference()
+        current_time_ref.header.stamp = current_time
+        current_time_ref.header.frame_id = frame_id
+        if self.time_ref_source:
+            current_time_ref.source = self.time_ref_source
+        else:
+            current_time_ref.source = frame_id
+
+        if not self.use_RMC and 'GGA' in parsed_sentence:
+            current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+
+            data = parsed_sentence['GGA']
+            fix_type = data['fix_type']
+
+            if not (fix_type in self.gps_qualities):
+                fix_type = -1
+            gps_qual = self.gps_qualities[fix_type]
+            default_epe = gps_qual[0]
+            current_fix.status.status = gps_qual[1]
+
+            current_fix.position_covariance_type = gps_qual[2]
+            if current_fix.status.status > 0:
+                self.valid_fix = True
+            else:
+                self.valid_fix = False
+
+            current_fix.status.service = NavSatStatus.SERVICE_GPS
+            latitude = data['latitude']
+            if data['latitude_direction'] == 'S':
+                latitude = -latitude
+            current_fix.latitude = latitude
+
+            longitude = data['longitude']
+            if data['longitude_direction'] == 'W':
+                longitude = -longitude
+            current_fix.longitude = longitude
+
+            # Altitude is above ellipsoid, so adjust for mean-sea-level
+            altitude = data['altitude'] + data['mean_sea_level']
+            current_fix.altitude = altitude
+
+            # use default epe std_dev unless we've received a GST sentence with epes
+            if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
+                self.lon_std_dev = default_epe
+            if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
+                self.lat_std_dev = default_epe
+            if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
+                self.alt_std_dev = default_epe * 2
+
+            hdop = data['hdop']
+            current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
+            current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
+            current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2  # FIXME
+
+            self.fix_pub.publish(current_fix)
+
+            if not math.isnan(data['utc_time']):
+                current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
+                self.last_valid_fix_time = current_time_ref
+                self.time_ref_pub.publish(current_time_ref)
+
+            # wheeltec tues debug    
+            gga_msg = Gpgga()
+            gga_msg.header.stamp = current_time
+            gga_msg.header.frame_id = frame_id
+            
+            gga_msg.utc_seconds = float(data['utc_time']) if not math.isnan(data['utc_time']) else 0.0
+            gga_msg.lat = latitude
+            gga_msg.lat_dir = data['latitude_direction']
+            gga_msg.lon = longitude
+            gga_msg.lon_dir = data['longitude_direction']
+            gga_msg.gps_qual = data['fix_type']
+            gga_msg.num_sats = data['num_satellites']
+            gga_msg.hdop = data['hdop']
+            gga_msg.alt = altitude
+            gga_msg.altitude_units = data.get('altitude_units', 'M')  
+            gga_msg.undulation = data['mean_sea_level']
+            gga_msg.undulation_units = data.get('mean_sea_level_units', 'M')
+            gga_msg.diff_age = int(round(data.get('age_gps_data', 0.0)))
+            gga_msg.station_id = data.get('ref_station_id', '')
+
+            self.gpgga_pub.publish(gga_msg)
+
+
+            nmea_msg = Sentence()
+            nmea_msg.header.stamp = current_time
+            nmea_msg.header.frame_id = frame_id
+            nmea_msg.sentence = nmea_string
+            self.nmea_pub.publish(nmea_msg)
+
+        elif not self.use_RMC and 'VTG' in parsed_sentence:
+            data = parsed_sentence['VTG']
+
+            # Only report VTG data when you've received a valid GGA fix as well.
+            if self.valid_fix:
+                current_vel = TwistStamped()
+                current_vel.header.stamp = current_time
+                current_vel.header.frame_id = frame_id
+                current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course'])
+                current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course'])
+                self.vel_pub.publish(current_vel)
+
+
+                eagleye_current_vel = TwistWithCovarianceStamped()
+                eagleye_current_vel.header.stamp = current_time
+                eagleye_current_vel.header.frame_id = frame_id
+                eagleye_current_vel.twist.twist.linear.x = data['speed'] * math.sin(data['true_course'])
+                eagleye_current_vel.twist.twist.linear.y = data['speed'] * math.cos(data['true_course'])
+
+                std_dev = 0.2  # m/s
+                var = std_dev ** 2  # = 0.004
+                eagleye_current_vel.twist.covariance = [
+                var, 0.0, 0.0, 0.0, 0.0, 0.0,   # vx
+                0.0, var, 0.0, 0.0, 0.0, 0.0,   # vy
+                0.0, 0.0, 9999.0, 0.0, 0.0, 0.0,  # vz unknown
+                0.0, 0.0, 0.0, 9999.0, 0.0, 0.0,  # roll rate
+                0.0, 0.0, 0.0, 0.0, 9999.0, 0.0,  # pitch rate
+                0.0, 0.0, 0.0, 0.0, 0.0, 9999.0   # yaw rate
+                ]
+
+                self.eagleye_vel_pub.publish(eagleye_current_vel)
+
+        elif 'RMC' in parsed_sentence:
+            data = parsed_sentence['RMC']
+            nmea_msg = Sentence()
+            nmea_msg.header.stamp = current_time
+            nmea_msg.header.frame_id = frame_id
+            nmea_msg.sentence = nmea_string
+            self.nmea_pub.publish(nmea_msg)
+
+            # Only publish a fix from RMC if the use_RMC flag is set.
+            if self.use_RMC:
+                if data['fix_valid']:
+                    current_fix.status.status = NavSatStatus.STATUS_FIX
+                else:
+                    current_fix.status.status = NavSatStatus.STATUS_NO_FIX
+
+                current_fix.status.service = NavSatStatus.SERVICE_GPS
+
+                latitude = data['latitude']
+                if data['latitude_direction'] == 'S':
+                    latitude = -latitude
+                current_fix.latitude = latitude
+
+                longitude = data['longitude']
+                if data['longitude_direction'] == 'W':
+                    longitude = -longitude
+                current_fix.longitude = longitude
+
+                current_fix.altitude = float('NaN')
+                current_fix.position_covariance_type = \
+                    NavSatFix.COVARIANCE_TYPE_UNKNOWN
+
+                self.fix_pub.publish(current_fix)
+
+                if not math.isnan(data['utc_time']):
+                    current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
+                    self.time_ref_pub.publish(current_time_ref)
+
+            # Publish velocity from RMC regardless, since GGA doesn't provide it.
+            if data['fix_valid']:
+                current_vel = TwistStamped()
+                current_vel.header.stamp = current_time
+                current_vel.header.frame_id = frame_id
+                current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course'])
+                current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course'])
+                self.vel_pub.publish(current_vel)
+
+                eagleye_current_vel = TwistWithCovarianceStamped()
+                eagleye_current_vel.header.stamp = current_time
+                eagleye_current_vel.header.frame_id = frame_id
+                eagleye_current_vel.twist.twist.linear.x = data['speed'] * math.sin(data['true_course'])
+                eagleye_current_vel.twist.twist.linear.y = data['speed'] * math.cos(data['true_course'])
+
+                std_dev = 0.2  # m/s
+                var = std_dev ** 2  # = 0.004
+                eagleye_current_vel.twist.covariance = [
+                var, 0.0, 0.0, 0.0, 0.0, 0.0,   # vx
+                0.0, var, 0.0, 0.0, 0.0, 0.0,   # vy
+                0.0, 0.0, 9999.0, 0.0, 0.0, 0.0,  # vz unknown
+                0.0, 0.0, 0.0, 9999.0, 0.0, 0.0,  # roll rate
+                0.0, 0.0, 0.0, 0.0, 9999.0, 0.0,  # pitch rate
+                0.0, 0.0, 0.0, 0.0, 0.0, 9999.0   # yaw rate
+                ]
+                self.eagleye_vel_pub.publish(eagleye_current_vel)
+
+        elif 'GST' in parsed_sentence:
+            data = parsed_sentence['GST']
+
+            # Use receiver-provided error estimate if available
+            self.using_receiver_epe = True
+            self.lon_std_dev = data['lon_std_dev']
+            self.lat_std_dev = data['lat_std_dev']
+            self.alt_std_dev = data['alt_std_dev']
+        elif 'HDT' in parsed_sentence:
+            data = parsed_sentence['HDT']
+            if data['heading']:
+                current_heading = QuaternionStamped()
+                current_heading.header.stamp = current_time
+                current_heading.header.frame_id = frame_id
+                heading_data=data['heading']+self.rotation_data
+                if(heading_data>360):
+                    heading_data=heading_data-360
+                #heading_data=data['heading']
+                yaw=math.radians(heading_data) 
+                q = quaternion_from_euler(0, 0, yaw)
+                current_heading.quaternion.x = q[0]
+                current_heading.quaternion.y = q[1]
+                current_heading.quaternion.z = q[2]
+                current_heading.quaternion.w = q[3]
+                self.heading_pub.publish(current_heading)
+
+                pose_msg = PoseStamped()
+                pose_msg.header.stamp = current_time
+                pose_msg.header.frame_id = frame_id
+                pose_msg.pose.position.x = current_fix.latitude
+                pose_msg.pose.position.y = current_fix.longitude
+                pose_msg.pose.position.z = current_fix.altitude
+                pose_msg.pose.orientation.x = q[0]
+                pose_msg.pose.orientation.y = q[1]
+                pose_msg.pose.orientation.z = q[2]
+                pose_msg.pose.orientation.w = q[3]
+                self.pose_pub.publish(pose_msg)
+                
+                heading_deg_msg = Float32() 
+                heading_deg_msg.data = heading_data 
+                self.heading_deg_pub.publish(heading_deg_msg) 
+        else:
+            return False
+        return True
+
+    """Helper method for getting the frame_id with the correct TF prefix"""
+    def get_frame_id(self):
+        frame_id = self.declare_parameter('frame_id', 'navsat_link').value
+        prefix = self.declare_parameter('tf_prefix', '').value
+        if len(prefix):
+            return '%s/%s' % (prefix, frame_id)
+        return frame_id
@@ -0,0 +1,101 @@
+import math
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+
+EARTH_RADIUS = 6378.137
+
+class GpsPath(Node):
+    def __init__(self):
+        super().__init__('GpsPath')
+        self.pose_init = False
+        self.state_pub_ = self.create_publisher(Path, 'gps_path', 10)
+        self.subscription_ = self.create_subscription(
+            NavSatFix,
+            '/gps/fix',
+            self.gps_callback,
+            1
+        )
+
+    def rad(self, d):
+        return d * math.pi / 180.0
+
+    def gps_callback(self, gps_msg):
+        if not self.pose_init:
+            self.init_pose = self.lla_pose()
+            self.init_pose.latitude = gps_msg.latitude
+            self.init_pose.longitude = gps_msg.longitude
+            self.init_pose.altitude = gps_msg.altitude
+            self.pose_init = True
+        else:
+            radLat1 = self.rad(self.init_pose.latitude)
+            radLong1 = self.rad(self.init_pose.longitude)
+            radLat2 = self.rad(gps_msg.latitude)
+            radLong2 = self.rad(gps_msg.longitude)
+            
+            delta_lat = radLat2 - radLat1
+            delta_long = 0
+            
+            if delta_lat > 0:
+                x = -2 * math.asin(math.sqrt(
+                    math.pow(math.sin(delta_lat / 2), 2) +
+                    math.cos(radLat1) * math.cos(radLat2) *
+                    math.pow(math.sin(delta_long / 2), 2)
+                ))
+            else:
+                x = 2 * math.asin(math.sqrt(
+                    math.pow(math.sin(delta_lat / 2), 2) +
+                    math.cos(radLat1) * math.cos(radLat2) *
+                    math.pow(math.sin(delta_long / 2), 2)
+                ))
+            x *= EARTH_RADIUS * 1000
+            
+            delta_lat = 0
+            delta_long = radLong2 - radLong1
+            if delta_long > 0:
+                y = 2 * math.asin(math.sqrt(
+                    math.pow(math.sin(delta_lat / 2), 2) +
+                    math.cos(radLat2) * math.cos(radLat2) *
+                    math.pow(math.sin(delta_long / 2), 2)
+                ))
+            else:
+                y = -2 * math.asin(math.sqrt(
+                    math.pow(math.sin(delta_lat / 2), 2) +
+                    math.cos(radLat2) * math.cos(radLat2) *
+                    math.pow(math.sin(delta_long / 2), 2)
+                ))
+            y *= EARTH_RADIUS * 1000
+            
+            z = gps_msg.altitude - self.init_pose.altitude
+            
+            ros_path_ = Path()
+            ros_path_.header.frame_id = 'path'
+            ros_path_.header.stamp = self.get_clock().now().to_msg()
+            pose = PoseStamped()
+            pose.header = ros_path_.header
+            pose.pose.position.x = x
+            pose.pose.position.y = y
+            pose.pose.position.z = z
+            ros_path_.poses.append(pose)
+            
+            self.state_pub_.publish(ros_path_)
+            
+            self.get_logger().info("( x:%0.6f ,y:%0.6f ,z:%0.6f)", x, y, z)
+    
+    class lla_pose:
+        def __init__(self):
+            self.latitude = 0.0
+            self.longitude = 0.0
+            self.altitude = 0.0
+
+def main(args=None):
+    rclpy.init(args=args)
+    gps_path = GpsPath()
+
+    rclpy.spin(gps_path)
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,68 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Eric Perko
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the names of the authors nor the names of their
+#    affiliated organizations may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import serial
+
+import rclpy
+
+from libnmea_navsat_driver.driver import Ros2NMEADriver
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    driver = Ros2NMEADriver()
+    frame_id = driver.get_frame_id()
+
+    serial_port = driver.declare_parameter('port', '/dev/ttyACM0').value
+    serial_baud = driver.declare_parameter('baud', 115200).value
+
+    try:
+        GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
+        driver.get_logger().info("Successfully connected to {0} at {1}.".format(serial_port, serial_baud))
+        try:
+            while rclpy.ok():
+                data = GPS.readline().strip()
+                sentence_stamp = driver.get_clock().now().to_msg()
+                try:
+                    if isinstance(data, bytes):
+                        data = data.decode("utf-8")
+                    driver.add_sentence(data, frame_id, timestamp=sentence_stamp)
+                except ValueError as e:
+                    driver.get_logger().warn(
+                        "Value error, likely due to missing fields in the NMEA message. message was: %s. " % e)
+
+        except Exception as e:
+            driver.get_logger().error("Ros error: {0}".format(e))
+            GPS.close()  # Close GPS serial port
+    except serial.SerialException as ex:
+        driver.get_logger().fatal("Could not open serial port: I/O error({0}): {1}".format(ex.errno, ex.strerror))
@@ -0,0 +1,99 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2016, Rein Appeldoorn
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the names of the authors nor the names of their
+#    affiliated organizations may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import socket
+import sys
+
+import rclpy
+
+from libnmea_navsat_driver.driver import Ros2NMEADriver
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    driver = Ros2NMEADriver()
+
+    try:
+        local_ip = driver.declare_parameter('ip', '0.0.0.0').value
+        local_port = driver.declare_parameter('port', 10110).value
+        buffer_size = driver.declare_parameter('buffer_size', 4096).value
+        timeout = driver.declare_parameter('timeout_sec', 2).value
+    except KeyError as e:
+        driver.get_logger().err("Parameter %s not found" % e)
+        sys.exit(1)
+
+    frame_id = driver.get_frame_id()
+
+    driver.get_logger().info(
+        " Using parameters ip {} port {} buffer_size {} timeout_sec {}"
+        .format(local_ip, local_port, buffer_size, timeout))
+
+    # Connection-loop: connect and keep receiving. If receiving fails, reconnect
+    while rclpy.ok():
+        try:
+            # Create a socket
+            socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+            # Bind the socket to the port
+            socket_.bind((local_ip, local_port))
+
+            # Set timeout
+            socket_.settimeout(timeout)
+        except socket.error as exc:
+            driver.get_logger().error("Caught exception socket.error when setting up socket: %s" % exc)
+            sys.exit(1)
+
+        # recv-loop: When we're connected, keep receiving stuff until that fails
+        while rclpy.ok():
+            try:
+                data, remote_address = socket_.recvfrom(buffer_size)
+
+                # strip the data
+                data_list = data.decode("ascii").strip().split("\n")
+
+                for data in data_list:
+
+                    try:
+                        driver.add_sentence(data, frame_id)
+                    except ValueError as e:
+                        driver.get_logger().warn(
+                            "Value error, likely due to missing fields in the NMEA message. "
+                            "Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, "
+                            "including a bag file with the NMEA sentences that caused it." % e)
+
+            except socket.error as exc:
+                driver.get_logger().error("Caught exception socket.error during recvfrom: %s" % exc)
+                socket_.close()
+                # This will break out of the recv-loop so we start another iteration of the connection-loop
+                break
+
+        socket_.close()  # Close socket
@@ -0,0 +1,83 @@
+# Copyright 2022 Open Source Robotics Foundation, Inc.
+#
+# 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 socket
+import sys
+
+import rclpy
+
+from libnmea_navsat_driver.driver import Ros2NMEADriver
+
+def main(args=None):
+    rclpy.init(args=args)
+    driver = Ros2NMEADriver()
+
+    try:
+        gnss_ip = driver.declare_parameter('ip', '192.168.131.22').value
+        gnss_port = driver.declare_parameter('port', 9001).value
+        buffer_size = driver.declare_parameter('buffer_size', 4096).value
+    except KeyError as e:
+        driver.get_logger().err("Parameter %s not found" % e)
+        sys.exit(1)
+
+    frame_id = driver.get_frame_id()
+
+    driver.get_logger().info("Using gnss sensor with ip {} and port {}".format(gnss_ip, gnss_port))
+
+    # Connection-loop: connect and keep receiving. If receiving fails, reconnect
+    # Connect to the gnss sensor using tcp
+    while rclpy.ok():
+        try:
+            # Create a socket
+            gnss_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+            # Connect to the gnss sensor
+            gnss_socket.connect((gnss_ip, gnss_port))
+        except socket.error as exc:
+            driver.get_logger().error("Caught exception socket.error when setting up socket: %s" % exc)
+            sys.exit(1)
+
+        # recv-loop: When we're connected, keep receiving stuff until that fails
+        partial = ""
+        while rclpy.ok():
+            try:
+                partial += gnss_socket.recv(buffer_size).decode("ascii")
+
+                # strip the data
+                lines = partial.splitlines()
+                if partial.endswith('\n'):
+                    full_lines = lines
+                    partial = ""
+                else:
+                    full_lines = lines[:-1]
+                    partial = lines[-1]
+
+                for data in full_lines:
+                    try:
+                        if driver.add_sentence(data, frame_id):
+                            driver.get_logger().info("Received sentence: %s" % data)
+                        else:
+                            driver.get_logger().warn("Error with sentence: %s" % data)
+                    except ValueError as e:
+                        driver.get_logger().warn(
+                            "Value error, likely due to missing fields in the NMEA message. "
+                            "Error was: %s. Please report this issue to me. " % e)
+
+            except socket.error as exc:
+                driver.get_logger().error("Caught exception socket.error when receiving: %s" % exc)
+                gnss_socket.close()
+                break
+
+
+        gnss_socket.close()
@@ -0,0 +1,64 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Eric Perko
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the names of the authors nor the names of their
+#    affiliated organizations may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+from functools import partial
+
+from nmea_msgs.msg import Sentence
+import rclpy
+
+from libnmea_navsat_driver.driver import Ros2NMEADriver
+
+
+def nmea_sentence_callback(nmea_sentence, driver):
+    try:
+        driver.add_sentence(nmea_sentence.sentence, frame_id=nmea_sentence.header.frame_id,
+                            timestamp=nmea_sentence.header.stamp)
+    except ValueError as e:
+        rclpy.get_logger().warn(
+            "Value error, likely due to missing fields in the NMEA message. Error was: %s. "
+            "Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with "
+            "the NMEA sentences that caused it." % e)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    driver = Ros2NMEADriver()
+    driver.get_frame_id()
+
+    driver.create_subscription(
+        Sentence, 'nmea_sentence', partial(nmea_sentence_callback, driver=driver), 10)
+
+    rclpy.spin(driver)
+
+    rclpy.shutdown()
@@ -0,0 +1,70 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Eric Perko
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the names of the authors nor the names of their
+#    affiliated organizations may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import serial
+
+from nmea_msgs.msg import Sentence
+import rclpy
+
+from libnmea_navsat_driver.driver import Ros2NMEADriver
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    driver = Ros2NMEADriver()
+
+    nmea_pub = driver.create_publisher(Sentence, "nmea_sentence", 10)
+
+    serial_port = driver.declare_parameter('port', '/dev/ttyACM0').value
+    serial_baud = driver.declare_parameter('baud', 9600).value
+
+    # Get the frame_id
+    frame_id = driver.get_frame_id()
+
+    try:
+        GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
+        try:
+            while rclpy.ok():
+                data = GPS.readline().strip()
+
+                sentence = Sentence()
+                sentence.header.stamp = driver.get_clock().now().to_msg()
+                sentence.header.frame_id = frame_id
+                sentence.sentence = data
+                nmea_pub.publish(sentence)
+
+        except Exception as e:
+            driver.get_logger().error("Ros error: {0}".format(e))
+            GPS.close()  # Close GPS serial port
+    except serial.SerialException as ex:
+        driver.get_logger().fatal("Could not open serial port: I/O error({0}): {1}".format(ex.errno, ex.strerror))
@@ -0,0 +1,169 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Eric Perko
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the names of the authors nor the names of their
+#    affiliated organizations may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import re
+import time
+import calendar
+import math
+import rclpy
+
+logger = rclpy.logging.get_logger('nmea_navsat_driver')
+
+
+def safe_float(field):
+    try:
+        return float(field)
+    except ValueError:
+        return float('NaN')
+
+
+def safe_int(field):
+    try:
+        return int(field)
+    except ValueError:
+        return 0
+
+
+def convert_latitude(field):
+    return safe_float(field[0:2]) + safe_float(field[2:]) / 60.0
+
+
+def convert_longitude(field):
+    return safe_float(field[0:3]) + safe_float(field[3:]) / 60.0
+
+
+def convert_time(nmea_utc):
+    # Get current time in UTC for date information
+    utc_struct = time.gmtime()  # immutable, so cannot modify this one
+    utc_list = list(utc_struct)
+    # If one of the time fields is empty, return NaN seconds
+    if not nmea_utc[0:2] or not nmea_utc[2:4] or not nmea_utc[4:6]:
+        return float('NaN')
+    else:
+        hours = int(nmea_utc[0:2])
+        minutes = int(nmea_utc[2:4])
+        seconds = int(nmea_utc[4:6])
+        utc_list[3] = hours
+        utc_list[4] = minutes
+        utc_list[5] = seconds
+        unix_time = calendar.timegm(tuple(utc_list))
+        return unix_time
+
+
+def convert_status_flag(status_flag):
+    if status_flag == "A":
+        return True
+    elif status_flag == "V":
+        return False
+    else:
+        return False
+
+
+def convert_knots_to_mps(knots):
+    return safe_float(knots) * 0.514444444444
+
+
+# Need this wrapper because math.radians doesn't auto convert inputs
+def convert_deg_to_rads(degs):
+    return math.radians(safe_float(degs))
+
+
+"""Format for this dictionary is a sentence identifier (e.g. "GGA") as the key, with a
+list of tuples where each tuple is a field name, conversion function and index
+into the split sentence"""
+parse_maps = {
+    "GGA": [
+        ("fix_type", int, 6),
+        ("latitude", convert_latitude, 2),
+        ("latitude_direction", str, 3),
+        ("longitude", convert_longitude, 4),
+        ("longitude_direction", str, 5),
+        ("altitude", safe_float, 9),
+        ("mean_sea_level", safe_float, 11),
+        ("hdop", safe_float, 8),
+        ("num_satellites", safe_int, 7),
+        ("utc_time", convert_time, 1),
+    ],
+    "RMC": [
+        ("utc_time", convert_time, 1),
+        ("fix_valid", convert_status_flag, 2),
+        ("latitude", convert_latitude, 3),
+        ("latitude_direction", str, 4),
+        ("longitude", convert_longitude, 5),
+        ("longitude_direction", str, 6),
+        ("speed", convert_knots_to_mps, 7),
+        ("true_course", convert_deg_to_rads, 8),
+    ],
+    "GST": [
+        ("utc_time", convert_time, 1),
+        ("ranges_std_dev", safe_float, 2),
+        ("semi_major_ellipse_std_dev", safe_float, 3),
+        ("semi_minor_ellipse_std_dev", safe_float, 4),
+        ("semi_major_orientation", safe_float, 5),
+        ("lat_std_dev", safe_float, 6),
+        ("lon_std_dev", safe_float, 7),
+        ("alt_std_dev", safe_float, 8),
+    ],
+    "HDT": [
+        ("heading", safe_float, 1),
+    ],
+    "VTG": [
+        ("true_course", convert_deg_to_rads, 1),
+        ("speed", convert_knots_to_mps, 5)
+    ]
+}
+
+
+def parse_nmea_sentence(nmea_sentence):
+    # Check for a valid nmea sentence
+
+    if not re.match(r'(^\$GP|^\$GN|^\$GL|^\$IN).*\*[0-9A-Fa-f]{2}$', nmea_sentence):
+        logger.debug("Regex didn't match, sentence not valid NMEA? Sentence was: %s"
+                     % repr(nmea_sentence))
+        return False
+    fields = [field.strip(',') for field in nmea_sentence.split(',')]
+
+    # Ignore the $ and talker ID portions (e.g. GP)
+    sentence_type = fields[0][3:]
+
+    if sentence_type not in parse_maps:
+        logger.debug("Sentence type %s not in parse map, ignoring."
+                     % repr(sentence_type))
+        return False
+
+    parse_map = parse_maps[sentence_type]
+
+    parsed_sentence = {}
+    for entry in parse_map:
+        parsed_sentence[entry[0]] = entry[1](fields[entry[2]])
+
+    return {sentence_type: parsed_sentence}
@@ -0,0 +1,63 @@
+?Á?®?ç?Æ?ç???¥ÔòØ???×???ª?¥?©?®?ò?áÔöé??ÔòØ?£
+
+?¥ÔòØ???×???ª?ò?¿ãÆ?×?ó?ó?ò?«?á?Ü?ú?ç?×?¬???¥?£?ñ?Á?Ñ?ÿ?×?ü????ÔòØ?£
+sudo pip3 install transforms3d
+sudo apt install ros-humble-tf-transformations
+sudo apt install ros-humble-gps-umd
+pip3 install serial
+pip3 install pyserial
+pip install um982-driver
+
+?Á?¬???Á?®?ç?Ü?Â?½?ò?½?½?×???á?Á?Ñ?ÿ?×?ü????ÔòØ?£
+rosdep install --from-paths src --ignore-src -r -y
+?Á?¬??
+rosdepc install --from-paths src --ignore-src -r -y
+
+---------------------------------------------------------------------------------------------
+
+?ÁÔòæ?«?Æ?ç???×ÔöÉ?ë?×?¡?«?¥?©?®?ò?áÔöé??ÔòØ?£
+
+?×ÔöÉ?ë?×?¡?«?×?½?Ñ?ò???º?ò?¬?¢?ò?ë?¼?×???ñ?ò?¬?û?Á???º?ÁÔòù?é:
+sudo sh wheeltec_gnss.sh
+
+
+?Á?óÔöÉ?¥?Â??NMEA?ò?¼?à?×?½?½?×?????Á?ù?ëWHEELTEC G60/G70??ÔòØ?£
+ros2 launch wheeltec_gps_driver wheeltec_nmea_driver.launch.py
+
+?Á?óÔöÉ?¥?Â??NMEA?ò?¼?à?×?½?½?×?????Á?ù?ëWHEELTEC G90??ÔòØ?£
+ros2 launch wheeltec_gps_driver wheeltec_dual_rtk_driver_nmea.launch.py
+
+--gps topic: /gps/fix
+--gpgga topic: /gnss/gpgga
+--heading topic: /heading
+--gps frame_id: navsat_link
+
+?ò?ú??RVIZ?Á?®?í?×?½Ôûæ?ò?ó???¥Ôòù?à?¥Ôòæ???òÔòæ?¬?×?ó???×ÔöÉÔòú(WHEELTEC G60/G70)??ÔòØ?£
+ros2 launch wheeltec_gps_driver nmea_gps_path.launch.py
+
+?ò?ú??RVIZ?Á?®?í?×?½Ôûæ?ò?ó???¥Ôòù?à?¥Ôòæ???òÔòæ?¬?×?ó???×ÔöÉÔòú(WHEELTEC 90)??ÔòØ?£
+ros2 launch wheeltec_gps_driver nmea_gps_path_dualrtk.launch.py
+
+---------------------------------------------------------------------------------------------
+
+?Á?óÔöÉ?¥?Â??UBLOX?ò?¼?à?×?½?½?×?????Á?ù?ëWHEELTEC G70?Á???¡?ò?ÿ??(?ÁÔòù?áWHEELTEC G70)??ÔòØ?£
+ros2 launch wheeltec_gps_driver wheeltec_ublox_driver.launch.py
+--gps topic: /ublox_gps_node/fix
+--gps frame_id: navsat_link
+
+?Á?óÔöÉ?¥?Â??UBLOX?ò?¼?à?×?½?½?ò?ú??RVIZ?Á?®?í?×?½Ôûæ?ò?ó???¥Ôòù?à?¥Ôòæ???òÔòæ?¬?×?ó???×ÔöÉÔòú(?ÁÔòù?áWHEELTEC G70)??ÔòØ?£
+ros2 launch wheeltec_gps_driver ublox_gps_path.launch.py
+
+---------------------------------------------------------------------------------------------
+
+?Á?óÔöÉ?¥?Â??Unicore?ò?¼?à?×?½?½?×?????Á?ù?ëWHEELTEC G90?Á???¡?ò?ÿ??(?ÁÔòù?áWHEELTEC G90)??ÔòØ?£
+ros2 launch wheeltec_gps_driver wheeltec_dual_rtk_driver_unicore.launch.py
+
+?Á?óÔöÉ?¥?Â??Unicore?ò?¼?à?×?½?½?ò?ú??RVIZ?Á?®?í?×?½Ôûæ?ò?ó???¥Ôòù?à?¥Ôòæ???òÔòæ?¬?×?ó???×ÔöÉÔòú(?ÁÔòù?áWHEELTEC G90)??ÔòØ?£
+ros2 launch wheeltec_gps_driver wheeltec_dual_rtk_driver_unicore.launch.py
+
+--gps topic: /gps/fix
+--gps frame_id: navsat_link
+--utm pose topic:/gps/utm_pose
+--heading topic:/gps/euler
+
@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    return LaunchDescription([
+         Node(
+            package='wheeltec_dual_rtk_driver',
+            executable='dual_rtk_driver_node',
+            name='dual_rtk_driver_node',
+            parameters=[
+                {'port': '/dev/wheeltec_gnss'},
+                {'baud': 115200},
+                {'gps_frame_id': 'navsat_link'},
+                ],
+            output='screen',
+            ),
+        ])
+
@@ -0,0 +1,20 @@
+<?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>wheeltec_dual_rtk_driver</name>
+  <version>0.0.0</version>
+  <maintainer email="powrbv@gmail.com">root</maintainer>
+  <description>TODO: Package description</description>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/wheeltec_dual_rtk_driver
+[install]
+install_scripts=$base/lib/wheeltec_dual_rtk_driver
@@ -0,0 +1,28 @@
+from setuptools import find_packages, setup
+
+package_name = 'wheeltec_dual_rtk_driver'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['launch/wheeltec_dual_rtk_driver_unicore.launch.py']),
+        ('share/' + package_name, ['package.xml']),
+        
+    ],
+    install_requires=['setuptools','launch'],
+    zip_safe=True,
+    maintainer='wheeltec',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            "dual_rtk_driver_node = wheeltec_dual_rtk_driver.wheeltec_dual_rtk_driver:main",
+            "serial_demo_node     = wheeltec_dual_rtk_driver.serial_demo:main",
+        ],
+    },
+)
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'
@@ -0,0 +1,34 @@
+# serial_demo.py
+# coding=utf-8
+
+from wheeltec_dual_rtk_driver.um982_serial import UM982Serial
+import time
+
+
+def main():
+    port = "/dev/wheeltec_gnss"
+    baudrate = 115200
+
+    print(f"/dev/wheeltec_gnss opening ...")
+    try:
+        um = UM982Serial(port, baudrate)
+        print(f'serial {port} open successfully! data reading...')
+
+        for i in range(10):
+            um.read_frame()
+            if um.fix:
+                print(f"[{i}] fix: {um.fix}")
+            if um.orientation:
+                print(f"[{i}] pose: {um.orientation}")
+            if um.vel:
+                print(f"[{i}] vel: {um.vel}")
+            time.sleep(0.5)
+
+        um.stop()
+        print("test stop,closed")
+
+    except Exception as e:
+        print(f"serial {port} do not open!: {e}")
+        
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,246 @@
+# coding=utf-8
+from pyproj import CRS, Transformer
+import threading
+import serial
+import time
+import math
+
+
+
+def crc_table():
+    table = []
+    for i in range(256):
+        crc = i
+        for j in range(8, 0, -1):
+            if crc & 1:
+                crc = (crc >> 1) ^ 0xEDB88320
+            else:
+                crc >>= 1
+        table.append(crc)
+    return table
+
+NMEA_EXPEND_CRC_TABLE = crc_table()
+
+def open_serial_with_retry(port, baudrate, retry=5, delay=1):
+    for i in range(retry):
+        try:
+            ser = serial.Serial(port, baudrate, timeout=1)
+            #print(f"串口{port}打开成功")
+            return ser
+        except serial.SerialException as e:
+            #print(f"[尝试{i+1}] 串口 {port} 打开失败: {e}")
+            time.sleep(delay)
+    raise serial.SerialException(f"串口 {port} 在 {retry} 次尝试后仍无法打开")
+    
+def nmea_expend_crc(nmea_expend_sentence):
+    def calculate_crc32(data):
+        crc = 0
+        for byte in data:
+            crc = NMEA_EXPEND_CRC_TABLE[(crc ^ byte) & 0xFF] ^ (crc >> 8)
+        return crc & 0xFFFFFFFF
+
+    try:
+        sentence, crc = nmea_expend_sentence[1:].split("*")
+        crc = crc[:8]
+    except:
+        return False
+    calculated_crc = calculate_crc32(sentence.encode())
+    return crc.lower() == format(calculated_crc, '08x')
+
+def nmea_crc(nmea_sentence):
+    # 移除起始的'$'和'*'及之后的校验和部分
+    try:
+        sentence, crc = nmea_sentence[1:].split("*")
+        crc = crc[:2]
+    except:
+        return False
+    calculated_checksum = 0
+    # 对字符串中的每个字符进行异或运算
+    for char in sentence:
+        calculated_checksum ^= ord(char)
+    # 将计算得到的校验和转换为十六进制格式，并大写
+    calculated_checksum_hex = format(calculated_checksum, 'X')
+    # 校验和比较
+    return calculated_checksum_hex.zfill(2) == crc.upper()
+
+def msg_seperate(msg:str):
+    return msg[1:msg.find('*')].split(',')
+
+def PVTSLN_solver(msg:str):
+    """解析PVTSLN消息，提取GPS时间和位置信息
+
+    PVTSLN消息格式：
+    #PVTSLNA,<len>,<time_system>,<sol_status>,<gps_week>,<gps_ms>,...;<pos_type>,<height>,<lat>,<lon>,...
+    """
+    parts = msg_seperate(msg)
+
+    # 消息头部分（字段0-9，注意parts[9]可能包含分号）
+    # parts[0] = PVTSLNA
+    # parts[1] = 消息长度
+    # parts[2] = 时间系统 (GPS/BDST/GLOST/GALST)
+    # parts[3] = 解状态 (FINE/...)
+    # parts[4] = GPS周
+    # parts[5] = GPS周内秒（毫秒）
+    gps_week = int(parts[4])
+    gps_seconds = float(parts[5]) / 1000.0  # 毫秒转秒
+    gps_time = (gps_week, gps_seconds)
+
+    # 处理可能包含分号的字段（如 "13;SINGLE"）
+    if ';' in parts[9]:
+        pos_type = parts[9].split(';')[1]  # 提取分号后的位置类型
+    else:
+        pos_type = parts[10] if len(parts) > 10 else ""
+
+    # 位置数据段（从字段10开始，如果parts[9]包含分号则从parts[10]开始）
+    # 数据段1：主天线位置
+    base_idx = 10 if ';' in parts[9] else 11
+    bestpos_hgt    = float(parts[base_idx])       # 海拔高
+    bestpos_lat    = float(parts[base_idx + 1])   # 纬度
+    bestpos_lon    = float(parts[base_idx + 2])   # 经度
+    bestpos_hgtstd = float(parts[base_idx + 3])   # 高程标准差
+    bestpos_latstd = float(parts[base_idx + 4])   # 纬度标准差
+    bestpos_lonstd = float(parts[base_idx + 5])   # 经度标准差
+
+    fix = (bestpos_hgt, bestpos_lat, bestpos_lon, bestpos_hgtstd, bestpos_latstd, bestpos_lonstd)
+
+    # 额外信息（如果有）
+    num_sats = int(parts[8]) if len(parts) > 8 else 0  # 卫星数
+
+    return fix, gps_time, pos_type, num_sats
+
+
+def GNHPR_solver(msg:str):
+    parts = msg_seperate(msg)
+    heading = float(parts[3-1])
+    pitch   = float(parts[4-1])
+    roll    = float(parts[5-1])
+    orientation = (heading, pitch, roll)
+    return orientation
+
+
+def BESTNAV_solver(msg:str):
+    parts = msg_seperate(msg)
+    vel_hor_std = float(parts[-1])  # 水平速度标准差，单位 m/s
+    vel_ver_std = float(parts[-2])  # 高程速度标准差，单位 m/s
+    vel_ver     = float(parts[-3])  # 垂直速度， m/s，正值表示高度增加（向上），负值表示高度下降（向下）
+    vel_heading = float(parts[-4])  # 相对于真北的实际对地运动方向（相对地面轨迹）， deg
+    vel_hor     = float(parts[-5])  # 对地水平速度， m/s
+    vel_north   = vel_hor * math.cos(math.radians(vel_heading))     # 分解得到北方向速度
+    vel_east    = vel_hor * math.sin(math.radians(vel_heading))     # 分解得到东方向速度
+    return (vel_east, vel_north, vel_ver, vel_hor_std, vel_hor_std, vel_ver_std)
+
+
+
+def create_utm_trans(lat, lon):
+    """构建转换器，用于将WGS84地理坐标系下的点转换为UTM坐标系下的点。
+
+    Args:
+        lon (float): 点的经度。
+        lat (float): 点的纬度。
+
+    Returns:
+        transformer: 转换器
+    """
+    # UTM区号是根据经度确定的，从-180度开始每6度一个区间。
+    zone_number            = int((lon + 180) / 6) + 1
+    # 北半球是赤道（纬度0度）以上的区域。
+    isnorth                = lat >= 0
+    # 定义WGS84坐标系
+    wgs84_crs              = CRS("epsg:4326")
+    # 根据是否位于北半球，选择合适的UTM EPSG代码
+    utm_crs_str            = f"epsg:326{zone_number}" if isnorth else f"epsg:327{zone_number}"
+    utm_crs                = CRS(utm_crs_str)
+    # 创建坐标转换器，从WGS84转换到UTM
+    transformer            = Transformer.from_crs(wgs84_crs, utm_crs, always_xy=True)
+    return transformer
+
+
+def utm_trans(transformer, lon, lat):
+    """将WGS84地理坐标系下的点转换为UTM坐标系下的点。
+
+    Args:
+        transformer: 转换器
+        lon (float): 点的经度。
+        lat (float): 点的纬度。
+
+    Returns:
+        tuple: 一个元组，包含转换后的UTM坐标系下的x（东坐标）和y（北坐标）。
+    """
+    # 进行坐标转换
+    utm_x, utm_y           = transformer.transform(lon, lat)
+    return (utm_x, utm_y)
+
+
+class UM982Serial(threading.Thread):
+    def __init__(self, port, band):
+        super().__init__()
+        # 打开串口
+        #self.ser            = serial.Serial(port, band)
+        self.ser = open_serial_with_retry(port, band)
+
+        # 设置运行标志位
+        self.isRUN          = True
+        # 数据
+        self.fix            = None   # sensor_msgs/NavSatFix 所需要的数据
+        self.orientation    = None   # 航向角
+        self.vel            = None   # 速度
+        self.utmpos         = None
+        self.gps_time       = None   # GPS时间 (week, seconds)
+        self.pos_type       = ""     # 定位类型
+        self.num_sats       = 0      # 卫星数
+        # 读初始数据
+        for i in range(30):
+            self.read_frame()
+        print(self.fix)
+        #if self.fix is None:
+        #    raise RuntimeError("未能从串口接收到有效的 PVTSLN 定位数据，串口初始化失败")
+
+        # wgs84转utm
+        bestpos_hgt, bestpos_lat, bestpos_lon, bestpos_hgtstd, bestpos_latstd, bestpos_lonstd = self.fix
+        self.transformer = create_utm_trans(bestpos_lat, bestpos_lon)
+        self.utmpos      = utm_trans(self.transformer, bestpos_lon, bestpos_lat)
+
+
+    def stop(self):
+        """ 结束运行 """
+        self.isRUN = False
+        time.sleep(0.1)
+        self.ser.close()
+
+
+#    def read_frame(self):
+#        frame = self.ser.readline().decode('utf-8')
+#        if frame.startswith("#PVTSLNA") and nmea_expend_crc(frame):
+#            self.fix = PVTSLN_solver(frame)
+#        elif frame.startswith("$GNHPR") and nmea_crc(frame):
+#            self.orientation = GNHPR_solver(frame)
+#        elif frame.startswith("#BESTNAVA") and nmea_expend_crc(frame):
+#            self.vel = BESTNAV_solver(frame)
+
+    def read_frame(self):
+        try:
+            raw = self.ser.readline()
+            frame = raw.decode('utf-8', errors='ignore').strip()
+            if frame.startswith("#PVTSLNA") and nmea_expend_crc(frame):
+                self.fix, self.gps_time, self.pos_type, self.num_sats = PVTSLN_solver(frame)
+            elif frame.startswith("$GNHPR") and nmea_crc(frame):
+                self.orientation = GNHPR_solver(frame)
+            elif frame.startswith("#BESTNAVA") and nmea_expend_crc(frame):
+                self.vel = BESTNAV_solver(frame)
+        except Exception as e:
+            print(f"Error: {e}")
+
+
+    def run(self):
+        while self.isRUN:
+            self.read_frame()
+            bestpos_hgt, bestpos_lat, bestpos_lon, bestpos_hgtstd, bestpos_latstd, bestpos_lonstd = self.fix
+            self.utmpos = utm_trans(self.transformer, bestpos_lon, bestpos_lat)
+
+
+
+
+if __name__ == "__main__":
+    um982 = UM982Serial("/dev/wheeltec_gnss", 115200)
+    um982.start()
+
@@ -0,0 +1,178 @@
+# coding=utf-8
+import sys
+import math
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import NavSatFix, TimeReference
+from nav_msgs.msg import Odometry
+from tf_transformations import quaternion_from_euler, euler_from_quaternion
+
+# GPS纪元偏移：从1980-01-06到1970-01-01的秒数，减去18秒闰秒
+GPS_EPOCH_OFFSET = 315964800 - 18
+
+
+from wheeltec_dual_rtk_driver.um982_serial import UM982Serial
+from geometry_msgs.msg import Vector3Stamped
+
+
+class wheeltec_dual_rtk_driver(Node):
+    def _ros_log_debug(self, log_data):
+        self.get_logger().debug(str(log_data))
+
+    def _ros_log_info(self, log_data):
+        self.get_logger().info(str(log_data))
+
+    def _ros_log_warn(self, log_data):
+        self.get_logger().warn(str(log_data))
+
+    def _ros_log_error(self, log_data):
+        self.get_logger().error(str(log_data))
+
+
+    def __init__(self) -> None:
+        super().__init__('um982_serial_driver')
+        global gps_frame_id
+
+        # Step1：从参数服务器获取所有参数
+        self.declare_parameter('port', '/dev/wheeltec_gnss')
+        self.declare_parameter('baud', 115200)
+        self.declare_parameter('gps_frame_id', 'navsat_link')
+        self.declare_parameter('publish_rate', 20.0)
+        self.declare_parameter('fix_topic', '/gps/fix')
+        self.declare_parameter('utm_pose_topic', '/gps/utm_pose')
+        self.declare_parameter('euler_topic', '/gps/euler')
+        self.declare_parameter('time_reference_topic', '/gps/time_reference')
+
+        port = self.get_parameter('port').get_parameter_value().string_value
+        baud = self.get_parameter('baud').get_parameter_value().integer_value
+        gps_frame_id = self.get_parameter('gps_frame_id').get_parameter_value().string_value
+        publish_rate = self.get_parameter('publish_rate').get_parameter_value().double_value
+        fix_topic = self.get_parameter('fix_topic').get_parameter_value().string_value
+        utm_pose_topic = self.get_parameter('utm_pose_topic').get_parameter_value().string_value
+        euler_topic = self.get_parameter('euler_topic').get_parameter_value().string_value
+        time_reference_topic = self.get_parameter('time_reference_topic').get_parameter_value().string_value
+
+        # Step2：打开串口
+        try:
+            self.um982serial = UM982Serial(port, baud)
+            self._ros_log_info(f'serial {port} open successfully!')
+        except:
+            self._ros_log_error(f'serial {port} do not open!')
+            sys.exit(0)
+        # Step3：新建一个线程用于处理串口数据
+        self.um982serial.start()
+        # Step4：ROS相关
+        self.fix_pub        = self.create_publisher(NavSatFix, fix_topic, 10)
+        self.utm_pub        = self.create_publisher(Odometry,  utm_pose_topic, 10)
+        self.euler_pub      = self.create_publisher(Vector3Stamped, euler_topic, 10)
+        self.time_ref_pub   = self.create_publisher(TimeReference, time_reference_topic, 10)
+
+        self.pub_timer      = self.create_timer(1.0 / publish_rate, self.pub_task)
+        self._ros_log_info(f'GPS publish rate: {publish_rate} Hz')
+
+    def pub_task(self):
+        bestpos_hgt, bestpos_lat, bestpos_lon, bestpos_hgtstd, bestpos_latstd, bestpos_lonstd = self.um982serial.fix
+        utm_x, utm_y = self.um982serial.utmpos
+        vel_east, vel_north, vel_ver, vel_east_std, vel_north_std, vel_ver_std = self.um982serial.vel
+        heading, pitch, roll = self.um982serial.orientation
+        gps_week, gps_seconds = self.um982serial.gps_time if self.um982serial.gps_time else (0, 0.0)
+        this_time = self.get_clock().now().to_msg()
+
+        # Step 1: Publish GPS Fix Data
+        fix_msg = NavSatFix()
+        fix_msg.header.stamp = this_time
+        fix_msg.header.frame_id = gps_frame_id
+        fix_msg.latitude = bestpos_lat
+        fix_msg.longitude = bestpos_lon
+        fix_msg.altitude = bestpos_hgt
+        fix_msg.position_covariance[0] = float(bestpos_latstd)**2
+        fix_msg.position_covariance[4] = float(bestpos_lonstd)**2
+        fix_msg.position_covariance[8] = float(bestpos_hgtstd)**2
+        fix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
+        self.fix_pub.publish(fix_msg)
+
+        euler_msg = Vector3Stamped()
+        euler_msg.header.stamp = this_time
+        euler_msg.header.frame_id = 'euler_link'
+        euler_msg.vector.x = roll     # x轴表示roll（横滚）?math.radians(roll)math.radians(pitch)math.radians(heading)
+        euler_msg.vector.y = pitch    # y轴表示pitch（俯仰）
+        euler_msg.vector.z = heading  # z轴表示heading（偏航）
+        self.euler_pub.publish(euler_msg)
+
+        # Step 2: Publish UTM Position Data
+        odom_msg = Odometry()
+        odom_msg.header.stamp = this_time
+        odom_msg.header.frame_id = 'earth'
+        odom_msg.child_frame_id  = 'base_link'
+        odom_msg.pose.pose.position.x = utm_x
+        odom_msg.pose.pose.position.y = utm_y
+        odom_msg.pose.pose.position.z = bestpos_hgt
+        quaternion = quaternion_from_euler(math.radians(roll), math.radians(pitch), math.radians(heading))
+        odom_msg.pose.pose.orientation.x = quaternion[0]
+        odom_msg.pose.pose.orientation.y = quaternion[1]
+        odom_msg.pose.pose.orientation.z = quaternion[2]
+        odom_msg.pose.pose.orientation.w = quaternion[3]
+        odom_msg.pose.covariance         = [0.0] * 36
+        odom_msg.pose.covariance[0]      = float(bestpos_latstd)**2
+        odom_msg.pose.covariance[7]      = float(bestpos_lonstd)**2
+        odom_msg.pose.covariance[14]     = float(bestpos_hgtstd)**2
+        odom_msg.pose.covariance[21]     = 0.1
+        odom_msg.pose.covariance[28]     = 0.1
+        odom_msg.pose.covariance[35]     = 0.1
+        odom_msg.twist.twist.linear.x    = vel_east
+        odom_msg.twist.twist.linear.y    = vel_north
+        odom_msg.twist.twist.linear.z    = vel_ver
+        odom_msg.twist.covariance        = [0.0] * 36
+        odom_msg.twist.covariance[0]     = float(vel_east_std)**2
+        odom_msg.twist.covariance[7]     = float(vel_north_std)**2
+        odom_msg.twist.covariance[14]    = float(vel_ver_std)**2
+        self.utm_pub.publish(odom_msg)
+
+        # Step 3: Publish GPS Time Reference for sensor synchronization
+        time_ref_msg = TimeReference()
+        time_ref_msg.header.stamp = this_time  # ROS接收时间
+        # GPS总秒数 = GPS周 * 604800 + 周内秒 + 纪元偏移
+        gps_total_seconds = gps_week * 604800 + int(gps_seconds) + GPS_EPOCH_OFFSET
+        gps_nanoseconds = int((gps_seconds % 1.0) * 1e9)
+        time_ref_msg.time_ref.sec = gps_total_seconds
+        time_ref_msg.time_ref.nanosec = gps_nanoseconds
+        time_ref_msg.source = "gps"
+        self.time_ref_pub.publish(time_ref_msg)
+
+        # Test
+        _, _, yaw = euler_from_quaternion(quaternion)
+        #print(heading)
+
+
+    def run(self):
+        if rclpy.ok():
+            rclpy.spin(self)
+
+    def stop(self):
+        self.um982serial.stop()
+        self.pub_timer.cancel()
+
+
+
+import time
+import signal
+
+def signal_handler(sig, frame):
+    dual_rtk_driver.stop()
+    time.sleep(0.1)
+    if rclpy.ok():
+        rclpy.shutdown()
+    sys.exit(0)
+
+
+signal.signal(signal.SIGINT, signal_handler)
+rclpy.init()
+dual_rtk_driver = wheeltec_dual_rtk_driver()
+
+
+def main():
+    dual_rtk_driver.run()
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,11 @@
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_gnss"' >/etc/udev/rules.d/wheeltec_gnss.rules
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0005", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_gnss"' >/etc/udev/rules.d/wheeltec_gps.rules
+echo  'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", ATTRS{serial}=="0005", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_gnss"' >/etc/udev/rules.d/wheeltec_gnss_343.rules
+#CH9102，同时系统没有安装对应驱动 串口号0005
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0005", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_gnss"' >>/etc/udev/rules.d/wheeltec_gnss_343.rules
+
+service udev reload
+sleep 2
+service udev restart
+
+
@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.8)
+project(wheeltec_gps_driver)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include_directories(include)
+
+add_executable(gps_path src/gps_path.cpp)
+ament_target_dependencies(gps_path geometry_msgs nav2_msgs rclcpp sensor_msgs tf2_geometry_msgs)
+
+install(TARGETS
+  gps_path
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(
+  DIRECTORY  launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
@@ -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,59 @@
+# Wheeltec Dual RTK GNSS Driver Parameters
+# 双RTK GNSS驱动参数配置
+
+/**:
+  ros__parameters:
+    # ============================================
+    # 串口连接参数
+    # ============================================
+    port: "/dev/ttyACM0"          # GNSS设备串口路径
+    baud: 115200                  # 串口波特率
+
+    # ============================================
+    # 坐标系配置
+    # ============================================
+    gps_frame_id: "gnss_link"     # GPS消息的frame_id
+
+    # ============================================
+    # 发布频率设置
+    # ============================================
+    publish_rate: 20.0            # GPS数据发布频率 (Hz)
+
+    # ============================================
+    # 话题名称配置
+    # ============================================
+    fix_topic: "/sensing/gnss/fix"                         # NavSatFix GPS原始数据
+    utm_pose_topic: "/sensing/gnss/utm_pose"               # UTM坐标系下的里程计数据
+    euler_topic: "/sensing/gnss/euler"                     # 欧拉角 (roll, pitch, heading)
+    time_reference_topic: "/sensing/gnss/time_reference"   # GPS时间参考
+
+    # ============================================
+    # NMEA解析参数 (保留用于兼容性)
+    # ============================================
+    time_ref_source: "gps"        # 时间参考源
+    useRMC: False                 # 是否使用RMC消息获取位置
+
+    # ============================================
+    # 安装参数
+    # ============================================
+    # 设备安装角度旋转（度），用于校准安装方向
+    rotation: 0.0
+
+    # ============================================
+    # TF参数
+    # ============================================
+    # TF坐标前缀
+    tf_prefix: ""
+    # 是否发布TF变换
+    publish_tf: true
+
+    # ============================================
+    # 诊断参数
+    # ============================================
+    # 估计位置误差(EPE)质量等级参数
+    epe_quality0: 1000000   # 未知/无效
+    epe_quality1: 4.0       # SPS模式
+    epe_quality2: 0.1       # DGPS模式
+    epe_quality4: 0.02      # RTK固定解
+    epe_quality5: 4.0       # RTK浮点解
+    epe_quality9: 3.0       # WAAS模式
@@ -0,0 +1,32 @@
+# NMEA GNSS Driver Parameters
+# NMEA GNSS驱动参数配置
+
+nmea_navsat_driver:
+  ros__parameters:
+    # ============================================
+    # 串口连接参数
+    # ============================================
+    port: "/dev/ttyACM0"          # GNSS设备串口路径
+    baud: 9600                    # 串口波特率 (NMEA通常使用9600)
+
+    # ============================================
+    # 坐标系配置
+    # ============================================
+    frame_id: "gnss_link"       # GPS消息的frame_id
+
+    # ============================================
+    # NMEA解析参数
+    # ============================================
+    time_ref_source: "gps"        # 时间参考源
+    useRMC: False                 # 是否使用RMC消息获取位置
+
+    # ============================================
+    # 安装参数
+    # ============================================
+    # 设备安装角度旋转（度），用于校准安装方向
+    rotation: 0.0
+
+    # ============================================
+    # TF参数
+    # ============================================
+    tf_prefix: ""                 # TF坐标前缀
@@ -0,0 +1,7 @@
+nmea_navsat_driver:
+  ros__parameters:
+    port: "/dev/ttyACM0"
+    baud: 9600
+    frame_id: "navsat_link"
+    time_ref_source: "gps"
+    useRMC: False
@@ -0,0 +1,161 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 557
+  - 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: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - 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: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /gps_path
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: path
+    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: 8.548384666442871
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.8553979992866516
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 2.980397939682007
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 854
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005cb0000003efc0100000002fb0000000800540069006d00650100000000000005cb000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000046f000002b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1483
+  X: 70
+  Y: 27
@@ -0,0 +1,59 @@
+# u-blox GNSS Driver Parameters
+# u-blox GNSS驱动参数配置
+
+ublox_gps_node:
+  ros__parameters:
+    # ============================================
+    # 设备连接参数
+    # ============================================
+    device: "/dev/ttyACM0"        # u-blox设备串口路径
+    frame_id: "navsat_link"       # GPS消息的frame_id
+
+    # ============================================
+    # 串口参数
+    # ============================================
+    uart1:
+      baudrate: 115200            # 波特率
+      in: 7                       # 输入协议 (7 = RTCM3 + NMEA + UBX)
+      out: 1                      # 输出协议 (1 = UBX)
+
+    # ============================================
+    # 消息配置
+    # ============================================
+    # 导航消息发布频率 (Hz)
+    rate: 10.0
+
+    # 导航解算动态模型
+    # 可选值: portable, stationary, pedestrian, automotive, sea, airborne1g, airborne2g, airborne4g, wrist
+    dyn_model: "automotive"
+
+    # ============================================
+    # GNSS系统配置
+    # ============================================
+    gnss:
+      gps: true                   # GPS启用
+      glonass: true               # GLONASS启用
+      beidou: true                # 北斗启用
+      galileo: true               # Galileo启用
+
+    # ============================================
+    # SBAS配置
+    # ============================================
+    sbas: true                    # SBAS启用
+    # ============================================
+    # 诊断参数
+    # ============================================
+    # 是否启用详细诊断信息
+    publish:
+      nav:
+        all: true
+      rxm:
+        all: false
+      mon:
+        all: false
+      aid:
+        all: false
+      esf:
+        all: false
+      hnr:
+        all: false
@@ -0,0 +1,31 @@
+# Configuration Settings for C94-M8P device
+ublox_gps_node:
+  ros__parameters:
+    debug: 0                    # Range 0-4 (0 means no debug statements will print)
+    device: /dev/ttyACM0
+    frame_id: navsat_link
+    uart1:
+      baudrate: 9600
+    # TMODE3 Config
+    tmode3: 1                   # Survey-In Mode
+    sv_in:
+      reset: True               # True: disables and re-enables survey-in (resets)
+                                # False: Disables survey-in only if TMODE3 is
+                                # disabled
+      min_dur: 300              # Survey-In Minimum Duration [s]
+      acc_lim: 3.0              # Survey-In Accuracy Limit [m]
+      
+    gnss:
+      glonass: true            
+      beidou: true           
+      qzss: true              
+
+    inf:
+      all: true                   # Whether to display all INF messages in console
+    
+    publish:
+      all: false
+      aid:
+        hui: false
+      nav:
+        posecef: false
@@ -0,0 +1,8 @@
+nmea_navsat_driver:
+  ros__parameters:
+    port: "/dev/ttyACM0"
+    baud: 115200
+    frame_id: "navsat_link"
+    time_ref_source: "gps"
+    useRMC: False
+    rotation: 90.0    
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.substitutions import EnvironmentVariable
+import launch.actions
+import launch_ros.actions
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    bringup_dir = get_package_share_directory('wheeltec_gps_driver')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    wheeltec_gps = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wheeltec_nmea_driver.launch.py')),
+    )
+
+    return LaunchDescription([
+        wheeltec_gps,
+        launch_ros.actions.Node(
+            package='wheeltec_gps_driver', executable='gps_path', output='screen',
+            remappings=[('/gps_topic','/gps/fix')]),
+            
+    ])
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.substitutions import EnvironmentVariable
+import launch.actions
+import launch_ros.actions
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    bringup_dir = get_package_share_directory('wheeltec_gps_driver')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    wheeltec_gps = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wheeltec_dual_rtk_driver_nmea.launch.py')),
+    )
+
+    return LaunchDescription([
+        wheeltec_gps,
+        launch_ros.actions.Node(
+            package='wheeltec_gps_driver', executable='gps_path', output='screen',
+            remappings=[('/gps_topic','/gps/fix')]),
+            
+    ])
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.substitutions import EnvironmentVariable
+import launch.actions
+import launch_ros.actions
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    bringup_dir = get_package_share_directory('wheeltec_gps_driver')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    wheeltec_gps = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wheeltec_ublox_driver.launch.py')),
+    )
+
+    return LaunchDescription([
+        wheeltec_gps,
+        launch_ros.actions.Node(
+            package='wheeltec_gps_driver', executable='gps_path', output='screen',
+            remappings=[('/gps_topic','/ublox_gps_node/fix')]),
+            
+    ])
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.substitutions import EnvironmentVariable
+import launch.actions
+import launch_ros.actions
+from ament_index_python.packages import get_package_share_directory
+import os
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    bringup_dir = get_package_share_directory('wheeltec_gps_driver')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    wheeltec_gps = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wheeltec_dual_rtk_driver_unicore.launch.py')),
+    )
+
+    return LaunchDescription([
+        wheeltec_gps,
+        launch_ros.actions.Node(
+            package='wheeltec_gps_driver', executable='gps_path', output='screen',
+            remappings=[('/gps_topic','/gps/fix')]),
+            
+    ])
@@ -0,0 +1,55 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+""" A simple launch file for the nmea_serial_driver node. """
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription, LaunchIntrospector, LaunchService
+from launch_ros import actions
+
+
+def generate_launch_description():
+    """Generate a launch description for a single serial driver."""
+    config_file = os.path.join(get_package_share_directory("wheeltec_gps_driver"), "config", "wheeltec_dual_rtk_nmea_serial_driver.yaml")
+    driver_node = actions.Node(
+        package='nmea_navsat_driver',
+        executable='nmea_serial_driver',
+        output='screen',
+        parameters=[config_file])
+
+    return LaunchDescription([driver_node])
+
+
+def main(argv):
+    ld = generate_launch_description()
+
+    print('Starting introspection of launch description...')
+    print('')
+
+    print(LaunchIntrospector().format_launch_description(ld))
+
+    print('')
+    print('Starting launch of launch description...')
+    print('')
+
+    ls = LaunchService()
+    ls.include_launch_description(ld)
+    return ls.run()
+
+
+if __name__ == '__main__':
+    main(sys.argv)
@@ -0,0 +1,35 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    """Generate launch description for Wheeltec Dual RTK GNSS Driver."""
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('wheeltec_gps_driver')
+    default_param_file = os.path.join(pkg_share, 'config', 'dual_rtk_driver.param.yaml')
+
+    # 声明启动参数
+    param_file_arg = DeclareLaunchArgument(
+        'param_file',
+        default_value=default_param_file,
+        description='Path to the parameter file'
+    )
+
+    param_file = LaunchConfiguration('param_file')
+
+    dual_rtk_driver_node = Node(
+        package='wheeltec_dual_rtk_driver',
+        executable='dual_rtk_driver_node',
+        name='dual_rtk_driver_node',
+        parameters=[param_file],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        param_file_arg,
+        dual_rtk_driver_node,
+    ])
@@ -0,0 +1,49 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+"""Launch file for the NMEA serial driver with YAML configuration."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros import actions
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    """Generate a launch description for NMEA serial driver."""
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory("wheeltec_gps_driver")
+    default_param_file = os.path.join(pkg_share, "config", "nmea_driver.param.yaml")
+
+    # 声明启动参数
+    param_file_arg = DeclareLaunchArgument(
+        'param_file',
+        default_value=default_param_file,
+        description='Path to the parameter file'
+    )
+
+    param_file = LaunchConfiguration('param_file')
+
+    driver_node = actions.Node(
+        package='nmea_navsat_driver',
+        executable='nmea_serial_driver',
+        name='nmea_serial_driver',
+        output='screen',
+        parameters=[param_file],
+    )
+
+    return LaunchDescription([param_file_arg, driver_node])
@@ -0,0 +1,41 @@
+import os
+import ament_index_python.packages
+import launch
+import launch_ros.actions
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    """Generate launch description for u-blox GPS driver."""
+    # 获取参数文件路径
+    pkg_share = ament_index_python.packages.get_package_share_directory('wheeltec_gps_driver')
+    default_param_file = os.path.join(pkg_share, 'config', 'ublox_driver.param.yaml')
+
+    # 声明启动参数
+    param_file_arg = DeclareLaunchArgument(
+        'param_file',
+        default_value=default_param_file,
+        description='Path to the parameter file'
+    )
+
+    param_file = LaunchConfiguration('param_file')
+
+    ublox_gps_node = launch_ros.actions.Node(
+        package='ublox_gps',
+        executable='ublox_gps_node',
+        name='ublox_gps_node',
+        output='both',
+        parameters=[param_file],
+    )
+
+    return launch.LaunchDescription([
+        param_file_arg,
+        ublox_gps_node,
+        launch.actions.RegisterEventHandler(
+            event_handler=launch.event_handlers.OnProcessExit(
+                target_action=ublox_gps_node,
+                on_exit=[launch.actions.EmitEvent(
+                    event=launch.events.Shutdown())],
+            )),
+    ])
@@ -0,0 +1,18 @@
+<?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>wheeltec_gps_driver</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wheeltec@todo.todo">wheeltec</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,160 @@
+
+#include "rclcpp/rclcpp.hpp"
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/path.hpp>
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <math.h>
+#define EARTH_RADIUS 6378.137
+using std::placeholders::_1;
+bool pose_init;
+
+rclcpp::Node::SharedPtr nh_=nullptr;
+
+class GpsPath : public rclcpp::Node
+{
+  public:
+    GpsPath()
+    : Node("GpsPath")
+    {
+      state_pub_ = create_publisher<nav_msgs::msg::Path>("gps_path", 10);
+      subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
+      "/gps_topic", 1, std::bind(&GpsPath::gps_callback, this, _1));
+    }
+    struct lla_pose
+    {
+      double latitude;
+      double longitude;
+      double altitude;
+    };
+    double rad(double d) 
+    {
+    	return d * 3.1415926 / 180.0;
+    }
+  
+  private:
+  
+ void publishTFFrames(const sensor_msgs::msg::NavSatFix::SharedPtr gps_msg_ptr, const geometry_msgs::msg::PoseStamped& current_position) {
+
+    //static tf2_ros::TransformBroadcaster tf_broadcaster;
+    static  std::shared_ptr<tf2_ros::TransformBroadcaster>  tf_broadcaster;
+    tf_broadcaster =std::make_shared<tf2_ros::TransformBroadcaster>(this);
+
+
+    // 发布GPS轨迹原点的TF坐标
+    geometry_msgs::msg::TransformStamped init_tf;
+    init_tf.header.stamp = rclcpp::Time(gps_msg_ptr->header.stamp.sec, gps_msg_ptr->header.stamp.nanosec);
+    init_tf.header.frame_id = "path"; // 假设GPS轨迹原点位于world坐标系
+    init_tf.child_frame_id = "gps_origin";
+    init_tf.transform.translation.x = 0.0;
+    init_tf.transform.translation.y = 0.0;
+    init_tf.transform.translation.z = 0.0;
+    tf2::Quaternion q;
+    q.setRPY(0, 0, 0);
+    init_tf.transform.rotation = tf2::toMsg(q);
+    tf_broadcaster->sendTransform(init_tf);
+
+    // 发布当前轨迹位置的TF坐标
+    geometry_msgs::msg::TransformStamped current_tf;
+    //current_tf.header.stamp = ros::Time::now();
+    current_tf.header.stamp = rclcpp::Time(current_position.header.stamp.sec, current_position.header.stamp.nanosec);
+    current_tf.header.frame_id = "path"; // 假设当前轨迹位置位于world坐标系
+    current_tf.child_frame_id = "current_path_position";
+    current_tf.transform.translation.x = current_position.pose.position.x;
+    current_tf.transform.translation.y = current_position.pose.position.y;
+    current_tf.transform.translation.z = current_position.pose.position.z;
+    current_tf.transform.rotation = current_position.pose.orientation;
+    q.setRPY(0, 0, 0); // 设置姿态为单位四元数
+    current_tf.transform.rotation = tf2::toMsg(q);
+    tf_broadcaster->sendTransform(current_tf);
+} 
+  
+  
+    void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr gps_msg) 
+    {
+      if(!pose_init)
+       {
+        init_pose.latitude = gps_msg->latitude;
+        init_pose.longitude = gps_msg->longitude;
+        init_pose.altitude = gps_msg->altitude;
+        pose_init = true;
+        }
+      else
+        {
+        //计算相对位置
+            double radLat1 ,radLat2, radLong1,radLong2,delta_lat,delta_long,x,y;
+            
+            radLat1 = rad(init_pose.latitude);
+            radLong1 = rad(init_pose.longitude);
+            
+            radLat2 = rad(gps_msg->latitude);
+            radLong2 = rad(gps_msg->longitude);
+                //计算x
+            delta_lat = radLat2 - radLat1;
+            delta_long = 0;
+            
+            
+            if(delta_lat>0)
+              x = -2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));
+            else
+              x = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));
+                x = x*EARTH_RADIUS*1000;
+        
+                //计算y
+            delta_lat = 0;
+                delta_long = radLong2  - radLong1;
+            if(delta_long>0)
+              y = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
+            else
+              y = -2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
+               y = y*EARTH_RADIUS*1000;
+    
+            //计算z
+            double z = gps_msg->altitude - init_pose.altitude;
+            // 更新当前位置
+            geometry_msgs::msg::PoseStamped current_position;
+            current_position.header.frame_id = "path";
+            current_position.header.stamp = rclcpp::Node::now();
+            current_position.pose.position.x = x;
+            current_position.pose.position.y = y;
+            current_position.pose.position.z = z;
+            
+            //·发布轨迹
+            ros_path_.header.frame_id = "path";
+            ros_path_.header.stamp = rclcpp::Node::now();  
+            geometry_msgs::msg::PoseStamped pose;
+            pose.header = ros_path_.header;
+            pose.pose.position.x = x;
+            pose.pose.position.y = y;
+            pose.pose.position.z = z;
+            ros_path_.poses.push_back(pose);
+            
+            state_pub_->publish(ros_path_);
+    
+            RCLCPP_INFO(this->get_logger(),"( x:%0.6f ,y:%0.6f ,z:%0.6f)",x ,y ,z );
+
+        }        
+        
+        
+    }
+    
+    nav_msgs::msg::Path ros_path_;
+    lla_pose init_pose;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr state_pub_;
+    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr subscription_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  pose_init = false;
+  rclcpp::spin(std::make_shared<GpsPath>());
+  rclcpp::shutdown();
+  return 0;
+}
+
+
+
@@ -0,0 +1,187 @@
+# ROS2串口例程
+
+本文档介绍如何在ROS2下来读取超核电子IMU&GNSS的数据，并提供了c++语言例程代码，通过执行ROS2命令，运行相应的节点，就可以看到打印到终端上的信息。
+
+* 测试环境：Ubuntu20.04   
+
+* ROS版本：ROS2 Foxy
+
+* 测试设备：超核电子IMU系列产品
+
+## 安装USB-UART驱动
+
+Ubuntu 系统自带CP210x的驱动，默认不需要安装串口驱动。将调试版连接到电脑上时，会自动识别设备。识别成功后，会在dev目录下出现一个对应的设备:ttyUSBx
+
+检查USB-UART设备是否被Ubantu识别：
+
+1. 打开终端，输入`ls /dev`,先查看已经存在的串口设备。
+2. 查看是否已经存在  ttyUSBx 这个设备文件，便于确认对应的端口号。
+4. 接下来插入USB线，连接调试板，然后再次执行`ls /dev`。 dev目录下多了一个设备`ttyUSB0`：
+
+```shell
+linux@ubuntu:~$ ls /dev
+.....
+hpet             net           tty11     tty4   ttyS0      ttyUSB0    vhost-vsock
+hugepages        null          tty12     tty40  ttyS1      udmabuf  vmci
+......
+```
+
+4.打开USB设备的可执行权限：
+
+```shell
+   $ sudo chmod 777 /dev/ttyUSB0
+```
+
+## 编译hipnuc_ws工作空间
+
+1. 打开终端进入/examples/ROS2/hipnuc_ws 目录
+2. 执行`colcon build`命令，编译成功后出现如下信息。
+
+```shell
+linux@ubuntu20:~/hipnuc_ws$ colcon build
+Starting >>> hipnuc_gnss
+Starting >>> hipnuc_imu
+Finished <<< hipnuc_imu [0.44s]                                      
+Finished <<< hipnuc_gnss [0.49s]
+
+Summary: 2 packages finished [0.61s]
+linux@ubuntu20:~/hipnuc_ws$ 
+```
+
+##  修改串口波特率和设备号
+
+1. 在Ubuntu环境中，支持的波特率为115200, 460800, 921600。本例程使用的默认波特率是115200，默认打开的串口名称是/dev/ttyUSB0。	
+
+2. 如果您需要更高的输出频率，请修改`config/hipnuc_config.yaml`文件中的配置参数。	
+
+```c
+#hipnuc_imu config file
+IMU_publisher:
+    ros__parameters:
+        serial_port: "/dev/ttyUSB0"
+        baud_rate: 115200
+        frame_id: "base_link"
+        imu_topic: "/IMU_data"
+
+            
+ #hipnuc_gnss config file
+ INS_publisher:
+    ros__parameters:
+        serial_port: "/dev/ttyUSB0"
+        baud_rate: 115200
+        frame_id: "gnss_link"
+        imu_topic: "/rawimu_data"
+        nav_topic: "/NavSatFix_data"
+```
+
+注意修改后需要回到hipnuc_ws目录下，重新执行`colcon build`命令
+
+## 显示数据
+	查看数据方式：
+
+	1、输出ROS 定义的sensor_msgs::Imu。
+
+	2、输出ROS 定义的sensor_msgs::NavSatFix
+
+###  输出ROS标准 Imu.msg
+
+1. 打开终端，执行：
+
+```shell
+linux@ubuntu20:~$ ros2 launch hipnuc_imu imu_spec_msg.launch.py
+```
+
+	2.如果执行失败，提示找不到相应的launch文件，则需要配置环境，在当前终端执行：
+
+```shell
+linux@ubuntu:~$source <hipnuc_ws_dir>/install/setup.bash
+```
+
+	3.执行成功后，就可以看到所有的信息：
+
+```c
+[listener-2] ---
+[listener-2] header:
+[listener-2] 	stamp:
+[listener-2] 	  secs:1639099575
+[listener-2] 	  nanosecs:538349240
+[listener-2] 	frame_id:base_link
+[listener-2] orientation:
+[listener-2] 	x: -0.095125280320644379
+[listener-2] 	y: -0.483648955821990967
+[listener-2] 	z: 0.053129896521568298
+[listener-2] 	w: 0.868453860282897949
+[listener-2] orientation_covariance: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+[listener-2] angular_velocity: 
+[listener-2] 	x: -0.000815955184543841
+[listener-2] 	y: -0.001057390143056437
+[listener-2] 	z: 0.001062464062371403
+[listener-2] angular_velocity_covariance: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+[listener-2] linear_acceleration:
+[listener-2] 	x: 8.110355603694916482
+[listener-2] 	y: -2.125157430768013000
+[listener-2] 	z: 5.013053989410400924
+[listener-2] linear_acceleration_covariance: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+[listener-2] ---
+```
+
+	4、另开一个终端窗口，执行`ros2 topic hz /Imu_data`，可以查看话题发布的频率。
+
+```shell
+linux@ubuntu20:~$ ros2 topic hz /Imu_data 
+average rate: 100.032
+	min: 0.008s max: 0.012s std dev: 0.00058s window: 102
+average rate: 100.014
+	min: 0.008s max: 0.012s std dev: 0.00054s window: 202
+average rate: 100.019
+	min: 0.007s max: 0.013s std dev: 0.00064s window: 303
+^C
+linux@ubuntu20:~$ 
+```
+
+### 输出ROS标准的NavSatFix.msg
+
+	1、打开终端，执行：
+
+```shell
+linux@ubuntu20:~$ ros2 launch hipnuc_gnss nav_spec_msg.launch.py 
+```
+
+	2、如果执行失败，提示找不到相应的launch文件，则需要配置环境，在当前终端执行：
+
+```shell
+linux@ubuntu:~$source <hipnuc_ws_dir>/install/setup.bash
+```
+
+	3、执行成功后，可以看到如下信息：
+
+```shell
+[listener_INS-2] header:
+[listener_INS-2] 	stamp:
+[listener_INS-2] 	  secs: 1724034005
+[listener_INS-2] 	  nanosecs: 370900173
+[listener_INS-2] 	frame_id: gnss_link
+[listener_INS-2] status:
+[listener_INS-2] 	status: 1
+[listener_INS-2] 	service: 0
+[listener_INS-2] latitude: 40.20336080
+[listener_INS-2] longitude: 116.24086010
+[listener_INS-2] altitude: 66.30100000
+[listener_INS-2] orientation_covariance: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+[listener_INS-2] position_covariance_type: 0
+```
+
+	4、另开一个终端窗口，执行`ros2 topic hz /NavSatFix_data`，可以查看话题发布的频率。
+
+```shell
+linux@ubuntu20:~$ ros2 topic hz /NavSatFix_data`
+average rate: 10.032
+	min: 0.008s max: 0.012s std dev: 0.00058s window: 10
+average rate: 10.014
+	min: 0.008s max: 0.012s std dev: 0.00054s window: 20
+average rate: 10.019
+	min: 0.007s max: 0.013s std dev: 0.00064s window: 30
+^C
+linux@ubuntu20:~$
+```
+
@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.5)
+project(hipnuc_imu)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(driver_timestamp_sync REQUIRED)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+add_executable(talker src/serial_port.cpp src/hipnuc_dec.c)
+ament_target_dependencies(talker rclcpp std_msgs sensor_msgs driver_timestamp_sync)
+
+add_executable(listener src/sub_spec.cpp)
+ament_target_dependencies(listener rclcpp std_msgs sensor_msgs)
+
+install(TARGETS talker listener  DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY
+  config/
+  DESTINATION share/${PROJECT_NAME}/config
+)
+
+ament_package()
@@ -0,0 +1,13 @@
+IMU_publisher:
+    ros__parameters:
+        serial_port: "/dev/ttyUSB0"
+        baud_rate: 115200
+        frame_id: "imu_link"
+        imu_topic: "/sensing/imu/hipnuc/imu_raw"
+        use_timestamp_sync: false
+        time_offset_sec: 0.0
+        bias_alpha: 0.01
+        jump_warn_threshold_ms: 1000
+        clamp_backward_time: true
+        timestamp_diagnostics: false
+        timestamp_diagnostics_throttle_ms: 2000
@@ -0,0 +1,28 @@
+##launch file
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('hipnuc_imu'),
+        'config',
+        'hipnuc_config.yaml',
+    ),
+
+    return LaunchDescription([
+         Node(
+            package='hipnuc_imu',
+            executable='talker',
+            name='IMU_publisher',
+            parameters=[config],
+            output='screen',
+            ),
+        Node(
+            package='hipnuc_imu',
+            executable='listener',
+            output='screen'
+            ),
+        ])
+
@@ -0,0 +1,23 @@
+<?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>hipnuc_imu</name>
+  <version>0.0.0</version>
+  <description>hipnuc_imu_data </description>
+  <maintainer email="linux@todo.todo">linux</maintainer>
+  <license>Apache License 2.0 </license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>driver_timestamp_sync</depend>
+  
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,136 @@
+#ifndef __HIPNUC_H__
+#define __HIPNUC_H__
+
+#ifdef __cplusplus
+extern "C"{
+#endif
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+
+#ifdef QT_CORE_LIB
+#pragma pack(push)
+#pragma pack(1)
+#endif
+
+#define CHSYNC1                 (0x5A)              /* CHAOHE message sync code 1 */
+#define CHSYNC2                 (0xA5)              /* CHAOHE message sync code 2 */
+#define CH_HDR_SIZE             (0x06)          /* CHAOHE protocol header size */
+#define HIPNUC_MAX_RAW_SIZE     (128+CH_HDR_SIZE)
+
+
+/**
+* packet 0x91: IMU data(floating point)
+ */
+typedef struct __attribute__((__packed__))
+{
+    uint8_t         tag; /* data packet tag, if tag = 0x00, means that this packet is null */
+    uint16_t        pps_sync_ms;
+    int8_t          temp;
+    float           prs;
+    uint32_t        ts; /* timestamp */
+    float           acc[3];
+    float           gyr[3];
+    float           mag[3];
+    float           roll;
+    float           pitch;
+    float           yaw;
+    float           quat[4];
+} hi91_t;
+
+/**
+* packet 0x92: IMU data(int type)
+ */
+typedef struct __attribute__((__packed__))
+{
+    uint8_t         tag;
+    uint16_t        status;
+    int8_t          temperature;
+    uint16_t        sync_time;
+    int16_t         air_pressure;
+    int16_t         reserved;
+    int16_t         gyr_b[3];
+    int16_t         acc_b[3];
+    int16_t         mag_b[3];
+    int32_t         roll;
+    int32_t         pitch;
+    int32_t         yaw;
+    int16_t        quat[4];
+} hi92_t;
+
+/**
+* packet 0x81: INS data, include lat, lon, eul, quat, raw IMU data 
+ */
+typedef struct __attribute__((__packed__))
+{
+    uint8_t         tag;
+    uint16_t        status;
+    uint8_t         ins_status;
+    uint16_t        gpst_wn;
+    uint32_t        gpst_tow;
+    uint16_t        sync_time;
+    int16_t         gyr_b[3];
+    int16_t         acc_b[3];
+    int16_t         mag_b[3];
+    int16_t         air_pressure;
+    int16_t         reserved1;
+    int8_t          temperature;
+    uint8_t         utc_year;
+    uint8_t         utc_mouth;
+    uint8_t         utc_day;
+    uint8_t         utc_hour;
+    uint8_t         utc_min;
+    uint16_t        utc_msec;
+    int16_t         roll;
+    int16_t         pitch;
+    uint16_t        yaw;
+    int16_t         quat[4];
+    int32_t         ins_lon;
+    int32_t         ins_lat;
+    int32_t         ins_msl;
+    uint8_t         pdop;
+    uint8_t         hdop;
+    uint8_t         solq_pos;
+    uint8_t         nv_pos;
+    uint8_t         solq_heading;
+    uint8_t         nv_heading;
+    uint8_t         diff_age;
+    int16_t         undulation;
+    uint8_t         reserved;
+    int16_t         vel_enu[3];
+    int16_t         acc_enu[3];
+    int32_t         gnss_lon;
+    int32_t         gnss_lat;
+    int32_t         gnss_msl;
+    uint8_t         reserved2[2];
+} hi81_t;
+
+
+
+typedef struct
+{
+    int nbyte;                          /* number of bytes in message buffer */ 
+    int len;                            /* message length (bytes) */
+    uint8_t buf[HIPNUC_MAX_RAW_SIZE];   /* message raw buffer */
+    hi91_t hi91;
+    hi92_t hi92;
+    hi81_t hi81;
+}hipnuc_raw_t;
+
+#ifdef QT_CORE_LIB
+#pragma pack(pop)
+#endif
+
+int hipnuc_input(hipnuc_raw_t *raw, uint8_t data);
+int hipnuc_dump_packet(hipnuc_raw_t *raw, char *buf, size_t buf_size);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif
+
+
@@ -0,0 +1,323 @@
+#include "hipnuc.h"
+
+
+/* legcy support of HI226/HI229 */
+#define HIPNUC_ID_USRID         (0x90)
+#define HIPNUC_ID_ACC_RAW       (0xA0)
+#define HIPNUC_ID_ACC_CAL       (0xA1)
+#define HIPNUC_ID_GYR_RAW       (0xB0)
+#define HIPNUC_ID_GYR_CAL       (0xB1)
+#define HIPNUC_ID_MAG_RAW       (0xC0)
+#define HIPNUC_ID_EUL           (0xD0)
+#define HIPNUC_ID_QUAT          (0xD1)
+#define HIPNUC_ID_PRS           (0xF0)
+
+/* new HiPNUC standard packet */
+#define HIPNUC_ID_IMUSOL        (0x91)
+#define HIPNUC_ID_IMUBIN        (0x92)
+#define HIPNUC_ID_INSSOL        (0x81)
+
+#ifndef D2R
+#define D2R (0.0174532925199433F)
+#endif
+
+#ifndef R2D
+#define R2D (57.2957795130823F)
+#endif
+
+#ifndef GRAVITY
+#define GRAVITY (9.8F)
+#endif
+
+
+static void hipnuc_crc16(uint16_t *inital, const uint8_t *buf, uint32_t len);
+
+/* common type conversion */
+#define I2(p) (*((int16_t *)(p)))
+static uint16_t U2(uint8_t *p)
+{
+    uint16_t u;
+    memcpy(&u, p, 2);
+    return u;
+}
+
+static float R4(uint8_t *p)
+{
+    float r;
+    memcpy(&r, p, 4);
+    return r;
+}
+
+/* parse the payload of a frame and feed into data section */
+static int parse_data(hipnuc_raw_t *raw)
+{
+    int ofs = 0;
+    uint8_t *p = &raw->buf[CH_HDR_SIZE];
+    
+    /* ignore all previous data */
+    raw->hi91.tag = 0;
+    raw->hi81.tag = 0;
+    raw->hi92.tag = 0;
+
+    while (ofs < raw->len)
+    {
+        switch (p[ofs])
+        {
+        case HIPNUC_ID_USRID:
+            ofs += 2;
+            break;
+        case HIPNUC_ID_ACC_RAW:
+        case HIPNUC_ID_ACC_CAL:
+             raw->hi91.tag = HIPNUC_ID_IMUSOL;
+             raw->hi91.acc[0] = (float)I2(p + ofs + 1) / 1000;
+             raw->hi91.acc[1] = (float)I2(p + ofs + 3) / 1000;
+             raw->hi91.acc[2] = (float)I2(p + ofs + 5) / 1000;
+            ofs += 7;
+            break;
+        case HIPNUC_ID_GYR_RAW:
+        case HIPNUC_ID_GYR_CAL:
+            raw->hi91.tag = HIPNUC_ID_IMUSOL;
+            raw->hi91.gyr[0] = (float)I2(p + ofs + 1) / 10;
+            raw->hi91.gyr[1] = (float)I2(p + ofs + 3) / 10;
+            raw->hi91.gyr[2] = (float)I2(p + ofs + 5) / 10;
+            ofs += 7;
+            break;
+        case HIPNUC_ID_MAG_RAW:
+            raw->hi91.tag = HIPNUC_ID_IMUSOL;
+            raw->hi91.mag[0] = (float)I2(p + ofs + 1) / 10;
+            raw->hi91.mag[1] = (float)I2(p + ofs + 3) / 10;
+            raw->hi91.mag[2] = (float)I2(p + ofs + 5) / 10;
+            ofs += 7;
+            break;
+        case HIPNUC_ID_EUL:
+            raw->hi91.tag = HIPNUC_ID_IMUSOL;
+            raw->hi91.pitch = (float)I2(p + ofs + 1) / 100;
+            raw->hi91.roll = (float)I2(p + ofs + 3) / 100;
+            raw->hi91.yaw = (float)I2(p + ofs + 5) / 10;
+            ofs += 7;
+            break;
+        case HIPNUC_ID_QUAT:
+            raw->hi91.tag = HIPNUC_ID_IMUSOL;
+            raw->hi91.quat[0] = R4(p + ofs + 1);
+            raw->hi91.quat[1] = R4(p + ofs + 5);
+            raw->hi91.quat[2] = R4(p + ofs + 9);
+            raw->hi91.quat[3] = R4(p + ofs + 13);
+            ofs += 17;
+            break;
+        case HIPNUC_ID_PRS:
+            raw->hi91.tag = HIPNUC_ID_IMUSOL;
+            raw->hi91.prs = R4(p + ofs + 1);
+            ofs += 5;
+            break;
+        case HIPNUC_ID_IMUSOL:
+            memcpy(&raw->hi91, p + ofs, sizeof(hi91_t));
+            ofs += sizeof(hi91_t);
+            break;
+        case HIPNUC_ID_INSSOL:
+            memcpy(&raw->hi81, p + ofs, sizeof(hi81_t));
+            ofs += sizeof(hi81_t);
+            break;
+        case HIPNUC_ID_IMUBIN:
+            memcpy(&raw->hi92, p + ofs, sizeof(hi92_t));
+            ofs += sizeof(hi92_t);
+            break;
+        default:
+            ofs++;
+            break;
+        }
+    }
+    return 1;
+}
+
+static int decode_hipnuc(hipnuc_raw_t *raw)
+{
+    uint16_t crc = 0;
+
+    /* checksum */
+    hipnuc_crc16(&crc, raw->buf, (CH_HDR_SIZE-2));
+    hipnuc_crc16(&crc, raw->buf + CH_HDR_SIZE, raw->len);
+    if (crc != U2(raw->buf + (CH_HDR_SIZE-2)))
+    {
+        // NL_TRACE("ch checksum error: frame:0x%X calcuate:0x%X, len:%d\n", U2(raw->buf + 4), crc, raw->len);
+        return -1;
+    }
+
+    return parse_data(raw);
+}
+
+/* sync code */
+static int sync_hipnuc(uint8_t *buf, uint8_t data)
+{
+    buf[0] = buf[1];
+    buf[1] = data;
+    return buf[0] == CHSYNC1 && buf[1] == CHSYNC2;
+}
+
+/**
+* @brief     hipnuc decoder input, read one byte at one time.
+ *
+ * @param    raw is the decoder struct.
+ * @param    data is the one byte read from stram.
+ * @param    buf is the log string buffer, ireturn > 0: decoder received a frame successfully, else: receiver not receive a frame successfully.
+ *
+ */
+int hipnuc_input(hipnuc_raw_t *raw, uint8_t data)
+{
+    /* synchronize frame */
+    if (raw->nbyte == 0)
+    {
+        if (!sync_hipnuc(raw->buf, data))
+            return 0;
+        raw->nbyte = 2;
+        return 0;
+    }
+
+    raw->buf[raw->nbyte++] = data;
+
+    if (raw->nbyte == CH_HDR_SIZE)
+    {
+        if ((raw->len = U2(raw->buf + 2)) > (HIPNUC_MAX_RAW_SIZE - CH_HDR_SIZE))
+        {
+            // NL_TRACE("ch length error: len=%d\n",raw->len);
+            raw->nbyte = 0;
+            return -1;
+        }
+    }
+
+    if (raw->nbyte < CH_HDR_SIZE || raw->nbyte < (raw->len + CH_HDR_SIZE))
+    {
+        return 0;
+    }
+
+    raw->nbyte = 0;
+
+    return decode_hipnuc(raw);
+}
+
+
+/**
+ * @brief    convert packet to string, only dump parts of data
+ *
+ * @param    raw is struct of decoder
+ * @param    buf is the log string buffer, make sure buf larger than 256
+ * @param    buf_size is the szie of the log buffer
+ *
+ */
+int hipnuc_dump_packet(hipnuc_raw_t *raw, char *buf, size_t buf_size)
+{
+    int written = 0;
+    int ret;
+
+    /* dump 0x91 packet */
+    if(raw->hi91.tag == HIPNUC_ID_IMUSOL)
+    {
+        ret = snprintf(buf + written, buf_size - written, "%-16s0x%X\r\n", "tag:", raw->hi91.tag);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%d\r\n", "sync_time(ms):", raw->hi91.pps_sync_ms);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "acc(m/s^(2)):", raw->hi91.acc[0]*GRAVITY, raw->hi91.acc[1]*GRAVITY, raw->hi91.acc[2]*GRAVITY);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "gyr(deg/s):", raw->hi91.gyr[0], raw->hi91.gyr[1], raw->hi91.gyr[2]);
+        if (ret > 0) written += ret;
+
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "mag(uT):", raw->hi91.mag[0], raw->hi91.mag[1], raw->hi91.mag[2]);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "Roll/Pitch/Yaw(deg):", raw->hi91.roll, raw->hi91.pitch, raw->hi91.yaw);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%d\r\n", "timestamp(ms):", raw->hi91.ts);
+        if (ret > 0) written += ret;
+    }
+    
+    /* dump 0x92 packet */
+    if(raw->hi92.tag == HIPNUC_ID_IMUBIN)
+    {
+        ret = snprintf(buf + written, buf_size - written, "%-16s0x%X\r\n", "tag:", raw->hi92.tag);
+        if (ret > 0) written += ret;
+    
+        ret = snprintf(buf + written, buf_size - written, "%-16s%d\r\n", "temperature", raw->hi92.temperature);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%d\r\n", "sync_time(ms):", raw->hi92.sync_time);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "acc(m/s^(2)):", raw->hi92.acc_b[0]*0.0048828, raw->hi92.acc_b[1]*0.0048828, raw->hi92.acc_b[2]*0.0048828);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "gyr(deg/s):", raw->hi92.gyr_b[0]*(0.001*R2D), raw->hi92.gyr_b[1]*(0.001*R2D), raw->hi92.gyr_b[2]*(0.001*R2D));
+        if (ret > 0) written += ret;
+
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "mag(uT):", raw->hi92.mag_b[0]*0.030517, raw->hi92.mag_b[1]*0.030517, raw->hi92.mag_b[2]*0.030517);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "Roll/Pitch/Yaw(deg):", raw->hi92.roll*0.001, raw->hi92.pitch*0.001, raw->hi92.yaw*0.001);
+        if (ret > 0) written += ret;
+    }
+
+    /* dump 0x81 packet */
+    if(raw->hi81.tag == HIPNUC_ID_INSSOL)
+    {
+        ret = snprintf(buf + written, buf_size - written, "%-16s0x%X\r\n", "tag:", raw->hi81.tag);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%d\r\n", "solq_pos:", raw->hi81.solq_pos);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%d\r\n", "sat number:", raw->hi81.nv_pos);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.7f %.7f\r\n", "Lat/Lon(deg):", raw->hi81.ins_lat*1e-7, raw->hi81.ins_lon*1e-7);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f\r\n", "height(m):", raw->hi81.ins_msl*1e-3);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "acc(m/s^(2)):", raw->hi81.acc_b[0]*0.0048828, raw->hi81.acc_b[1]*0.0048828, raw->hi81.acc_b[2]*0.0048828);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "gyr(deg/s):", raw->hi81.gyr_b[0]*(0.001*R2D), raw->hi81.gyr_b[1]*(0.001*R2D), raw->hi81.gyr_b[2]*(0.001*R2D));
+        if (ret > 0) written += ret;
+
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "mag(uT):", raw->hi81.mag_b[0]*0.030517, raw->hi81.mag_b[1]*0.030517, raw->hi81.mag_b[2]*0.030517);
+        if (ret > 0) written += ret;
+        
+        ret = snprintf(buf + written, buf_size - written, "%-16s%.3f %.3f %.3f\r\n", "Roll/Pitch/Yaw(deg):", raw->hi81.roll*0.01, raw->hi81.pitch*0.01, raw->hi81.yaw*0.01);
+        if (ret > 0) written += ret;
+    }
+    
+    return written;
+}
+
+/**
+ * @brief    calcuate hipnuc_crc16
+ *
+ * @param    inital is intial value
+ * @param    buf    is input buffer pointer
+ * @param    len    is length of the buffer
+ *
+ */
+static void hipnuc_crc16(uint16_t *inital, const uint8_t *buf, uint32_t len)
+{
+    uint32_t crc = *inital;
+    uint32_t j;
+    for (j=0; j < len; ++j)
+    {
+        uint32_t i;
+        uint32_t byte = buf[j];
+        crc ^= byte << 8;
+        for (i = 0; i < 8; ++i)
+        {
+            uint32_t temp = crc << 1;
+            if (crc & 0x8000)
+            {
+                temp ^= 0x1021;
+            }
+            crc = temp;
+        }
+    } 
+    *inital = crc;
+}
@@ -0,0 +1,251 @@
+#include <iostream>
+#include <sensor_msgs/msg/imu.hpp>
+#include "rclcpp/rclcpp.hpp"
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <termios.h>
+
+#include "driver_timestamp_sync/timestamp_sync.hpp"
+
+#ifdef __cplusplus
+extern "C"{
+#endif
+#include <poll.h>
+#include "hipnuc.h"
+
+#define GRA_ACC     (9.8)
+#define DEG_TO_RAD  (0.01745329)
+#define BUF_SIZE    (1024)
+#ifdef __cplusplus
+}
+#endif
+
+using namespace std::chrono_literals;
+using namespace std;
+static hipnuc_raw_t raw;
+
+class IMUPublisher : public rclcpp::Node
+{
+	public:
+		int fd = 0;
+		uint8_t buf[BUF_SIZE] = {0};
+		IMUPublisher() : Node("IMU_publisher")
+		{
+			this->declare_parameter<std::string>("serial_port", "/dev/ttyUSB1");
+			this->declare_parameter<int>("baud_rate", 460800);
+			this->declare_parameter<std::string>("frame_id", "base_link");
+			this->declare_parameter<std::string>("imu_topic", "/IMU_data");
+			this->declare_parameter<bool>("use_timestamp_sync", true);
+			this->declare_parameter<double>("time_offset_sec", 0.0);
+			this->declare_parameter<double>("bias_alpha", 0.01);
+			this->declare_parameter<int>("jump_warn_threshold_ms", 1000);
+			this->declare_parameter<bool>("clamp_backward_time", true);
+			this->declare_parameter<bool>("timestamp_diagnostics", false);
+			this->declare_parameter<int>("timestamp_diagnostics_throttle_ms", 2000);
+
+			this->get_parameter("serial_port", serial_port);
+			this->get_parameter("baud_rate", baud_rate);
+			this->get_parameter("frame_id", frame_id);
+			this->get_parameter("imu_topic", imu_topic);
+
+			use_timestamp_sync_ = this->get_parameter("use_timestamp_sync").as_bool();
+
+			driver_timestamp_sync::TimestampSyncConfig config;
+			config.time_offset_sec = this->get_parameter("time_offset_sec").as_double();
+			config.bias_alpha = this->get_parameter("bias_alpha").as_double();
+			config.jump_warn_threshold_ms = static_cast<uint32_t>(
+				this->get_parameter("jump_warn_threshold_ms").as_int());
+			config.clamp_backward_time = this->get_parameter("clamp_backward_time").as_bool();
+			timestamp_sync_.set_config(config);
+			timestamp_diagnostics_ = this->get_parameter("timestamp_diagnostics").as_bool();
+			timestamp_diagnostics_throttle_ms_ = this->get_parameter(
+				"timestamp_diagnostics_throttle_ms").as_int();
+
+			RCLCPP_INFO(this->get_logger(),"serial_port: %s\r\n", serial_port.c_str());
+			RCLCPP_INFO(this->get_logger(), "baud_rate: %d\r\n", baud_rate);
+			RCLCPP_INFO(this->get_logger(), "frame_id: %s\r\n", frame_id.c_str());
+			RCLCPP_INFO(this->get_logger(), "imu_topic: %s\r\n", imu_topic.c_str());
+			RCLCPP_INFO(
+				this->get_logger(),
+				"timestamp_sync: enabled=%s offset=%.6f alpha=%.3f jump_warn_ms=%u clamp_backward=%s diagnostics=%s throttle_ms=%d",
+				use_timestamp_sync_ ? "true" : "false",
+				config.time_offset_sec,
+				config.bias_alpha,
+				config.jump_warn_threshold_ms,
+				config.clamp_backward_time ? "true" : "false",
+				timestamp_diagnostics_ ? "true" : "false",
+				timestamp_diagnostics_throttle_ms_);
+
+			imu_data.header.frame_id = frame_id;
+			imu_pub = this->create_publisher<sensor_msgs::msg::Imu>(imu_topic, 20);
+
+			fd = open_serial(serial_port, baud_rate);
+
+			while(1)
+				imu_read();
+		}
+
+	private:
+		void imu_read(void)
+		{
+			struct pollfd p;
+			p.fd = fd;
+			p.events = POLLIN;
+
+			int rpoll = poll(&p, 1, 5);
+
+			if(rpoll == 0)
+				return ;
+
+			const auto host_rx_time = this->get_clock()->now();
+			int n = read(fd, buf, sizeof(buf));
+
+			for(int i = 0; i < n; i++)
+			{
+				int rev = hipnuc_input(&raw, buf[i]);
+
+				if(rev)
+				{
+					imu_data.orientation.w = raw.hi91.quat[0];
+					imu_data.orientation.x = raw.hi91.quat[1];
+					imu_data.orientation.y = raw.hi91.quat[2];
+					imu_data.orientation.z = raw.hi91.quat[3];
+					imu_data.angular_velocity.x = raw.hi91.gyr[0] * DEG_TO_RAD;
+					imu_data.angular_velocity.y = raw.hi91.gyr[1] * DEG_TO_RAD;
+					imu_data.angular_velocity.z = raw.hi91.gyr[2] * DEG_TO_RAD;
+					imu_data.linear_acceleration.x = raw.hi91.acc[0] * GRA_ACC;
+					imu_data.linear_acceleration.y = raw.hi91.acc[1] * GRA_ACC;
+					imu_data.linear_acceleration.z = raw.hi91.acc[2] * GRA_ACC;
+
+					if (use_timestamp_sync_)
+					{
+						const auto sync_result = timestamp_sync_.reconstruct(raw.hi91.ts, host_rx_time);
+						imu_data.header.stamp = sync_result.stamp;
+
+						if (sync_result.large_jump_detected)
+						{
+							RCLCPP_WARN_THROTTLE(
+								this->get_logger(), *this->get_clock(), 2000,
+								"hipnuc timestamp jump detected: raw_ts_ms=%u delta_ms=%u unwrapped_ms=%llu",
+								raw.hi91.ts,
+								sync_result.delta_tick_ms,
+								static_cast<unsigned long long>(sync_result.unwrapped_device_tick_ms));
+						}
+
+						if (sync_result.backward_clamped)
+						{
+							RCLCPP_WARN_THROTTLE(
+								this->get_logger(), *this->get_clock(), 2000,
+								"hipnuc backward timestamp clamped: raw_ts_ms=%u stamp_ns=%lld",
+								raw.hi91.ts,
+								static_cast<long long>(sync_result.stamp.nanoseconds()));
+						}
+
+						if (timestamp_diagnostics_)
+						{
+							RCLCPP_INFO_THROTTLE(
+								this->get_logger(), *this->get_clock(), timestamp_diagnostics_throttle_ms_,
+								"hipnuc timestamp: raw_ts_ms=%u host_rx_ns=%lld stamp_ns=%lld delta_ms=%u bias_ns=%lld init=%s",
+								raw.hi91.ts,
+								static_cast<long long>(host_rx_time.nanoseconds()),
+								static_cast<long long>(sync_result.stamp.nanoseconds()),
+								sync_result.delta_tick_ms,
+								static_cast<long long>(sync_result.filtered_bias_ns),
+								sync_result.initialized_this_call ? "true" : "false");
+						}
+					}
+					else
+					{
+						imu_data.header.stamp = host_rx_time;
+					}
+
+					imu_pub->publish(imu_data);
+				}
+			}
+
+			memset(buf,0,sizeof(buf));
+		}
+
+		int open_serial(std::string port, int baud)
+		{
+			const char* port_device = port.c_str();
+			int fd = open(port_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+			if(fd == -1)
+			{
+				perror("unable to open serial port");
+				exit(0);
+			}
+
+			if(fcntl(fd, F_SETFL, O_NONBLOCK) < 0)
+				cout << "fcntl failed" << "\n" << endl;
+			else
+				fcntl(fd, F_SETFL, O_NONBLOCK);
+
+			struct termios options;
+			memset(&options, 0, sizeof(options));
+			tcgetattr(fd, &options);
+
+			switch(baud)
+			{
+				case 115200:
+					cfsetispeed(&options, B115200);
+					cfsetospeed(&options, B115200);
+				break;
+				case 460800:
+					cfsetispeed(&options, B460800);
+					cfsetospeed(&options, B460800);
+				break;
+				case 921600:
+					cfsetispeed(&options, B921600);
+					cfsetospeed(&options, B921600);
+				break;
+			}
+
+			options.c_cflag &= ~PARENB;
+			options.c_cflag &= ~CSTOPB;
+			options.c_cflag &= ~CSIZE;
+			options.c_cflag |= HUPCL;
+			options.c_cflag |= CS8;
+			options.c_cflag &= ~CRTSCTS;
+			options.c_cflag |= CREAD | CLOCAL;
+
+			options.c_iflag &= ~(IXON | IXOFF | IXANY);
+			options.c_iflag &= ~(INLCR|ICRNL);
+
+			options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+
+			options.c_oflag &= ~OPOST;
+			options.c_oflag &= ~(ONLCR|OCRNL);
+
+			options.c_cc[VMIN] = 0;
+			options.c_cc[VTIME] = 0;
+			tcsetattr(fd, TCSANOW, &options);
+
+			return fd;
+		}
+
+
+		std::string serial_port;
+		int baud_rate;
+		std::string frame_id;
+		std::string imu_topic;
+		sensor_msgs::msg::Imu imu_data = sensor_msgs::msg::Imu();
+		rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub;
+		driver_timestamp_sync::TimestampSync timestamp_sync_;
+		bool use_timestamp_sync_{true};
+		bool timestamp_diagnostics_{false};
+		int timestamp_diagnostics_throttle_ms_{2000};
+};
+
+
+int main(int argc, const char * argv[])
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<IMUPublisher>());
+	rclcpp::shutdown();
+
+	return 0;
+}
@@ -0,0 +1,76 @@
+#include <unistd.h>
+#include <memory>
+#include <iostream>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include <sensor_msgs/msg/imu.hpp>
+#include <iomanip>
+
+rclcpp::Node::SharedPtr nh = nullptr;
+using namespace std;
+
+void topic_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
+{
+
+	cout << "header:" << "\n";
+	cout << "	" << "stamp:"<< "\n";
+	cout << "	  " << "secs:" << msg->header.stamp.sec<< "\n";
+	cout << "	  " << "nanosecs:" << msg->header.stamp.nanosec << "\n";
+	cout << "	" << "frame_id:" << msg->header.frame_id << "\n" ;
+
+	cout << "orientation:" << "\n";
+	cout << "	" << "x: " << fixed << setprecision(18) << msg->orientation.x << "\n";
+	cout << "	" << "y: " << fixed << setprecision(18) << msg->orientation.y << "\n";
+	cout << "	" << "z: " << fixed << setprecision(18) << msg->orientation.z << "\n";
+	cout << "	" << "w: " << fixed << setprecision(18) << msg->orientation.w << "\n";
+	cout  << "orientation_covariance: [ " << fixed << setprecision(1) << msg->orientation_covariance[0];
+	cout  << ", " << msg->orientation_covariance[1];
+	cout  << ", " << msg->orientation_covariance[2];
+	cout  << ", " << msg->orientation_covariance[3];
+	cout  << ", " << msg->orientation_covariance[4];
+	cout  << ", " << msg->orientation_covariance[5];
+	cout  << ", " << msg->orientation_covariance[6];
+	cout  << ", " << msg->orientation_covariance[7];
+	cout  << ", " << msg->orientation_covariance[8] << "]" << "\n";
+
+	cout  << "angular_velocity: " << "\n";
+	cout  << "	" << "x: " << fixed << setprecision(18) << msg->angular_velocity.x << "\n";
+	cout  << "	" << "y: " << fixed << setprecision(18) << msg->angular_velocity.y << "\n";
+	cout  << "	" << "z: " << fixed << setprecision(18) << msg->angular_velocity.z << "\n";
+	cout  << "angular_velocity_covariance: [ " << fixed << setprecision(1) << msg->angular_velocity_covariance[0];
+	cout  << ", " << msg->angular_velocity_covariance[1];
+	cout  << ", " << msg->angular_velocity_covariance[2];
+	cout  << ", " << msg->angular_velocity_covariance[3];
+	cout  << ", " << msg->angular_velocity_covariance[4];
+	cout  << ", " << msg->angular_velocity_covariance[5];
+	cout  << ", " << msg->angular_velocity_covariance[6];
+	cout  << ", " << msg->angular_velocity_covariance[7];
+	cout  << ", " << msg->angular_velocity_covariance[8] << "]" << "\n";
+
+	cout  << "linear_acceleration:" << "\n";
+	cout  << "	" << "x: " << fixed << setprecision(18) << msg->linear_acceleration.x << "\n" ;
+	cout  << "	" << "y: " << fixed << setprecision(18) << msg->linear_acceleration.y << "\n" ;
+	cout  << "	" << "z: " << fixed << setprecision(18) << msg->linear_acceleration.z << "\n" ;
+	cout  << "linear_acceleration_covariance: [ " << fixed << setprecision(1) << msg->linear_acceleration_covariance[0];
+	cout  << ", " << msg->linear_acceleration_covariance[1] ;
+	cout  << ", " << msg->linear_acceleration_covariance[2] ;
+	cout  << ", " << msg->linear_acceleration_covariance[3] ;
+	cout  << ", " << msg->linear_acceleration_covariance[4] ;
+	cout  << ", " << msg->linear_acceleration_covariance[5] ;
+	cout  << ", " << msg->linear_acceleration_covariance[6] ;
+	cout  << ", " << msg->linear_acceleration_covariance[7] ;
+	cout  << ", " << msg->linear_acceleration_covariance[8] << "]" << "\n" << "---" << endl;
+}
+
+
+int main(int argc,const char* argv[])
+{
+	rclcpp::init(argc, argv);
+	nh = std::make_shared<rclcpp::Node>("imu_sub");
+	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub ;
+	imu_sub = nh->create_subscription<sensor_msgs::msg::Imu>("IMU_data", 10,topic_callback);
+	rclcpp::spin(nh);
+	rclcpp::shutdown();
+
+	return 0;
+}
@@ -0,0 +1,12 @@
+#CP2102 串口号0003 设置别名为wheeltec_IMU
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_IMU"' >/etc/udev/rules.d/wheeltec_imu.rules
+
+#CH9102 ，如果已经安装了驱动使用CH343	
+echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_IMU"' >/etc/udev/rules.d/wheeltec_imu_343.rules
+
+#CH9102  如果没有安装驱动
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_IMU"' >/etc/udev/rules.d/wheeltec_imu_ACM.rules
+
+service udev reload
+sleep 2
+service udev restart
@@ -0,0 +1,96 @@
+cmake_minimum_required(VERSION 3.8)
+project(pointcloud_process)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+add_library(pointcloud_converter_component SHARED
+  src/pointcloud_converter_component.cpp
+)
+
+target_include_directories(pointcloud_converter_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(pointcloud_converter_component
+  "rclcpp"
+  "rclcpp_components"
+  "sensor_msgs"
+  "std_msgs"
+)
+
+rclcpp_components_register_node(pointcloud_converter_component
+  PLUGIN "XyzirtToXyzircaedtNode"
+  EXECUTABLE pointcloud_converter_component_node  
+)
+
+install(TARGETS pointcloud_converter_component pointcloud_converter_component_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+add_library(pointcloud_transform_component SHARED
+  src/pointcloud_transform.cpp
+)
+
+target_include_directories(pointcloud_transform_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+target_compile_features(pointcloud_transform_component PUBLIC cxx_std_17)
+
+ament_target_dependencies(pointcloud_transform_component
+  "rclcpp"
+  "rclcpp_components"
+  "sensor_msgs"
+  "std_msgs"
+  "tf2"
+  "tf2_ros"
+  "pcl_conversions"
+  "pcl_ros"
+  "yaml-cpp"
+)
+
+rclcpp_components_register_node(pointcloud_transform_component
+  PLUGIN "PointCloudInterface"
+  EXECUTABLE pointcloud_transform_node  
+)
+
+install(TARGETS pointcloud_transform_component pointcloud_transform_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+  
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
@@ -0,0 +1,36 @@
+#ifndef POINTCLOUD_CONVERTER_COMPONENT_HPP_
+#define POINTCLOUD_CONVERTER_COMPONENT_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <memory>
+#include <string>
+
+class XyzirtToXyzircaedtNode : public rclcpp::Node
+{
+public:
+  explicit XyzirtToXyzircaedtNode(const rclcpp::NodeOptions & options);
+
+private:
+  void pandarCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const;
+  void rslidarCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const;
+  void timooCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const;
+  void fastlioCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const;
+  void liorfCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const;
+
+  bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name) const;
+
+  int addPointField(sensor_msgs::msg::PointCloud2 & cloud,
+                    const std::string & name, int count, int datatype, int offset) const;
+
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_fastlio_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_liorf_;
+  rclcpp::CallbackGroup::SharedPtr cb_group_;
+  std::string lidar_type_;
+  bool enable_fastlio_;
+  bool enable_liorf_;
+};
+
+#endif
@@ -0,0 +1,49 @@
+#ifndef POINTCLOUD_TRANSFORM_HPP_
+#define POINTCLOUD_TRANSFORM_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <memory>
+#include <string>
+
+class PointCloudInterface : public rclcpp::Node
+{
+public:
+   explicit PointCloudInterface(const rclcpp::NodeOptions & node_options);
+   virtual ~PointCloudInterface();
+  
+private:
+  struct CropBox
+  {
+    double min_x;
+    double max_x;
+    double min_y;
+    double max_y;
+    double min_z;
+    double max_z;
+  };
+
+  void processScan(const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg);
+  void setupTF();
+  std::unique_ptr<sensor_msgs::msg::PointCloud2> removeSelfPoints(
+    const sensor_msgs::msg::PointCloud2 & input);
+  bool getFieldOffset(
+    const sensor_msgs::msg::PointCloud2 & cloud, const std::string & field_name,
+    uint32_t & offset) const;
+
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr carla_cloud_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pandar_points_localization;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pandar_points_perception;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  rclcpp::CallbackGroup::SharedPtr cb_group_; 
+  std::string tf_output_frame_;
+  bool enable_self_crop_;
+  bool self_crop_apply_to_localization_;
+  CropBox self_crop_box_;
+};
+
+#endif
@@ -0,0 +1,123 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    lidar_type_arg = DeclareLaunchArgument(
+        'lidar_type',
+        default_value='timoo',
+        description='Lidar type: pandar, timoo, or rslidar'
+    )
+
+    input_pointcloud_topic_arg = DeclareLaunchArgument(
+        'input_pointcloud_topic',
+        default_value='/timoo/top/pointcloud',
+        description='Raw lidar pointcloud topic'
+    )
+
+    enable_fastlio_arg = DeclareLaunchArgument(
+        'enable_fastlio',
+        default_value='false',
+        description='Enable Fast-LIO2 pointcloud output'
+    )
+
+    enable_liorf_arg = DeclareLaunchArgument(
+        'enable_liorf',
+        default_value='false',
+        description='Enable LIO-RF pointcloud output'
+    )
+    enable_self_crop_arg = DeclareLaunchArgument(
+        'enable_self_crop',
+        default_value='true',
+        description='Remove ego-vehicle fixed-structure points from perception pointcloud'
+    )
+    self_crop_apply_to_localization_arg = DeclareLaunchArgument(
+        'self_crop_apply_to_localization',
+        default_value='true',
+        description='Apply self crop to localization pointcloud as well'
+    )
+    self_crop_min_x_arg = DeclareLaunchArgument('self_crop_min_x', default_value='1.00')
+    self_crop_max_x_arg = DeclareLaunchArgument('self_crop_max_x', default_value='1.70')
+    self_crop_min_y_arg = DeclareLaunchArgument('self_crop_min_y', default_value='-0.65')
+    self_crop_max_y_arg = DeclareLaunchArgument('self_crop_max_y', default_value='-0.05')
+    self_crop_min_z_arg = DeclareLaunchArgument('self_crop_min_z', default_value='1.10')
+    self_crop_max_z_arg = DeclareLaunchArgument('self_crop_max_z', default_value='2.05')
+
+    lidar_type = LaunchConfiguration('lidar_type')
+    input_pointcloud_topic = LaunchConfiguration('input_pointcloud_topic')
+    enable_fastlio = LaunchConfiguration('enable_fastlio')
+    enable_liorf = LaunchConfiguration('enable_liorf')
+    enable_self_crop = LaunchConfiguration('enable_self_crop')
+    self_crop_apply_to_localization = LaunchConfiguration('self_crop_apply_to_localization')
+    self_crop_min_x = LaunchConfiguration('self_crop_min_x')
+    self_crop_max_x = LaunchConfiguration('self_crop_max_x')
+    self_crop_min_y = LaunchConfiguration('self_crop_min_y')
+    self_crop_max_y = LaunchConfiguration('self_crop_max_y')
+    self_crop_min_z = LaunchConfiguration('self_crop_min_z')
+    self_crop_max_z = LaunchConfiguration('self_crop_max_z')
+
+    return LaunchDescription([
+        lidar_type_arg,
+        input_pointcloud_topic_arg,
+        enable_fastlio_arg,
+        enable_liorf_arg,
+        enable_self_crop_arg,
+        self_crop_apply_to_localization_arg,
+        self_crop_min_x_arg,
+        self_crop_max_x_arg,
+        self_crop_min_y_arg,
+        self_crop_max_y_arg,
+        self_crop_min_z_arg,
+        self_crop_max_z_arg,
+        ComposableNodeContainer(
+            name='pc_processor_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container_mt',
+            composable_node_descriptions=[
+                # 节点 1: 格式转换 (XYZIRT -> XYZIRCAEDT + FastLIO2 + LIO-RF)
+                ComposableNode(
+                    package='pointcloud_process',
+                    plugin='XyzirtToXyzircaedtNode',
+                    name='converter',
+                    parameters=[{
+                        'lidar_type': lidar_type,
+                        'enable_fastlio': enable_fastlio,
+                        'enable_liorf': enable_liorf
+                    }],
+                    remappings=[
+                            ('~/input/pointcloud', input_pointcloud_topic),
+                            ('~/output/pointcloud', '/sensing/lidar/top/converted/pointcloud'),
+                            ('~/output/fastlio_pointcloud', '/sensing/lidar/top/fastlio/pointcloud'),
+                            ('~/output/liorf_pointcloud', '/sensing/lidar/top/liorf/pointcloud')
+                        ]
+                ),
+
+                # 节点 2: 坐标变换
+                ComposableNode(
+                    package='pointcloud_process',
+                    plugin='PointCloudInterface',
+                    name='transformer',
+                    parameters=[{
+                        'enable_self_crop': ParameterValue(enable_self_crop, value_type=bool),
+                        'self_crop_apply_to_localization': ParameterValue(
+                            self_crop_apply_to_localization, value_type=bool),
+                        'self_crop_min_x': ParameterValue(self_crop_min_x, value_type=float),
+                        'self_crop_max_x': ParameterValue(self_crop_max_x, value_type=float),
+                        'self_crop_min_y': ParameterValue(self_crop_min_y, value_type=float),
+                        'self_crop_max_y': ParameterValue(self_crop_max_y, value_type=float),
+                        'self_crop_min_z': ParameterValue(self_crop_min_z, value_type=float),
+                        'self_crop_max_z': ParameterValue(self_crop_max_z, value_type=float)
+                    }],
+                    remappings=[('~/input/pointcloud', '/sensing/lidar/top/converted/pointcloud'),
+                                ('~/output/localization/pointcloud', '/sensing/lidar/top/localization/pointcloud'),
+                                ('~/output/perception/pointcloud', '/sensing/lidar/top/perception/pointcloud')]
+                )
+            ],
+            output='screen',
+        )
+    ])
@@ -0,0 +1,26 @@
+<?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>pointcloud_process</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>pcl_ros</depend>
+  <depend>pcl_conversions</depend>
+  
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,444 @@
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <algorithm>
+#include <cmath>
+#include "rclcpp_components/register_node_macro.hpp" 
+#include "pointcloud_process/pointcloud_converter_component.hpp"
+
+XyzirtToXyzircaedtNode::XyzirtToXyzircaedtNode(const rclcpp::NodeOptions & options)
+: Node("xyzirt_to_xyzircaedt_node", options)
+{
+  this->declare_parameter("lidar_type", "pandar");
+  this->declare_parameter("enable_fastlio", false);
+  this->declare_parameter("enable_liorf", false);
+  this->get_parameter("lidar_type", lidar_type_);
+  this->get_parameter("enable_fastlio", enable_fastlio_);
+  this->get_parameter("enable_liorf", enable_liorf_);
+
+  cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = cb_group_;
+
+  if (lidar_type_ == "timoo") {
+    sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
+        "~/input/pointcloud", 10,
+        std::bind(&XyzirtToXyzircaedtNode::timooCloudCb, this, std::placeholders::_1),
+        sub_opt);
+  } else if (lidar_type_ == "rslidar" || lidar_type_ == "robosense") {
+    sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
+        "~/input/pointcloud", 10,
+        std::bind(&XyzirtToXyzircaedtNode::rslidarCloudCb, this, std::placeholders::_1),
+        sub_opt);
+  } else {
+    sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
+        "~/input/pointcloud", 10,
+        std::bind(&XyzirtToXyzircaedtNode::pandarCloudCb, this, std::placeholders::_1),
+        sub_opt);
+  }
+
+  pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(
+      "~/output/pointcloud", rclcpp::SensorDataQoS());
+  if (enable_fastlio_) {
+    pub_fastlio_ = create_publisher<sensor_msgs::msg::PointCloud2>(
+        "~/output/fastlio_pointcloud", rclcpp::SensorDataQoS());
+  }
+  if (enable_liorf_) {
+    pub_liorf_ = create_publisher<sensor_msgs::msg::PointCloud2>(
+        "~/output/liorf_pointcloud", rclcpp::SensorDataQoS());
+  }
+}
+
+bool XyzirtToXyzircaedtNode::hasField(
+  const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name) const
+{
+  for (const auto & field : cloud.fields) {
+    if (field.name == name) {
+      return true;
+    }
+  }
+  return false;
+}
+
+int XyzirtToXyzircaedtNode::addPointField(
+  sensor_msgs::msg::PointCloud2 & cloud, const std::string & name, 
+  int count, int datatype, int offset) const 
+{
+  sensor_msgs::msg::PointField field;
+  field.name = name;
+  field.count = count;
+  field.datatype = datatype;
+  field.offset = offset;
+  cloud.fields.push_back(field);
+  return offset + (count * (datatype == sensor_msgs::msg::PointField::FLOAT32 ? 4 : 
+                          datatype == sensor_msgs::msg::PointField::UINT16 ? 2 : 1));
+}
+
+void XyzirtToXyzircaedtNode::pandarCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const
+{
+  auto out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  out->header = msg->header;
+  out->width  = msg->width;
+  out->height = msg->height;
+  out->is_dense = msg->is_dense;
+
+  int offset = 0;
+  offset = addPointField(*out, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "intensity", 1, sensor_msgs::msg::PointField::UINT8, offset);
+  offset = addPointField(*out, "return_type", 1, sensor_msgs::msg::PointField::UINT8, offset);
+  offset = addPointField(*out, "channel", 1, sensor_msgs::msg::PointField::UINT16, offset);
+  offset = addPointField(*out, "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "elevation", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "time_stamp", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  
+  out->point_step = offset;
+  out->row_step   = out->width * out->point_step;
+  out->data.resize(out->row_step * out->height);
+  
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
+  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_intensity(*msg, "intensity");
+  sensor_msgs::PointCloud2ConstIterator<double> iter_time(*msg, "timestamp");
+
+  sensor_msgs::PointCloud2Iterator<float> out_x(*out, "x");
+  sensor_msgs::PointCloud2Iterator<float> out_y(*out, "y");
+  sensor_msgs::PointCloud2Iterator<float> out_z(*out, "z");
+  sensor_msgs::PointCloud2Iterator<uint8_t> out_intensity(*out, "intensity");
+  sensor_msgs::PointCloud2Iterator<float> out_distance(*out, "distance");
+  sensor_msgs::PointCloud2Iterator<float> out_azimuth(*out, "azimuth");
+  sensor_msgs::PointCloud2Iterator<float> out_elevation(*out, "elevation");
+  sensor_msgs::PointCloud2Iterator<float> out_time(*out, "time_stamp");
+
+  double frame_start_ts = iter_time[0];
+
+  for (size_t i = 0; i < out->width * out->height; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_time,
+       ++out_x, ++out_y, ++out_z, ++out_intensity, ++out_distance, ++out_azimuth, ++out_elevation, ++out_time)
+  {
+    *out_x = *iter_x;
+    *out_y = *iter_y;
+    *out_z = *iter_z;
+    *out_intensity = static_cast<uint8_t>(*iter_intensity);
+    
+    float dist = std::sqrt((*iter_x) * (*iter_x) + (*iter_y) * (*iter_y) + (*iter_z) * (*iter_z));
+    *out_distance = dist;
+    *out_azimuth  = std::atan2((*iter_y), (*iter_x));
+    *out_elevation = std::asin((*iter_z) / (dist + 1e-6f));
+    *out_time = static_cast<float>(*iter_time - frame_start_ts);
+  }
+
+  pub_->publish(std::move(out));
+
+  if (enable_fastlio_) {
+    fastlioCloudCb(msg);
+  }
+  if (enable_liorf_) {
+    liorfCloudCb(msg);
+  }
+}
+
+void XyzirtToXyzircaedtNode::timooCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const
+{
+  auto out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  out->header = msg->header;
+  out->width  = msg->width;
+  out->height = msg->height;
+  out->is_dense = msg->is_dense;
+
+  int offset = 0;
+  offset = addPointField(*out, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "intensity", 1, sensor_msgs::msg::PointField::UINT8, offset);
+  offset = addPointField(*out, "return_type", 1, sensor_msgs::msg::PointField::UINT8, offset);
+  offset = addPointField(*out, "channel", 1, sensor_msgs::msg::PointField::UINT16, offset);
+  offset = addPointField(*out, "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "elevation", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "time_stamp", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+
+  out->point_step = offset;
+  out->row_step   = out->width * out->point_step;
+  out->data.resize(out->row_step * out->height);
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_intensity(*msg, "intensity");
+  sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_ring(*msg, "ring");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_time(*msg, "time");
+
+  sensor_msgs::PointCloud2Iterator<float> out_x(*out, "x");
+  sensor_msgs::PointCloud2Iterator<float> out_y(*out, "y");
+  sensor_msgs::PointCloud2Iterator<float> out_z(*out, "z");
+  sensor_msgs::PointCloud2Iterator<uint8_t> out_intensity(*out, "intensity");
+  sensor_msgs::PointCloud2Iterator<uint8_t> out_return_type(*out, "return_type");
+  sensor_msgs::PointCloud2Iterator<uint16_t> out_channel(*out, "channel");
+  sensor_msgs::PointCloud2Iterator<float> out_azimuth(*out, "azimuth");
+  sensor_msgs::PointCloud2Iterator<float> out_elevation(*out, "elevation");
+  sensor_msgs::PointCloud2Iterator<float> out_distance(*out, "distance");
+  sensor_msgs::PointCloud2Iterator<float> out_time(*out, "time_stamp");
+
+  double frame_start_ts = static_cast<double>(iter_time[0]);
+
+  for (size_t i = 0; i < out->width * out->height; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_ring, ++iter_time,
+       ++out_x, ++out_y, ++out_z, ++out_intensity, ++out_return_type, ++out_channel, ++out_azimuth, ++out_elevation, ++out_distance, ++out_time)
+  {
+    *out_x = *iter_x;
+    *out_y = *iter_y;
+    *out_z = *iter_z;
+    *out_intensity = static_cast<uint8_t>(*iter_intensity);
+    *out_return_type = 0;
+    *out_channel = *iter_ring;
+
+    float dist = std::sqrt((*iter_x) * (*iter_x) + (*iter_y) * (*iter_y) + (*iter_z) * (*iter_z));
+    *out_distance = dist;
+    *out_azimuth  = std::atan2((*iter_y), (*iter_x));
+    *out_elevation = std::asin((*iter_z) / (dist + 1e-6f));
+    *out_time = static_cast<float>(static_cast<double>(*iter_time) - frame_start_ts);
+  }
+
+  pub_->publish(std::move(out));
+
+  if (enable_fastlio_) {
+    fastlioCloudCb(msg);
+  }
+  if (enable_liorf_) {
+    liorfCloudCb(msg);
+  }
+}
+
+void XyzirtToXyzircaedtNode::rslidarCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const
+{
+  const char * required_fields[] = {"x", "y", "z", "intensity", "ring", "timestamp"};
+  for (const auto * field : required_fields) {
+    if (!hasField(*msg, field)) {
+      RCLCPP_ERROR(
+        get_logger(),
+        "rslidar pointcloud is missing required field '%s'. Build rslidar_sdk with POINT_TYPE XYZIRT.",
+        field);
+      return;
+    }
+  }
+
+  auto out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  out->header = msg->header;
+  out->width  = msg->width;
+  out->height = msg->height;
+  out->is_dense = msg->is_dense;
+
+  int offset = 0;
+  offset = addPointField(*out, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "intensity", 1, sensor_msgs::msg::PointField::UINT8, offset);
+  offset = addPointField(*out, "return_type", 1, sensor_msgs::msg::PointField::UINT8, offset);
+  offset = addPointField(*out, "channel", 1, sensor_msgs::msg::PointField::UINT16, offset);
+  offset = addPointField(*out, "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "elevation", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "time_stamp", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+
+  out->point_step = offset;
+  out->row_step   = out->width * out->point_step;
+  out->data.resize(out->row_step * out->height);
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_intensity(*msg, "intensity");
+  sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_ring(*msg, "ring");
+  sensor_msgs::PointCloud2ConstIterator<double> iter_timestamp(*msg, "timestamp");
+
+  sensor_msgs::PointCloud2Iterator<float> out_x(*out, "x");
+  sensor_msgs::PointCloud2Iterator<float> out_y(*out, "y");
+  sensor_msgs::PointCloud2Iterator<float> out_z(*out, "z");
+  sensor_msgs::PointCloud2Iterator<uint8_t> out_intensity(*out, "intensity");
+  sensor_msgs::PointCloud2Iterator<uint8_t> out_return_type(*out, "return_type");
+  sensor_msgs::PointCloud2Iterator<uint16_t> out_channel(*out, "channel");
+  sensor_msgs::PointCloud2Iterator<float> out_azimuth(*out, "azimuth");
+  sensor_msgs::PointCloud2Iterator<float> out_elevation(*out, "elevation");
+  sensor_msgs::PointCloud2Iterator<float> out_distance(*out, "distance");
+  sensor_msgs::PointCloud2Iterator<float> out_time(*out, "time_stamp");
+
+  const double frame_start_ts = iter_timestamp[0];
+
+  for (size_t i = 0; i < out->width * out->height;
+       ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_ring, ++iter_timestamp,
+       ++out_x, ++out_y, ++out_z, ++out_intensity, ++out_return_type, ++out_channel,
+       ++out_azimuth, ++out_elevation, ++out_distance, ++out_time) {
+    *out_x = *iter_x;
+    *out_y = *iter_y;
+    *out_z = *iter_z;
+    *out_intensity = static_cast<uint8_t>(
+      std::max(0.0F, std::min(255.0F, *iter_intensity)));
+    *out_return_type = 0;
+    *out_channel = *iter_ring;
+
+    const float dist = std::sqrt((*iter_x) * (*iter_x) + (*iter_y) * (*iter_y) + (*iter_z) * (*iter_z));
+    *out_distance = dist;
+    *out_azimuth = std::atan2((*iter_y), (*iter_x));
+    *out_elevation = std::asin((*iter_z) / (dist + 1e-6f));
+    *out_time = static_cast<float>(*iter_timestamp - frame_start_ts);
+  }
+
+  pub_->publish(std::move(out));
+
+  if (enable_fastlio_) {
+    fastlioCloudCb(msg);
+  }
+  if (enable_liorf_) {
+    liorfCloudCb(msg);
+  }
+}
+
+void XyzirtToXyzircaedtNode::fastlioCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const
+{
+  auto out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  out->header = msg->header;
+  out->width  = msg->width;
+  out->height = msg->height;
+  out->is_dense = msg->is_dense;
+
+  int offset = 0;
+  offset = addPointField(*out, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "time", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "ring", 1, sensor_msgs::msg::PointField::UINT8, offset);
+
+  out->point_step = offset;
+  out->row_step   = out->width * out->point_step;
+  out->data.resize(out->row_step * out->height);
+
+  bool has_time_field = false;
+  for (const auto& field : msg->fields) {
+    if (field.name == "time") {
+      has_time_field = true;
+      break;
+    }
+  }
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_intensity(*msg, "intensity");
+  sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_ring(*msg, "ring");
+
+  sensor_msgs::PointCloud2Iterator<float> out_x(*out, "x");
+  sensor_msgs::PointCloud2Iterator<float> out_y(*out, "y");
+  sensor_msgs::PointCloud2Iterator<float> out_z(*out, "z");
+  sensor_msgs::PointCloud2Iterator<float> out_intensity(*out, "intensity");
+  sensor_msgs::PointCloud2Iterator<float> out_time(*out, "time");
+  sensor_msgs::PointCloud2Iterator<uint8_t> out_ring(*out, "ring");
+
+  if (has_time_field) {
+    sensor_msgs::PointCloud2ConstIterator<float> iter_time(*msg, "time");
+    double frame_start_ts = static_cast<double>(iter_time[0]);
+
+    for (size_t i = 0; i < out->width * out->height; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_ring, ++iter_time,
+         ++out_x, ++out_y, ++out_z, ++out_intensity, ++out_time, ++out_ring)
+    {
+      *out_x = *iter_x;
+      *out_y = *iter_y;
+      *out_z = *iter_z;
+      *out_intensity = *iter_intensity;
+      *out_ring = static_cast<uint8_t>(*iter_ring);
+      *out_time = static_cast<float>(static_cast<double>(*iter_time) - frame_start_ts);
+    }
+  } else {
+    sensor_msgs::PointCloud2ConstIterator<double> iter_timestamp(*msg, "timestamp");
+    double frame_start_ts = iter_timestamp[0];
+
+    for (size_t i = 0; i < out->width * out->height; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_ring, ++iter_timestamp,
+         ++out_x, ++out_y, ++out_z, ++out_intensity, ++out_time, ++out_ring)
+    {
+      *out_x = *iter_x;
+      *out_y = *iter_y;
+      *out_z = *iter_z;
+      *out_intensity = *iter_intensity;
+      *out_ring = static_cast<uint8_t>(*iter_ring);
+      *out_time = static_cast<float>(*iter_timestamp - frame_start_ts);
+    }
+  }
+
+  pub_fastlio_->publish(std::move(out));
+}
+
+void XyzirtToXyzircaedtNode::liorfCloudCb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) const
+{
+  auto out = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  out->header = msg->header;
+  out->width  = msg->width;
+  out->height = msg->height;
+  out->is_dense = msg->is_dense;
+
+  int offset = 0;
+  offset = addPointField(*out, "x", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "y", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "z", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+  offset = addPointField(*out, "ring", 1, sensor_msgs::msg::PointField::UINT16, offset);
+  offset = addPointField(*out, "time", 1, sensor_msgs::msg::PointField::FLOAT32, offset);
+
+  out->point_step = offset;
+  out->row_step   = out->width * out->point_step;
+  out->data.resize(out->row_step * out->height);
+
+  bool has_time_field = false;
+  for (const auto& field : msg->fields) {
+    if (field.name == "time") {
+      has_time_field = true;
+      break;
+    }
+  }
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_intensity(*msg, "intensity");
+  sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_ring(*msg, "ring");
+
+  sensor_msgs::PointCloud2Iterator<float> out_x(*out, "x");
+  sensor_msgs::PointCloud2Iterator<float> out_y(*out, "y");
+  sensor_msgs::PointCloud2Iterator<float> out_z(*out, "z");
+  sensor_msgs::PointCloud2Iterator<float> out_intensity(*out, "intensity");
+  sensor_msgs::PointCloud2Iterator<uint16_t> out_ring(*out, "ring");
+  sensor_msgs::PointCloud2Iterator<float> out_time(*out, "time");
+
+  if (has_time_field) {
+    sensor_msgs::PointCloud2ConstIterator<float> iter_time(*msg, "time");
+    double frame_start_ts = static_cast<double>(iter_time[0]);
+
+    for (size_t i = 0; i < out->width * out->height; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_ring, ++iter_time,
+         ++out_x, ++out_y, ++out_z, ++out_intensity, ++out_ring, ++out_time)
+    {
+      *out_x = *iter_x;
+      *out_y = *iter_y;
+      *out_z = *iter_z;
+      *out_intensity = *iter_intensity;
+      *out_ring = *iter_ring;
+      *out_time = static_cast<float>(static_cast<double>(*iter_time) - frame_start_ts);
+    }
+  } else {
+    sensor_msgs::PointCloud2ConstIterator<double> iter_timestamp(*msg, "timestamp");
+    double frame_start_ts = iter_timestamp[0];
+
+    for (size_t i = 0; i < out->width * out->height; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_ring, ++iter_timestamp,
+         ++out_x, ++out_y, ++out_z, ++out_intensity, ++out_ring, ++out_time)
+    {
+      *out_x = *iter_x;
+      *out_y = *iter_y;
+      *out_z = *iter_z;
+      *out_intensity = *iter_intensity;
+      *out_ring = *iter_ring;
+      *out_time = static_cast<float>(*iter_timestamp - frame_start_ts);
+    }
+  }
+
+  pub_liorf_->publish(std::move(out));
+}
+
+RCLCPP_COMPONENTS_REGISTER_NODE(XyzirtToXyzircaedtNode)
@@ -0,0 +1,157 @@
+#include "pointcloud_process/pointcloud_transform.hpp"
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <pcl_ros/transforms.hpp>
+#include <utility>
+#include "rclcpp_components/register_node_macro.hpp"
+
+PointCloudInterface::PointCloudInterface(const rclcpp::NodeOptions & node_options)
+: Node("carla_pointcloud_interface_node", node_options)
+{
+    tf_output_frame_ = this->declare_parameter<std::string>("tf_output_frame", "base_link");
+    enable_self_crop_ = this->declare_parameter<bool>("enable_self_crop", true);
+    self_crop_apply_to_localization_ =
+        this->declare_parameter<bool>("self_crop_apply_to_localization", true);
+    self_crop_box_.min_x = this->declare_parameter<double>("self_crop_min_x", 1.00);
+    self_crop_box_.max_x = this->declare_parameter<double>("self_crop_max_x", 1.70);
+    self_crop_box_.min_y = this->declare_parameter<double>("self_crop_min_y", -0.65);
+    self_crop_box_.max_y = this->declare_parameter<double>("self_crop_max_y", -0.05);
+    self_crop_box_.min_z = this->declare_parameter<double>("self_crop_min_z", 1.10);
+    self_crop_box_.max_z = this->declare_parameter<double>("self_crop_max_z", 2.05);
+
+    cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+    auto sub_opt = rclcpp::SubscriptionOptions();
+    sub_opt.callback_group = cb_group_;
+
+    carla_cloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+        "~/input/pointcloud", rclcpp::SensorDataQoS(),
+        std::bind(&PointCloudInterface::processScan, this, std::placeholders::_1),
+        sub_opt);
+
+    setupTF();
+
+    pandar_points_localization = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+        "~/output/localization/pointcloud", rclcpp::SensorDataQoS());
+    pandar_points_perception = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+        "~/output/perception/pointcloud", rclcpp::SensorDataQoS());
+}
+
+void PointCloudInterface::setupTF()
+{
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+}
+
+void PointCloudInterface::processScan(const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
+{
+    auto transformed_cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    if (pcl_ros::transformPointCloud(tf_output_frame_, *scanMsg, *transformed_cloud, *tf_buffer_))
+    {
+        transformed_cloud->header.stamp = scanMsg->header.stamp;
+
+        auto perception_msg = enable_self_crop_
+            ? removeSelfPoints(*transformed_cloud)
+            : std::make_unique<sensor_msgs::msg::PointCloud2>(*transformed_cloud);
+
+        if (enable_self_crop_ && self_crop_apply_to_localization_) {
+            auto localization_msg = removeSelfPoints(*transformed_cloud);
+            pandar_points_localization->publish(std::move(localization_msg));
+        } else {
+            pandar_points_localization->publish(std::move(transformed_cloud));
+        }
+
+        pandar_points_perception->publish(std::move(perception_msg));
+    }
+}
+
+bool PointCloudInterface::getFieldOffset(
+    const sensor_msgs::msg::PointCloud2 & cloud, const std::string & field_name,
+    uint32_t & offset) const
+{
+    for (const auto & field : cloud.fields) {
+        if (field.name == field_name && field.datatype == sensor_msgs::msg::PointField::FLOAT32) {
+            offset = field.offset;
+            return true;
+        }
+    }
+    return false;
+}
+
+std::unique_ptr<sensor_msgs::msg::PointCloud2> PointCloudInterface::removeSelfPoints(
+    const sensor_msgs::msg::PointCloud2 & input)
+{
+    uint32_t x_offset = 0;
+    uint32_t y_offset = 0;
+    uint32_t z_offset = 0;
+    if (!getFieldOffset(input, "x", x_offset) || !getFieldOffset(input, "y", y_offset) ||
+        !getFieldOffset(input, "z", z_offset)) {
+        RCLCPP_WARN_THROTTLE(
+            get_logger(), *get_clock(), 5000,
+            "Self crop skipped because PointCloud2 does not have FLOAT32 x/y/z fields");
+        return std::make_unique<sensor_msgs::msg::PointCloud2>(input);
+    }
+
+    const size_t point_count = static_cast<size_t>(input.width) * static_cast<size_t>(input.height);
+    const uint32_t max_field_end =
+        std::max(std::max(x_offset, y_offset), z_offset) + static_cast<uint32_t>(sizeof(float));
+    const size_t expected_size = static_cast<size_t>(input.row_step) * input.height;
+    if (input.point_step == 0 || input.row_step < input.width * input.point_step ||
+        max_field_end > input.point_step || input.data.size() < expected_size) {
+        RCLCPP_WARN_THROTTLE(
+            get_logger(), *get_clock(), 5000,
+            "Self crop skipped because PointCloud2 data layout is invalid");
+        return std::make_unique<sensor_msgs::msg::PointCloud2>(input);
+    }
+
+    auto output = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    output->header = input.header;
+    output->fields = input.fields;
+    output->is_bigendian = input.is_bigendian;
+    output->point_step = input.point_step;
+    output->is_dense = input.is_dense;
+    output->height = 1;
+    output->data.resize(input.data.size());
+
+    size_t output_size = 0;
+    size_t removed_count = 0;
+    for (uint32_t row = 0; row < input.height; ++row) {
+        const size_t row_offset = static_cast<size_t>(row) * input.row_step;
+        for (uint32_t col = 0; col < input.width; ++col) {
+            const size_t input_offset = row_offset + static_cast<size_t>(col) * input.point_step;
+            float x = 0.0F;
+            float y = 0.0F;
+            float z = 0.0F;
+            std::memcpy(&x, &input.data[input_offset + x_offset], sizeof(float));
+            std::memcpy(&y, &input.data[input_offset + y_offset], sizeof(float));
+            std::memcpy(&z, &input.data[input_offset + z_offset], sizeof(float));
+
+            const bool inside_self_box =
+                std::isfinite(x) && std::isfinite(y) && std::isfinite(z) &&
+                x >= self_crop_box_.min_x && x <= self_crop_box_.max_x &&
+                y >= self_crop_box_.min_y && y <= self_crop_box_.max_y &&
+                z >= self_crop_box_.min_z && z <= self_crop_box_.max_z;
+            if (inside_self_box) {
+                ++removed_count;
+                continue;
+            }
+
+            std::memcpy(&output->data[output_size], &input.data[input_offset], input.point_step);
+            output_size += input.point_step;
+        }
+    }
+
+    output->data.resize(output_size);
+    output->width = static_cast<uint32_t>(output_size / output->point_step);
+    output->row_step = static_cast<uint32_t>(output->data.size());
+
+    RCLCPP_DEBUG_THROTTLE(
+        get_logger(), *get_clock(), 5000, "Self crop removed %zu/%zu points", removed_count,
+        point_count);
+
+    return output;
+}
+
+PointCloudInterface::~PointCloudInterface() {}
+
+RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudInterface)
@@ -0,0 +1,115 @@
+cmake_minimum_required(VERSION 3.8)
+project(timoo_ros2_driver VERSION 0.0.1 LANGUAGES CXX C)
+
+# Default to C++17 (组件需要)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS on)
+
+# Fix MPI/VTK issues by setting policies
+if(POLICY CMP0144)
+  cmake_policy(SET CMP0144 NEW)
+endif()
+
+# Find MPI first with proper configuration
+find_package(MPI REQUIRED)
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(PCL REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(driver_timestamp_sync REQUIRED)
+
+#=======================================
+# Custom Point Type (XYZI,XYZIRT)
+#=======================================
+set(POINT_TYPE XYZIRT)
+
+#========================
+# Point Type Definition
+#========================
+if(${POINT_TYPE} STREQUAL "XYZI")
+  add_definitions(-DPOINT_TYPE_XYZI)
+  message(STATUS "Point type is XYZI")
+elseif(${POINT_TYPE} STREQUAL "XYZIRT")
+  add_definitions(-DPOINT_TYPE_XYZIRT)
+  message(STATUS "Point type is XYZIRT")
+endif()
+
+# Include directories
+include_directories(
+  include
+  ${PCL_INCLUDE_DIRS}
+  ${YAML_CPP_INCLUDE_DIR}
+  /opt/timoo_lidar_driver/include
+)
+
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+#===============================
+# Create component library (ROS2插件)
+#===============================
+add_library(${PROJECT_NAME} SHARED
+  src/timoo_driver_node.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  rclcpp_components
+  std_msgs
+  sensor_msgs
+  pcl_conversions
+  driver_timestamp_sync
+)
+
+target_link_libraries(${PROJECT_NAME}
+  yaml-cpp
+  ${PCL_LIBRARIES}
+  /opt/timoo_lidar_driver/lib/libtimoo_driver.so
+  /opt/timoo_lidar_driver/lib/libtimoo_input.so
+  /opt/timoo_lidar_driver/lib/libtimoo_data_parser.so
+  /opt/timoo_lidar_driver/lib/libtimoo_func.so
+)
+
+# Register component (ROS2插件注册)
+rclcpp_components_register_nodes(${PROJECT_NAME} "timoo_ros2_driver::TimooDriverNode")
+
+#===============================
+# Install
+#===============================
+install(TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(export_${PROJECT_NAME})
+
+ament_package()
@@ -0,0 +1,25 @@
+
+# timoo  ROS2驱动
+
+#### 依赖
+使用前需参照[TimooLidarDriveSDK](http://192.168.9.120/timoo-software/timoolidardriversdk) 对驱动库进行安装
+
+#### 操作方式
+
+```bash 
+# 激活环境
+source /opt/ros/eloquent/setup.bash
+# 使用SDk
+export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:/opt/timoo_lidar_driver
+# 编译
+colcon build
+# 启动驱动（3雷达驱动）
+source install/setup.bash && ros2 launch timoo_ros2_driver multi_lidar.launch.py 
+# 启动驱动（单雷达驱动）
+source install/setup.bash && ros2 launch timoo_ros2_driver timoo.launch.py
+
+#另开终端激活环境
+source /opt/ros/eloquent/setup.bash
+# 启动rviz
+rviz2 -f timoo
+```
@@ -0,0 +1,49 @@
+# Timoo Lidar Driver Parameters
+# 从该YAML文件加载所有驱动参数
+
+/**:
+  ros__parameters:
+    # ============================================
+    # LiDAR连接参数
+    # ============================================
+    udp_port: 2368                # LiDAR数据UDP端口
+    status_port: 8603           # 设备状态UDP端口
+    imu_port: 65535              # IMU数据UDP端口
+    host_ip: "192.168.1.106"     # 本机IP地址
+
+    # ============================================
+    # LiDAR型号配置
+    # 可选值: TIMOO16, TIMOO32, TIMOO1550, TIMOO1550STD, TIMOO128
+    # ============================================
+    lidar_type: "TIMOO32"
+
+    # ============================================
+    # 点云处理参数
+    # ============================================
+    cut_angle: -1.0              # 分帧角度（弧度），-0.1表示自动分帧
+    fixed_points_count: false     # 是否使用固定点数量
+    remove_invalid_points: true # 是否移除无效点
+
+    # ============================================
+    # 距离过滤参数
+    # ============================================
+    min_distance: 0.2            # 最小探测距离(m)
+    max_distance: 150.0          # 最大探测距离(m)
+
+    # ============================================
+    # 话题配置
+    # ============================================
+    # 点云话题
+    points_topic: "/timoo/top/pointcloud"  # 点云发布话题名称
+    points_frame_id: "lidar_top"               # 点云消息的frame_id
+
+    # IMU话题
+    imu_topic: "/timoo/top/imu"                   # IMU发布话题名称
+    imu_frame_id: "imu_top"               # IMU消息的frame_id
+
+    # ============================================
+    # GNSS时间同步参数
+    # ============================================
+    use_gnss_time_sync: false                # 是否启用GNSS时间同步
+    use_packet_timestamp: false              # false: 点云header使用ROS接收时间，避免未同步雷达包时间导致TF外推
+    gnss_time_ref_topic: "/nmea_navsat_driver/time_reference"  # GNSS时间参考话题
@@ -0,0 +1,109 @@
+// Copyright 2026 FaRui
+
+#ifndef TIMOO_ROS2_DRIVER__TIMOO_DRIVER_NODE_HPP_
+#define TIMOO_ROS2_DRIVER__TIMOO_DRIVER_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/time_reference.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/io/pcd_io.h>
+#include "driver_timestamp_sync/gnss_time_anchor.hpp"
+
+// Timoo SDK forward declarations
+namespace timoo {
+namespace driver {
+class TimooDriver;
+namespace base {
+struct TimooPointCloud;
+struct IMUData;
+using TimooPointCloudPtr = std::shared_ptr<TimooPointCloud>;
+using TimooIMUPtr = std::shared_ptr<IMUData>;
+}  // namespace base
+}  // namespace driver
+}  // namespace timoo
+
+namespace timoo_ros2_driver
+{
+
+// 自定义点类型定义
+struct TimooPointXYZIRT {
+  PCL_ADD_POINT4D;
+  float intensity;
+  uint16_t ring = 0;
+  float time = 0.0F;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+class TimooDriverNode : public rclcpp::Node
+{
+public:
+  explicit TimooDriverNode(const rclcpp::NodeOptions & options);
+  ~TimooDriverNode();
+
+private:
+  // 参数声明
+  void declare_parameters();
+  void load_parameters();
+
+  // GNSS时间同步设置
+  void setup_gnss_time_sync();
+
+  // 获取时间戳（考虑GNSS同步）
+  rclcpp::Time get_timestamp();
+  rclcpp::Time get_pointcloud_timestamp(double packet_timestamp_sec) const;
+  rclcpp::Time to_ros_time(double timestamp_sec) const;
+
+  // 回调函数
+  void on_point_cloud(const timoo::driver::base::TimooPointCloudPtr & points_data);
+  void on_imu_data(const timoo::driver::base::TimooIMUPtr & imu_data);
+  void on_gnss_time_ref(const sensor_msgs::msg::TimeReference::SharedPtr msg);
+
+  // 点云处理模板函数
+  template <typename PointT>
+  void process_point_cloud(const timoo::driver::base::TimooPointCloudPtr & points_data);
+
+  // 参数
+  struct {
+    double cut_angle = 360.0;
+    int udp_port = 2368;
+    int status_port = 8603;
+    int imu_port = 7788;
+    bool fixed_points_count = false;
+    bool remove_invalid_points = false;
+    std::string lidar_type = "TIMOO32";
+    std::string host_ip = "192.168.1.106";
+    double min_distance = 0.2;
+    double max_distance = 150.0;
+    double min_angle = 0.0;
+    double max_angle = 360.0;
+    std::string points_topic = "/timoo/top/pointcloud";
+    std::string points_frame_id = "timoo";
+    std::string imu_topic = "imu_data";
+    std::string imu_frame_id = "lidar_top";
+    bool use_gnss_time_sync = false;
+    bool use_packet_timestamp = true;
+    std::string gnss_time_ref_topic = "/nmea_navsat_driver/time_reference";
+  } params_;
+
+  // 发布者
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+
+  // GNSS时间同步
+  std::shared_ptr<driver_timestamp_sync::GnssTimeAnchor> gnss_anchor_;
+  rclcpp::Subscription<sensor_msgs::msg::TimeReference>::SharedPtr gnss_time_sub_;
+
+  // Timoo驱动
+  std::unique_ptr<timoo::driver::TimooDriver> timoo_driver_;
+
+  // 运行状态
+  bool is_running_ = false;
+};
+
+}  // namespace timoo_ros2_driver
+
+#endif  // TIMOO_ROS2_DRIVER__TIMOO_DRIVER_NODE_HPP_
@@ -0,0 +1,86 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, TimerAction
+from launch.substitutions import LaunchConfiguration
+import os
+
+def generate_launch_description():
+    os.environ['LD_LIBRARY_PATH'] = f"/opt/timoo_lidar_driver/lib:{os.environ.get('LD_LIBRARY_PATH', '')}"
+    
+    return LaunchDescription([
+        # 雷达1 - 立即启动
+        Node(
+            package='timoo_ros2_driver',
+            node_executable='tm_ros_driver_node',
+            node_name='tm_ros_driver_node',
+            node_namespace='lidar1',
+            output='screen',
+            parameters=[{
+                'cut_angle': -0.1,
+                'min_distance': 0.2,
+                'max_distance': 150.0,
+                'udp_port': 2368,
+                'status_port': 8603,
+                'imu_port': 65535,
+                'fixed_points_count': True,
+                'remove_invalid_points': False,
+                'lidar_type': 'TIMOO16',
+                'host_ip': '192.168.1.106',
+                'points_topic': 'timoo_points',
+            }]
+        ),
+        
+        # 雷达2 - 延迟2秒启动
+        TimerAction(
+            period=2.0,
+            actions=[
+                Node(
+                    package='timoo_ros2_driver',
+                    node_executable='tm_ros_driver_node',
+                    node_name='tm_ros_driver_node',
+                    node_namespace='lidar2',
+                    output='screen',
+                    parameters=[{
+                        'cut_angle': -0.1,
+                        'min_distance': 0.2,
+                        'max_distance': 150.0,
+                        'udp_port': 2369,
+                        'status_port': 8604,
+                        'imu_port': 65536,
+                        'fixed_points_count': True,
+                        'remove_invalid_points': False,
+                        'lidar_type': 'TIMOO16',
+                        'host_ip': '192.168.1.106',
+                        'points_topic': 'timoo_points',
+                    }]
+                )
+            ]
+        ),
+        
+        # 雷达3 - 延迟4秒启动
+        TimerAction(
+            period=4.0,
+            actions=[
+                Node(
+                    package='timoo_ros2_driver',
+                    node_executable='tm_ros_driver_node',
+                    node_name='tm_ros_driver_node',
+                    node_namespace='lidar3',
+                    output='screen',
+                    parameters=[{
+                        'cut_angle': -0.1,
+                        'min_distance': 0.2,
+                        'max_distance': 150.0,
+                        'udp_port': 2370,
+                        'status_port': 8605,
+                        'imu_port': 6553,
+                        'fixed_points_count': True,
+                        'remove_invalid_points': False,
+                        'lidar_type': 'TIMOO16',
+                        'host_ip': '192.168.1.106',
+                        'points_topic': 'timoo_points',
+                    }]
+                )
+            ]
+        )
+    ])
@@ -0,0 +1,79 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('timoo_ros2_driver')
+    default_param_file = os.path.join(pkg_share, 'config', 'timoo_driver.param.yaml')
+
+    # 声明启动参数
+    param_file_arg = DeclareLaunchArgument(
+        'param_file',
+        default_value=default_param_file,
+        description='Path to the parameter file'
+    )
+
+    # 作为组件运行的选项
+    use_container_arg = DeclareLaunchArgument(
+        'use_container',
+        default_value='false',
+        description='Whether to run as a composable node in a container'
+    )
+
+    # 容器名称
+    container_name_arg = DeclareLaunchArgument(
+        'container_name',
+        default_value='timoo_container',
+        description='Name of the container node'
+    )
+
+    param_file = LaunchConfiguration('param_file')
+    use_container = LaunchConfiguration('use_container')
+    container_name = LaunchConfiguration('container_name')
+
+    # 方式1: 作为独立节点运行（传统方式）
+    standalone_node = Node(
+        package='timoo_ros2_driver',
+        executable='timoo_ros2_driver_node',
+        name='timoo_ros_driver_node',
+        output='screen',
+        parameters=[param_file],
+        condition=lambda context: not context.launch_configurations.get('use_container', 'false') == 'true'
+    )
+
+    # 方式2: 作为组件节点在容器中运行
+    # 注意：这需要与外部容器配合，或创建一个新的容器
+    composable_node = ComposableNode(
+        package='timoo_ros2_driver',
+        plugin='timoo_ros2_driver::TimooDriverNode',
+        name='timoo_ros_driver_node',
+        parameters=[param_file],
+        extra_arguments=[{'use_intra_process_comms': True}]
+    )
+
+    # 创建容器（当需要时使用）
+    container = ComposableNodeContainer(
+        name=container_name,
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[composable_node],
+        output='screen',
+        condition=lambda context: context.launch_configurations.get('use_container', 'false') == 'true'
+    )
+
+    return LaunchDescription([
+        param_file_arg,
+        use_container_arg,
+        container_name_arg,
+        # 默认使用独立节点
+        standalone_node,
+        # 如果使用容器，取消注释下面这行
+        # container,
+    ])
@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    """Generate launch description for Timoo Lidar Driver as a composable node."""
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('timoo_ros2_driver')
+    default_param_file = os.path.join(pkg_share, 'config', 'timoo_driver.param.yaml')
+
+    # 声明启动参数
+    param_file_arg = DeclareLaunchArgument(
+        'param_file',
+        default_value=default_param_file,
+        description='Path to the parameter file'
+    )
+
+    container_name_arg = DeclareLaunchArgument(
+        'container_name',
+        default_value='timoo_driver_container',
+        description='Name of the container node'
+    )
+
+    param_file = LaunchConfiguration('param_file')
+    container_name = LaunchConfiguration('container_name')
+
+    # 创建组件节点
+    timoo_composable_node = ComposableNode(
+        package='timoo_ros2_driver',
+        plugin='timoo_ros2_driver::TimooDriverNode',
+        name='timoo_ros_driver_node',
+        # 从YAML文件加载所有参数
+        parameters=[param_file],
+        extra_arguments=[{'use_intra_process_comms': True}]
+    )
+
+    # 创建容器
+    container = ComposableNodeContainer(
+        name=container_name,
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[timoo_composable_node],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        param_file_arg,
+        container_name_arg,
+        container,
+    ])
@@ -0,0 +1,51 @@
+from launch import LaunchDescription
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    """Generate launch description to load Timoo Driver into an existing container."""
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('timoo_ros2_driver')
+    default_param_file = os.path.join(pkg_share, 'config', 'timoo_driver.param.yaml')
+
+    # 声明启动参数
+    param_file_arg = DeclareLaunchArgument(
+        'param_file',
+        default_value=default_param_file,
+        description='Path to the parameter file'
+    )
+
+    container_name_arg = DeclareLaunchArgument(
+        'container_name',
+        default_value='sensing_container',
+        description='Name of the target container to load the node into'
+    )
+
+    param_file = LaunchConfiguration('param_file')
+    container_name = LaunchConfiguration('container_name')
+
+    # 创建加载组件的动作
+    load_composable_nodes = LoadComposableNodes(
+        target_container=container_name,
+        composable_node_descriptions=[
+            ComposableNode(
+                package='timoo_ros2_driver',
+                plugin='timoo_ros2_driver::TimooDriverNode',
+                name='timoo_ros_driver_node',
+                # 从YAML文件加载所有参数
+                parameters=[param_file],
+                extra_arguments=[{'use_intra_process_comms': True}]
+            ),
+        ],
+    )
+
+    return LaunchDescription([
+        param_file_arg,
+        container_name_arg,
+        load_composable_nodes,
+    ])
@@ -0,0 +1,23 @@
+<?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>timoo_ros2_driver</name>
+  <version>0.0.1</version>
+  <description>The timoo_ros2_driver package</description>
+  <maintainer email="gongjie@tm.com">tm</maintainer>
+  <license>TODO</license>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>driver_timestamp_sync</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/Show More
+++ b/Show More