add：添加了实车模式，未验证

src/LSLIDAR_X_ROS2-20240228/src/README.md

@@ -0,0 +1,44 @@
+# lslidar
+
+## Description
+The `lslidar package is a linux ROS2 driver for lslidar M10 ,M10_GPS,M10_P,M10_PLUS and N10.
+The package is tested on Ubuntu 20.04 with ROS2 indigo.
+
+## Compling
+This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compling a catkin package will work.
+
+```
+cd your_work_space
+colcon build
+source install/setup.bash
+ros2 launch lslidar_driver lslidar_launch.py
+```
+open new terminal
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ 		(open radar)
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ 		(close radar)
+
+
+
+
+
+ros2 launch lslidar_driver lslidar_launch.py
+
+```
+
+Note that this launch file launches both the driver, which is the only launch file needed to be used.
+
+
+## FAQ
+
+
+## Bug Report
+
+Prefer to open an issue. You can also send an E-mail to honghangli@lslidar.com
+
+
+
+
+RERTION 
+
+
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_driver)
+
+# 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()
+
+set(libpcap_LIBRARIES -lpcap)
+
+#set(FastRTPS_INCLUDE_DIR /opt/ros/foxy/include)
+#set(FastRTPS_LIBRARY_RELEASE /opt/ros/foxy/lib/libfastrtps.so)
+
+#find_package(Boost REQUIRED COMPONENTS )
+find_package(Boost REQUIRED thread)
+find_package(rclcpp REQUIRED)
+find_package(PCL REQUIRED)
+find_package(diagnostic_updater REQUIRED)
+find_package(lslidar_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(sensor_msgs REQUIRED)
+#find_package(PCL REQUIRED COMPONENTS common io)											   
+
+include_directories(
+		include
+    ${PCL_INCLUDE_DIRS}
+    ${PCL_COMMON_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+)
+
+# Node
+add_executable(lslidar_driver_node src/lslidar_driver_node.cc src/lslidar_driver.cc src/input.cc src/lsiosr.cpp)
+target_link_libraries(lslidar_driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES} Boost::thread)
+ament_target_dependencies(lslidar_driver_node rclcpp std_msgs lslidar_msgs sensor_msgs diagnostic_updater pcl_conversions)
+
+
+install(DIRECTORY launch params rviz
+		DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+  lslidar_driver_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_dependencies(rclcpp pluginlib lslidar_msgs sensor_msgs pcl_conversions)
+ament_export_include_directories(include ${PCL_COMMON_INCLUDE_DIRS})
+
+ament_package()

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h

@@ -0,0 +1,134 @@
+/*
+ * This file is part of lslidar_ch driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *     Input -- base class used to access the data independently of
+ *              its source
+ *
+ *     InputSocket -- derived class reads live data from the device
+ *              via a UDP socket
+ *
+ *     InputPCAP -- derived class provides a similar interface from a
+ *              PCAP dump
+ */
+
+#ifndef __LSLIDAR_INPUT_H_
+#define __LSLIDAR_INPUT_H_
+
+#include <unistd.h>
+#include <stdio.h>
+#include <pcap.h>
+#include <netinet/in.h>
+#include "rclcpp/rclcpp.hpp"
+#include <lslidar_msgs/msg/lslidar_packet.hpp>
+#include <string>
+#include <sstream>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+#include <signal.h>
+#include <sensor_msgs/msg/time_reference.hpp>
+#include <std_msgs/msg/int8.hpp>
+#include <cmath>
+
+namespace lslidar_driver
+{
+static uint16_t MSOP_DATA_PORT_NUMBER = 2368;   // lslidar default data port on PC
+/**
+ *  从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
+ * @brief The Input class,
+     *
+     * @param private_nh  一个NodeHandled,用于通过节点传递参数
+     * @param port
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          >0 if incomplete packet (is this possible?)
+ */
+class Input
+{
+public:
+  Input(rclcpp::Node* private_nh, uint16_t port);
+
+  virtual ~Input()
+  {
+  }
+
+  virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet) = 0;
+
+  int getRpm(void);
+  int getReturnMode(void);
+  bool getUpdateFlag(void);
+  void clearUpdateFlag(void);
+  void UDP_order(const std_msgs::msg::Int8 msg);
+  void UDP_difop();
+protected:
+  rclcpp::Node*  private_nh_;
+  uint16_t port_;
+  std::string devip_str_;
+  std::string lidar_name;
+  int cur_rpm_;
+  int return_mode_;
+  bool npkt_update_flag_;
+  bool add_multicast;
+  std::string group_ip;
+  int UDP_PORT_NUMBER_DIFOP;
+	int socket_id_difop;
+  int sockfd_;
+  std::string devip_str_difop;
+};
+
+/** @brief Live lslidar input from socket. */
+class InputSocket : public Input
+{
+public:
+  InputSocket(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
+
+  virtual ~InputSocket();
+
+  virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet);
+
+private:
+private:
+  
+  in_addr devip_;
+  in_addr devip_difop;
+  //struct ip_mreq group;
+
+};
+class InputPCAP : public Input
+{
+public:
+    InputPCAP(rclcpp::Node* private_nh,uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
+            std::string filename="");
+    virtual ~InputPCAP();
+    virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt);
+private:
+
+    rclcpp::Rate packet_rate_;
+    std::string filename_;
+    pcap_t *pcap_;
+    bpf_program pcap_packet_filter_;
+    char errbuf_[PCAP_ERRBUF_SIZE];
+    bool empty_;
+    bool read_once_;
+    bool read_fast_;
+    double repeat_delay_;
+  };
+}
+
+#endif  // __LSLIDAR_INPUT_H

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h

@@ -0,0 +1,88 @@
+/*******************************************************
+@company: Copyright (C) 2021, Leishen Intelligent System
+@product: LSM10_N10
+@filename: lsiosr.cpp
+@brief:
+@version:       date:       author:     comments:
+@v1.0           22-10-24      li          new
+*******************************************************/
+#ifndef LSIOSR_H
+#define LSIOSR_H
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <errno.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <fstream>
+#include <iostream>
+
+//波特率
+#define     BAUD_230400     230400
+#define     BAUD_460800     460800
+#define     BAUD_500000     500000
+#define     BAUD_921600     921600
+
+//奇偶校验位
+#define     PARITY_ODD    	'O' //奇数
+#define     PARITY_EVEN   	'E' //偶数
+#define     PARITY_NONE   	'N' //无奇偶校验位
+
+//停止位
+#define     STOP_BIT_1     	1
+#define     STOP_BIT_2     	2
+
+//数据位
+#define     DATA_BIT_7     	7
+#define     DATA_BIT_8     	8
+
+namespace lslidar_driver
+{
+class LSIOSR{
+public:
+  static LSIOSR* instance(std::string name, int speed, int fd = 0);
+
+  ~LSIOSR();
+
+  /* 从串口中读取数据 */
+  int read(unsigned char *buffer, int length, int timeout = 30);
+
+  /* 向串口传数据 */
+  int send(const char* buffer, int length, int timeout = 30);
+
+  /* Empty serial port input buffer */
+  void flushinput();
+
+  /* 串口初始化 */
+  int init();
+
+  int close();
+
+  /* 获取串口号 */
+  std::string getPort();
+
+  /* 设置串口号 */
+  int setPortName(std::string name);
+
+private:
+  LSIOSR(std::string name, int speed, int fd);
+
+  int waitWritable(int millis);
+  int waitReadable(int millis);
+
+  /* 串口配置的函数 */
+  int setOpt(int nBits, uint8_t nEvent, int nStop);
+
+  std::string port_;
+  int baud_rate_;
+
+  int fd_;
+};
+}
+#endif
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h

@@ -0,0 +1,159 @@
+/*
+ * This file is part of lslidar driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef LSLIDAR_DRIVER_H
+#define LSLIDAR_DRIVER_H
+
+#include <unistd.h>
+#include <stdio.h>
+#include <netinet/in.h>
+#include <string>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/thread.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include <thread>
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "diagnostic_updater/publisher.hpp"
+#include "lslidar_msgs/msg/lslidar_packet.hpp"
+#include "std_msgs/msg/byte.hpp"
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "pcl_conversions/pcl_conversions.h"
+#include "pcl/point_types.h"
+
+#include "time.h"
+#include "input.h"
+#include "lsiosr.h"
+#include "sensor_msgs/msg/laser_scan.hpp"
+namespace lslidar_driver {
+
+struct PointXYZIT {
+    PCL_ADD_POINT4D;
+    uint8_t intensity;
+    double timestamp;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+ 
+typedef struct {
+    double degree;
+    double range;
+    double intensity;
+} ScanPoint;
+
+class LslidarDriver: public rclcpp::Node {
+public:
+	LslidarDriver();
+	LslidarDriver(const rclcpp::NodeOptions& options);
+	~LslidarDriver();
+
+    bool initialize();
+    bool polling();
+
+    typedef std::shared_ptr<LslidarDriver> LslidarDriverPtr;
+    typedef std::shared_ptr<const LslidarDriver> LslidarDriverConstPtr;
+
+private:    
+    uint64_t get_gps_stamp(struct tm t);
+    uint8_t N10_CalCRC8(unsigned char * p, int len);
+    bool loadParameters();
+    bool createRosIO();
+    void open_serial();
+    void lidar_difop();
+    void lidar_order(const std_msgs::msg::Int8::SharedPtr msg);
+    void data_processing(unsigned char *packet_bytes,int len);
+    void data_processing_2(unsigned char *packet_bytes,int len);
+    void difop_processing(unsigned char *packet_bytes);
+    void pubScanThread();
+    void recvThread_crc(int &count,int &link_time);
+    int receive_data(unsigned char *packet_bytes);
+    int getScan(std::vector<ScanPoint> &points, rclcpp::Time &scan_time, float &scan_duration);
+
+    boost::thread *pubscan_thread_ ;
+    boost::shared_ptr<Input> msop_input_;
+    boost::mutex mutex_; 
+    boost::mutex pubscan_mutex_;
+    boost::condition_variable pubscan_cond_;
+    int UDP_PORT_NUMBER; 
+    int count_num;
+    int package_points;
+    int data_bits_start;
+    int degree_bits_start;
+    int end_degree_bits_start;
+    int rpm_bits_start;
+	int baud_rate_;
+    int points_size_;
+    int idx = 0;
+    int link_time = 0;
+    int fixed_array_length;//cyy_add
+
+    bool use_gps_ts;   
+    bool is_start;
+    bool high_reflection;
+    bool compensation;
+    bool first_compensation = true;
+    bool pubScan;
+    bool pubPointCloud2;
+
+    double min_range;
+    double max_range;
+    double angle_disable_min;
+    double angle_disable_max;
+    double angle_able_min;
+    double angle_able_max;
+    double last_degree = 0.0;	
+    double degree_compensation = 0.0;
+
+    uint16_t PACKET_SIZE ;
+    uint64_t sweep_end_time_gps;
+    uint64_t sweep_end_time_hardware;
+    uint64_t sub_second;
+
+    std::string frame_id;
+    std::string interface_selection;
+    std::string scan_topic;
+    std::string lidar_name;
+    std::string serial_port_;
+    std::string dump_file;
+    std::string pointcloud_topic;
+    std::string in_file_name;
+    
+    tm pTime;    
+    rclcpp::Time pre_time_;
+    rclcpp::Time time_;
+    std::vector<ScanPoint> scan_points_;
+    std::vector<ScanPoint> scan_points_bak_;
+    // Diagnostics updater
+    diagnostic_updater::Updater diagnostics;
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic;
+    double diag_min_freq;
+    double diag_max_freq;
+    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_pub;
+	rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr difop_switch;
+    LSIOSR * serial_;
+};
+typedef PointXYZIT VPoint;
+typedef pcl::PointCloud<VPoint> VPointCloud;
+
+} // namespace lslidar_driver
+POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIT,
+                                  (float, x, x)(float, y, y)(float, z, z)(
+                                          std::uint8_t, intensity,
+                                          intensity)(double, timestamp, timestamp))
+#endif // _LSLIDAR_DRIVER_H_

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py

@@ -0,0 +1,50 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir_1 = os.path.join(get_package_share_directory('lslidar_driver_n10p'), 'params', 'lsx10_1.yaml')
+    driver_dir_2 = os.path.join(get_package_share_directory('lslidar_driver_n10p'), 'params', 'lsx10_2.yaml')
+                     
+    driver_node_1 = LifecycleNode(package='lslidar_driver_n10p',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		#设置激光数据topic名称
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='lidar_1',
+                                parameters=[driver_dir_1],
+                                )
+
+    driver_node_2 = LifecycleNode(package='lslidar_driver_n10p',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		#设置激光数据topic名称
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='lidar_2',
+                                parameters=[driver_dir_2],
+                                )
+
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz', 'lslidar.rviz')
+
+    rviz_node = Node(
+        package='rviz2',
+        namespace='',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d', rviz_dir],
+        output='screen')
+
+    return LaunchDescription([
+        driver_node_1,
+        driver_node_2,
+        rviz_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py

@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_net_ros2','lsm10_net.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py

@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_uart_ros2','lsm10.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py

@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_net_ros2','lsm10p_net.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py

@@ -0,0 +1,27 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lidar_uart_ros2','lsm10_p.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py

@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_uart_ros2', 'lsn10.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		#设置激光数据topic名称
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py

@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_net_ros2', 'lsn10_net.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		#设置激光数据topic名称
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py

@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_uart_ros2', 'lsn10p.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		#设置激光数据topic名称
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py

@@ -0,0 +1,28 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_net_ros2', 'lsn10p_net.yaml')
+                     
+    driver_node = LifecycleNode(package='lslidar_driver',
+                                executable='lslidar_driver_node',
+                                name='lslidar_driver_node',		#设置激光数据topic名称
+                                output='screen',
+                                emulate_tty=True,
+                                namespace='',
+                                parameters=[driver_dir],
+                                )
+
+    return LaunchDescription([
+        driver_node,
+    ])
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py

@@ -0,0 +1,26 @@
+#!/usr/bin/python3
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+import lifecycle_msgs.msg
+import os
+
+def generate_launch_description():
+
+    rviz2_config = os.path.join(get_package_share_directory('lslidar_driver'),'rviz','lslidar.rviz')
+      
+    rviz2_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d',rviz2_config],
+        output='screen')                                               
+
+    return LaunchDescription([
+        rviz2_node,
+    ])
+    

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lslidar_driver</name>
+  <version>1.2.0</version>
+  <description>ROS device driver for Leishen lidar.</description>
+  <maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
+  <author>Nick Shu</author>
+  <license>GNU General Public License V3.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>lslidar_msgs</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>rclpy</build_depend>
+  <build_depend>libpcap</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>lslidar_msgs</exec_depend>
+  <exec_depend>pcl_conversions</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>libpcap</exec_depend>
+  <exec_depend>libpcl-all</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+  <depend>diagnostic_updater</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml

@@ -0,0 +1,22 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser                                         #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                                #雷达目的ip
+    device_ip_difop: 192.168.1.102                          #雷达源IP
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: M10                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+    ceil_increase: -1                                        #Lsm10*时改值应设置为-1
+    angle_disable_min: 0.0                                  #单角度裁剪开始值
+    angle_disable_max: 0.0                                  #单角度裁剪结束值
+    truncated_mode_: 0                                      #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0
+                                                            #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改
+    min_range: 0.0                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: net                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/wheeltec_laser                              #串口连接时的串口号
+#    pcap: /home/ls/work/2211/M10_P_gps.pcap                 #雷达是否使用pcap包读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml

@@ -0,0 +1,22 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser                                         #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                                #雷达目的ip
+    device_ip_difop: 192.168.1.102                          #雷达源IP
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: M10_P                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+    ceil_increase: -1                                        #Lsm10*时改值应设置为-1
+    angle_disable_min: 0.0                                  #单角度裁剪开始值
+    angle_disable_max: 0.0                                  #单角度裁剪结束值
+    truncated_mode_: 0                                      #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0
+                                                            #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改
+    min_range: 0.0                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: net                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/wheeltec_laser                              #串口连接时的串口号
+#    pcap: /home/ls/work/2211/M10_P_gps.pcap                 #雷达是否使用pcap包读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml

@@ -0,0 +1,25 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser                                       #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                   	            #雷达源IP
+    device_ip_difop: 192.168.1.102                          #雷达目的ip
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: N10                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+    angle_disable_min: 0.0                                  #角度裁剪开始值
+    angle_disable_max: 0.0                                  #角度裁剪结束值
+    min_range: 0.2                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: net                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/wheeltec_laser                              #串口连接时的串口号
+    high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
+    compensation: false				                              #M10系列是否使用角度补偿功能
+    pubScan: true                                           #是否发布scan话题
+    pubPointCloud2: false                                    #是否发布pointcloud2话题
+    pointcloud_topic: /lslidar_point_cloud                  #设置激光数据topic名称
+#    pcap: /home/ls/1.pcap                                  #雷达是否使用pcap包读取功能
+#    in_file_name: /home/ls/1.txt                           #雷达是否使用txt文件读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml

@@ -0,0 +1,25 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser                                       #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                   	            #雷达源IP
+    device_ip_difop: 192.168.1.102                          #雷达目的ip
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: N10_P                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+    angle_disable_min: 120.0                                  #角度裁剪开始值
+    angle_disable_max: 230.0                                  #角度裁剪结束值
+    min_range: 0.2                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: net                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/wheeltec_laser                              #串口连接时的串口号
+    high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
+    compensation: false				                              #M10系列是否使用角度补偿功能
+    pubScan: true                                           #是否发布scan话题
+    pubPointCloud2: false                                    #是否发布pointcloud2话题
+    pointcloud_topic: /lslidar_point_cloud                  #设置激光数据topic名称
+#    pcap: /home/ls/1.pcap                                  #雷达是否使用pcap包读取功能
+#    in_file_name: /home/ls/1.txt                           #雷达是否使用txt文件读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml

@@ -0,0 +1,28 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser                                         #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                                #雷达目的ip
+    device_ip_difop: 192.168.1.102                          #雷达源IP
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: M10                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+    ceil_increase: -1                                        #Lsm10*时改值应设置为-1
+    angle_disable_min: 0.0                                  #单角度裁剪开始值
+    angle_disable_max: 0.0                                  #单角度裁剪结束值
+    truncated_mode_: 0                                      #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0
+                                                            #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改
+    min_range: 0.0                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: serial                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/wheeltec_laser                              #串口连接时的串口号
+    high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
+    compensation: false				                              #M10系列是否使用角度补偿功能
+    pubScan: true                                           #是否发布scan话题
+    pubPointCloud2: false                                    #是否发布pointcloud2话题
+    pointcloud_topic: /lslidar_point_cloud                  #设置激光数据topic名称
+#    pcap: /home/ls/1.pcap                                  #雷达是否使用pcap包读取功能
+#    in_file_name: /home/ls/1.txt                           #雷达是否使用txt文件读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml

@@ -0,0 +1,28 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser                                         #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                                #雷达目的ip
+    device_ip_difop: 192.168.1.102                          #雷达源IP
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: M10_P                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10
+    ceil_increase: -1                                        #Lsm10*时改值应设置为-1
+    angle_disable_min: 0.0                                  #单角度裁剪开始值
+    angle_disable_max: 0.0                                  #单角度裁剪结束值
+    truncated_mode_: 0                                      #多角度裁剪开关：值为0时表示不使用多角度裁剪，默认为0
+                                                            #值为1表示使用多角度裁剪，同时angle_disable_min与angle_disable_max设为 0，角度值在/lslidar_driver.cc中修改
+    min_range: 0.0                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: serial                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/wheeltec_laser                              #串口连接时的串口号
+    high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
+    compensation: false				                              #M10系列是否使用角度补偿功能
+    pubScan: true                                           #是否发布scan话题
+    pubPointCloud2: false                                    #是否发布pointcloud2话题
+    pointcloud_topic: /lslidar_point_cloud                  #设置激光数据topic名称
+#    pcap: /home/ls/1.pcap                                  #雷达是否使用pcap包读取功能
+#    in_file_name: /home/ls/1.txt                           #雷达是否使用txt文件读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml

@@ -0,0 +1,26 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser_link                                       #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                   	            #雷达源IP
+    device_ip_difop: 192.168.1.102                          #雷达目的ip
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: N10                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+    angle_disable_min: 0.0                                  #角度裁剪开始值
+    angle_disable_max: 0.0                                  #角度裁剪结束值
+    min_range: 0.15                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: serial                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/ttyCH343USB0                         #串口连接时的串口号
+    high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
+    compensation: false				                              #M10系列是否使用角度补偿功能
+    pubScan: true                                           #是否发布scan话题
+    pubPointCloud2: false                                    #是否发布pointcloud2话题
+    pointcloud_topic: /lslidar_point_cloud                  #设置激光数据topic名称
+    fixed_array_length: 450
+#    pcap: /home/ls/1.pcap                                  #雷达是否使用pcap包读取功能
+#    in_file_name: /home/ls/1.txt                           #雷达是否使用txt文件读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml

@@ -0,0 +1,25 @@
+/lslidar_driver_node:
+  ros__parameters: 
+    frame_id: laser                                       #激光坐标
+    group_ip: 224.1.1.2
+    add_multicast: false 
+    device_ip: 192.168.1.200                   	            #雷达源IP
+    device_ip_difop: 192.168.1.102                          #雷达目的ip
+    msop_port: 2368                                         #雷达目的端口号
+    difop_port: 2369                                        #雷达源端口号
+    lidar_name: N10_P                                         #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
+    angle_disable_min: 120.0                                  #角度裁剪开始值
+    angle_disable_max: 230.0                                  #角度裁剪结束值
+    min_range: 0.2                                          #雷达接收距离最小值
+    max_range: 200.0                                        #雷达接收距离最大值
+    use_gps_ts: false                                       #雷达是否使用GPS授时
+    scan_topic: /scan                                       #设置激光数据topic名称
+    interface_selection: serial                             #接口选择:net 为网口,serial 为串口。
+    serial_port_: /dev/wheeltec_laser                              #串口连接时的串口号
+    high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
+    compensation: false				                              #M10系列是否使用角度补偿功能
+    pubScan: true                                           #是否发布scan话题
+    pubPointCloud2: false                                    #是否发布pointcloud2话题
+    pointcloud_topic: /lslidar_point_cloud                  #设置激光数据topic名称
+#    pcap: /home/ls/1.pcap                                  #雷达是否使用pcap包读取功能
+#    in_file_name: /home/ls/1.txt                           #雷达是否使用txt文件读取功能

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz

@@ -0,0 +1,161 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /LaserScan1
+      Splitter Ratio: 0.3441176414489746
+    Tree Height: 617
+  - 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
+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
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: laser
+    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: 3.635173797607422
+      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.8653978705406189
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.095397710800171
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000292000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1283
+  X: 406
+  Y: 152

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc

@@ -0,0 +1,398 @@
+#include "lslidar_driver/input.h"
+
+extern volatile sig_atomic_t flag;
+namespace lslidar_driver
+{
+    static const size_t packet_size_input = 400;
+    ////////////////////////////////////////////////////////////////////////
+    // Input base class implementation
+    ////////////////////////////////////////////////////////////////////////
+
+    /** @brief constructor
+     *
+     *  @param private_nh ROS private handle for calling node.
+     *  @param port UDP port number.
+     */
+    Input::Input(rclcpp::Node *private_nh, uint16_t port) : private_nh_(private_nh), port_(port) {
+        npkt_update_flag_ = false;
+        cur_rpm_ = 0;
+        return_mode_ = 1;
+        devip_str_difop = std::string("192.168.1.200");
+        devip_str_ = std::string("192.168.1.102");
+        lidar_name = std::string("M10");
+        add_multicast = false;
+        group_ip = std::string("224.1.1.2");
+        UDP_PORT_NUMBER_DIFOP = 2369;
+
+
+	    private_nh->declare_parameter<std::string>("device_ip","192.168.1.102");
+        private_nh->declare_parameter<std::string>("device_ip_difop","192.168.1.200");
+        private_nh->declare_parameter<bool>("add_multicast",false);
+        private_nh->declare_parameter<std::string>("group_ip","224.1.1.2");
+        private_nh->declare_parameter<int>("difop_port",2369);
+
+
+        private_nh->get_parameter("lidar_name", lidar_name);
+        private_nh->get_parameter("device_ip", devip_str_);
+        private_nh->get_parameter("add_multicast", add_multicast);
+        private_nh->get_parameter("group_ip", group_ip);
+        private_nh->get_parameter("difop_port", UDP_PORT_NUMBER_DIFOP);
+        private_nh->get_parameter("device_ip_difop", devip_str_difop);
+
+        if (!devip_str_.empty())
+            RCLCPP_INFO(private_nh->get_logger(), "[driver][input] accepting packets from IP address: %s  port: %d",
+                        devip_str_.c_str(),port);
+    }
+
+    /** @brief constructor
+     *
+     *  @param private_nh ROS private handle for calling node.
+     *  @param port UDP port number
+     */
+    InputSocket::InputSocket(rclcpp::Node *private_nh, uint16_t port) : Input(private_nh, port) {
+        sockfd_ = -1;
+
+        if (!devip_str_.empty()) {
+            inet_aton(devip_str_.c_str(), &devip_);
+            inet_aton(devip_str_difop.c_str(), &devip_difop);
+        }
+
+        RCLCPP_INFO(private_nh_->get_logger(), "[driver][socket] Opening UDP socket: port %d", port);
+        sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
+        if (sockfd_ == -1) {
+            perror("socket");  // TODO: ROS_ERROR errno
+            return;
+        }
+
+        int opt = 1;
+        if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) {
+            perror("setsockopt error!\n");
+            return;
+        }
+
+        sockaddr_in my_addr;                   // my address information
+        memset(&my_addr, 0, sizeof(my_addr));  // initialize to zeros
+        my_addr.sin_family = AF_INET;          // host byte order
+        my_addr.sin_port = htons(port);        // port in network byte order
+        my_addr.sin_addr.s_addr = INADDR_ANY;  // automatically fill in my IP
+
+        if (bind(sockfd_, (sockaddr * ) & my_addr, sizeof(sockaddr)) == -1) {
+            perror("bind");  // TODO: ROS_ERROR errno
+            return;
+        }
+
+        if (add_multicast) {
+            struct ip_mreq group;
+            group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
+            group.imr_interface.s_addr = htonl(INADDR_ANY);
+
+            if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
+                perror("Adding multicast group error ");
+                close(sockfd_);
+                exit(1);
+            } else
+                printf("Adding multicast group...OK.\n");
+        }
+        if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
+            perror("non-block");
+            return;
+        }
+    }
+
+    /** @brief destructor */
+    InputSocket::~InputSocket(void) {
+        (void) close(sockfd_);
+    }
+
+    void Input::UDP_difop()
+    {
+        sockaddr_in server_sai;
+        server_sai.sin_family = AF_INET; // IPV4 协议族
+        server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
+        server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
+        for (int k = 0; k < 10; k++)
+        {
+            unsigned char data[188]= {0x00};
+            data[0] = 0xA5;
+            data[1] = 0x5A;
+            data[2] = 0x55;
+            data[184] = 0x08;
+            data[185] = 0x01;
+            data[186] = 0xFA;
+            data[187] = 0xFB;
+            int rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
+            if (rtn < 0)    printf("start scan error !\n");
+            else return; 
+        }
+        return; 
+    }
+    
+    void Input::UDP_order(const std_msgs::msg::Int8 msg)
+    {
+        int i = msg.data;
+        sockaddr_in server_sai;
+        server_sai.sin_family = AF_INET; // IPV4 协议族
+        server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
+        server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
+        int rtn = 0;
+        for (int k = 0; k < 10; k++)
+        {
+            unsigned char data[188]= {0x00};
+            data[0] = 0xA5;
+            data[1] = 0x5A;
+            data[2] = 0x55;
+            data[186] = 0xFA;
+            data[187] = 0xFB;  
+            if(lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P"){
+                if (i <= 1){				    //雷达启停
+                    data[184] = 0x01;
+                    data[185] = char(i);
+                }
+                else if (i == 2){			    //雷达点云不滤波
+                    data[181] = 0x0A;
+                    data[184] = 0x06;
+                    data[185] = 0x01;
+                }
+                else if (i == 3){				//雷达点云正常滤波
+                    data[181] = 0x0B;
+                    data[184] = 0x06;
+                    data[185] = 0x01;
+                }
+                else if (i == 4){				//雷达近距离滤波
+                    data[181] = 0x0C;
+                    data[184] = 0x06;
+                    data[185] = 0x01;
+                }    
+                else if (i == 100){				//接收设备包
+                    data[184] = 0x08;
+                    data[185] = 0x01;
+                }      
+                else return;    
+            }
+            else if (lidar_name == "M10_PLUS"){   
+                data[184] = 0x0A;
+                data[185] = 0x01;           
+                if(i == 5)       {
+                data[141] = 0x01;
+                data[142] = 0x2c;
+                }
+                else if(i == 6)  {
+                data[141] = 0x01;
+                data[142] = 0x68;
+                }
+                else if(i == 8)  {
+                data[141] = 0x01;
+                data[142] = 0xe0;
+                }
+                else if(i == 10) {
+                data[141] = 0x02;
+                data[142] = 0x58;
+                }
+                else if(i == 12) {
+                data[141] = 0x02;
+                data[142] = 0xd0;
+                }
+                else if(i == 15) {
+                data[141] = 0x03;
+                data[142] = 0x84;
+                }
+                else if(i == 20) {
+                data[141] = 0x04;
+                data[142] = 0xb0;
+                }
+                else if(i <= 1)  {
+                data[184] = 0x01;
+                data[185] = char(i);
+                }
+                else if(i == 100) {				//接收设备包
+                    data[184] = 0x08;
+                    data[185] = 0x01;
+                }      
+                else return;
+            }
+            else if(lidar_name == "N10"){
+                if(i <= 1){
+                data[185] = char(i);
+                data[184] = 0x01;
+                }
+                else if(i>=6 && i<=12){
+                data[172] = char(i);
+                data[184] = 0x0a;
+                data[185] = 0X01;
+                }
+                else return; 
+            }
+            rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
+            if (rtn < 0)
+            {
+                printf("start scan error !\n");
+            }
+            else
+            {
+                if (i == 1)
+                    usleep(3000000);
+                return;
+            }
+        }
+        return;
+    }
+
+
+
+    int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet)
+    {
+        int q = 0;
+        struct pollfd fds[1];
+        fds[0].fd = sockfd_;
+        fds[0].events = POLLIN;
+        static const int POLL_TIMEOUT = 2000; // one second (in msec)
+
+        sockaddr_in sender_address{};
+        socklen_t sender_address_len = sizeof(sender_address);
+        while (flag == 1)
+        {
+            // poll() until input available
+            do {
+                int retval = poll(fds, 1, POLL_TIMEOUT);
+                if (retval < 0)  // poll() error?
+                {
+                    if (errno != EINTR)
+                        RCLCPP_ERROR(private_nh_->get_logger(), "[driver][socket] poll() error: %s", strerror(errno));
+                    return 0;
+                }
+                if (retval == 0) // poll() timeout?
+                {
+                    RCLCPP_WARN(private_nh_->get_logger(), "lslidar poll() timeout, port: %d",port_);
+                    return 0;
+                }
+                if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
+                {
+                    RCLCPP_ERROR(private_nh_->get_logger(),"poll() reports lslidar error");
+                    return 0;
+                }
+            } while ((fds[0].revents & POLLIN) == 0);
+
+            // Receive packets that should now be available from the
+            // socket using a blocking read.
+            ssize_t nbytes = recvfrom(sockfd_, &packet->data[0], packet_size_input, 0,
+                                      (sockaddr *)&sender_address, &sender_address_len);
+            //        ROS_DEBUG_STREAM("incomplete lslidar packet read: "
+            //                         << nbytes << " bytes");
+            q = (int)nbytes;
+            if (nbytes < 0)
+            {
+                if (errno != EWOULDBLOCK)
+                {
+                    perror("recvfail");
+                    RCLCPP_ERROR(private_nh_->get_logger(),"recvfail");
+                    return 1;
+                }
+            }
+            else if ((size_t)nbytes <= packet_size_input || (size_t)nbytes >= 50)
+            {
+
+                // read successful,
+                // if packet is not from the lidar scanner we selected by IP,
+                // continue otherwise we are done
+                if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr)
+                    continue;
+                else
+                    break; // done
+            }
+            
+        }
+        if (flag == 0)
+        {
+            abort();
+        }
+
+        return q;
+    }
+    InputPCAP::InputPCAP(rclcpp::Node *private_nh, uint16_t port, double packet_rate, std::string filename) : Input(private_nh, port),
+                                                                                packet_rate_(packet_rate),
+                                                                                filename_(filename)
+    {
+        pcap_ = NULL;
+        empty_ = true;
+        read_once_ = false;
+        read_fast_ = false;
+        repeat_delay_ = 0.0;
+        private_nh->get_parameter("read_once", read_once_);
+        private_nh->get_parameter("read_fast", read_fast_);
+        private_nh->get_parameter("repeat_delay", repeat_delay_);
+
+        if (read_once_)
+            RCLCPP_WARN(private_nh_->get_logger(),"Read input file only once.");
+        if (read_fast_)
+            RCLCPP_WARN(private_nh_->get_logger(),"Read input file as quickly as possible.");
+        if (repeat_delay_ > 0.0)
+            RCLCPP_WARN(private_nh_->get_logger(),"Delay %.3f seconds before repeating input file.", repeat_delay_);
+
+        RCLCPP_INFO(private_nh_->get_logger(),"Opening PCAP file %s",filename_.c_str());
+        if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
+        {
+            RCLCPP_WARN(private_nh_->get_logger(),"Error opening lslidar socket dump file.");
+            return;
+        }
+        std::stringstream filter;
+        if (devip_str_ != "")
+        {
+            filter << "src host " << devip_str_ << "&&";
+        }
+        filter << "udp dst port " << port;
+        pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
+    }
+
+    InputPCAP::~InputPCAP(void)
+    {
+        pcap_close(pcap_);
+    }
+
+    int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt)
+    {
+        struct pcap_pkthdr *header;
+        const u_char *pkt_data;
+        while (flag == 1)
+            {
+            int res;
+            if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
+            {
+                // skip packets not for the correct port and from the selected IP address
+                if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
+                    continue;
+
+                if (read_fast_ == false)
+                    packet_rate_.sleep();
+                mempcpy(&pkt->data[0], pkt_data + 42, packet_size_input);
+                empty_ = false;
+                return 0;
+            }
+            if (empty_)
+            {
+                RCLCPP_WARN(private_nh_->get_logger(),"Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
+                return -1;
+            }
+            if (read_once_)
+            {
+                RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- done reading.");
+                return -1;
+            }
+            if (repeat_delay_ > 0.0)
+            {
+                RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- delaying %.3f seconds.", repeat_delay_);
+                usleep(rint(repeat_delay_ * 1000000.0));
+            }
+            RCLCPP_WARN(private_nh_->get_logger(),"replayding lsliar dump file");
+
+            pcap_close(pcap_);
+            pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
+            empty_ = true;
+        }
+        if (flag == 0)
+        {
+            abort();
+        }
+        return 0;
+    }
+
+} // namespace

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp

@@ -0,0 +1,400 @@
+/*******************************************************
+@company: Copyright (C) 2022, Leishen Intelligent System
+@product: LSM10 and N10
+@filename: lsiosr.cpp
+@brief:
+@version:       date:       author:     comments:
+@v1.0           21-2-4      yao          new
+*******************************************************/
+#include "lslidar_driver/lsiosr.h"
+
+namespace lslidar_driver {
+
+LSIOSR * LSIOSR::instance(std::string name, int speed, int fd)
+{
+  static LSIOSR obj(name, speed, fd);
+  return &obj;
+}
+
+LSIOSR::LSIOSR(std::string port, int baud_rate, int fd):port_(port), baud_rate_(baud_rate), fd_(fd)
+{
+  printf("port = %s, baud_rate = %d\n", port.c_str(), baud_rate);
+}
+
+LSIOSR::~LSIOSR()
+{
+  close();
+}
+/* 串口配置的函数 */
+int LSIOSR::setOpt(int nBits, uint8_t nEvent, int nStop)
+{
+  struct termios newtio, oldtio;
+  /*保存测试现有串口参数设置，在这里如果串口号等出错，会有相关的出错信息*/
+  if (tcgetattr(fd_, &oldtio) != 0)
+  {
+    perror("SetupSerial 1");
+    return -1;
+  }
+  bzero(&newtio, sizeof(newtio));
+  /*步骤一，设置字符大小*/
+  newtio.c_cflag |= CLOCAL;   //如果设置，modem 的控制线将会被忽略。如果没有设置，则 open()函数会阻塞直到载波检测线宣告 modem 处于摘机状态为止。
+  newtio.c_cflag |= CREAD;    //使端口能读取输入的数据
+  /*设置每个数据的位数*/
+  switch (nBits)
+  {
+  case 7:
+    newtio.c_cflag |= CS7;
+    break;
+  case 8:
+    newtio.c_cflag |= CS8;
+    break;
+  }
+  /*设置奇偶校验位*/
+  switch (nEvent)
+  {
+  case 'O': //奇数
+    newtio.c_iflag |= (INPCK | ISTRIP);
+    newtio.c_cflag |= PARENB;   //使能校验，如果不设PARODD则是偶校验
+    newtio.c_cflag |= PARODD;   //奇校验
+    break;
+  case 'E': //偶数
+    newtio.c_iflag |= (INPCK | ISTRIP);
+    newtio.c_cflag |= PARENB;
+    newtio.c_cflag &= ~PARODD;
+    break;
+  case 'N':  //无奇偶校验位
+    newtio.c_cflag &= ~PARENB;
+    break;
+  }
+  /*设置波特率*/
+  switch (baud_rate_)
+  {
+  case 230400:
+    cfsetispeed(&newtio, B230400);
+    cfsetospeed(&newtio, B230400);
+    break;
+  case 460800:
+    cfsetispeed(&newtio, B460800);
+    cfsetospeed(&newtio, B460800);
+    break;
+  case 500000:
+    cfsetispeed(&newtio, B500000);
+    cfsetospeed(&newtio, B500000);
+    break;
+  case 921600:
+    cfsetispeed(&newtio, B921600);
+    cfsetospeed(&newtio, B921600);
+    break;
+  default:
+    cfsetispeed(&newtio, B460800);
+    cfsetospeed(&newtio, B460800);
+    break;
+  }
+
+  /*
+   * 设置停止位
+   * 设置停止位的位数， 如果设置，则会在每帧后产生两个停止位， 如果没有设置，则产生一个
+   * 停止位。一般都是使用一位停止位。需要两位停止位的设备已过时了。
+   * */
+  if (nStop == 1)
+    newtio.c_cflag &= ~CSTOPB;
+  else if (nStop == 2)
+    newtio.c_cflag |= CSTOPB;
+  /*设置等待时间和最小接收字符*/
+  newtio.c_cc[VTIME] = 0;
+  newtio.c_cc[VMIN] = 0;
+  /*处理未接收字符*/
+  tcflush(fd_, TCIFLUSH);
+  /*激活新配置*/
+  if ((tcsetattr(fd_, TCSANOW, &newtio)) != 0)
+  {
+    perror("serial set error");
+    return -1;
+  }
+
+  return 0;
+}
+
+void LSIOSR::flushinput() {
+  tcflush(fd_, TCIFLUSH);
+}
+
+/* 从串口中读取数据 */
+int LSIOSR::read(unsigned char *buffer, int length, int timeout)
+{
+  memset(buffer, 0, length);
+
+  int totalBytesRead = 0;
+  int rc;
+  int unlink = 0;
+  unsigned char* pb = buffer;
+
+  if (timeout > 0)
+  {
+    rc = waitReadable(timeout);
+    if (rc <= 0)
+    {
+      return (rc == 0) ? 0 : -1;
+    }
+
+    int	retry = 3;
+    while (length > 0)
+    {
+      rc = ::read(fd_, pb, (size_t)length);
+
+      if (rc > 0)
+      {		
+        length -= rc;
+        pb += rc;
+        totalBytesRead += rc;
+
+        if (length == 0)
+        {
+          break;
+        }
+      }
+      else if (rc < 0)
+      {
+        printf("error \n");
+        retry--;
+        if (retry <= 0)
+        {
+          break;
+        }
+      }
+	  unlink++;
+      rc = waitReadable(20);
+	  if(unlink > 10)
+		  return -1;
+	  
+      if (rc <= 0)
+      {
+        break;
+      }
+    }
+  }
+  else
+  {
+    rc = ::read(fd_, pb, (size_t)length);
+
+    if (rc > 0)
+    {
+      totalBytesRead += rc;
+    }
+    else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
+    {
+      printf("read error\n");
+      return -1;
+    }
+  }
+
+  return totalBytesRead;
+}
+
+int LSIOSR::waitReadable(int millis)
+{
+  if (fd_ < 0)
+  {
+    return -1;
+  }
+  int serial = fd_;
+  
+  fd_set fdset;
+  struct timeval tv;
+  int rc = 0;
+  
+  while (millis > 0)
+  {
+    if (millis < 5000)
+    {
+      tv.tv_usec = millis % 1000 * 1000;
+      tv.tv_sec  = millis / 1000;
+
+      millis = 0;
+    }
+    else
+    {
+      tv.tv_usec = 0;
+      tv.tv_sec  = 5;
+
+      millis -= 5000;
+    }
+
+    FD_ZERO(&fdset);
+    FD_SET(serial, &fdset);
+    
+    rc = select(serial + 1, &fdset, NULL, NULL, &tv);
+    if (rc > 0)
+    {
+      rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
+      break;
+    }
+    else if (rc < 0)
+    {
+      rc = -1;
+      break;
+    }
+  }
+
+  return rc;
+}
+
+
+int LSIOSR::waitWritable(int millis)
+{
+  if (fd_ < 0)
+  {
+    return -1;
+  }
+  int serial = fd_;
+
+  fd_set fdset;
+  struct timeval tv;
+  int rc = 0;
+
+  while (millis > 0)
+  {
+    if (millis < 5000)
+    {
+      tv.tv_usec = millis % 1000 * 1000;
+      tv.tv_sec  = millis / 1000;
+
+      millis = 0;
+    }
+    else
+    {
+      tv.tv_usec = 0;
+      tv.tv_sec  = 5;
+
+      millis -= 5000;
+    }
+
+    FD_ZERO(&fdset);
+    FD_SET(serial, &fdset);
+
+    rc = select(serial + 1, NULL, &fdset, NULL, &tv);
+    if (rc > 0)
+    {
+      rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
+      break;
+    }
+    else if (rc < 0)
+    {
+      rc = -1;
+      break;
+    }
+  }
+
+  return rc;
+}
+
+/* 向串口中发送数据 */
+int LSIOSR::send(const char* buffer, int length, int timeout)
+{
+  if (fd_ < 0)
+  {
+    return -1;
+  }
+
+  if ((buffer == 0) || (length <= 0))
+  {
+    return -1;
+  }
+
+  int	totalBytesWrite = 0;
+  int rc;
+  char* pb = (char*)buffer;
+
+
+  if (timeout > 0)
+  {
+    rc = waitWritable(timeout);
+    if (rc <= 0)
+    {
+      return (rc == 0) ? 0 : -1;
+    }
+
+    int	retry = 3;
+    while (length > 0)
+    {
+      rc = write(fd_, pb, (size_t)length);
+      if (rc > 0)
+      {
+        length -= rc;
+        pb += rc;
+        totalBytesWrite += rc;
+
+        if (length == 0)
+        {
+          break;
+        }
+      }
+      else
+      {
+        retry--;
+        if (retry <= 0)
+        {
+          break;
+        }
+      }
+
+      rc = waitWritable(50);
+      if (rc <= 0)
+      {
+        break;
+      }
+    }
+  }
+  else
+  {
+    rc = write(fd_, pb, (size_t)length);
+    if (rc > 0)
+    {
+      totalBytesWrite += rc;
+    }
+    else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
+    {
+      return -1;
+    }
+  }
+
+  return totalBytesWrite;
+}
+
+int LSIOSR::init()
+{
+	int error_code = 0;
+		
+	fd_ = open(port_.c_str(), O_RDWR|O_NOCTTY|O_NDELAY);
+	if (0 < fd_)
+	{
+		error_code = 0;
+		setOpt(DATA_BIT_8, PARITY_NONE, STOP_BIT_1);//设置串口参数
+		//printf("open_port %s  OK !\n", port_.c_str());
+	}
+	else
+	{
+		error_code = -1;
+	}
+
+	return error_code;
+}
+
+int LSIOSR::close()
+{
+  ::close(fd_);
+  return 0;
+}
+
+std::string LSIOSR::getPort()
+{
+  return port_;
+}
+
+int LSIOSR::setPortName(std::string name)
+{
+  port_ = name;
+  return 0;
+}
+
+}

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc

@@ -0,0 +1,35 @@
+/*
+ * This file is part of lslidar driver.
+ *
+ * The driver is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The driver is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rclcpp/rclcpp.hpp"
+#include "lslidar_driver/lslidar_driver.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<lslidar_driver::LslidarDriver>();
+  
+  while (rclcpp::ok() && node->polling()) {
+        rclcpp::spin_some(node);
+  }
+  //rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_msgs)
+
+# 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(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+rosidl_generate_interfaces(lslidar_msgs
+  "msg/LslidarDifop.msg"
+  "msg/LslidarPacket.msg"
+  "msg/LslidarPoint.msg"
+  "msg/LslidarScan.msg"
+  "msg/LslidarSweep.msg"
+  DEPENDENCIES builtin_interfaces std_msgs
+  )
+
+ament_package()

src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg

@@ -0,0 +1,2 @@
+int64 temperature
+int64 rpm

src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg

@@ -0,0 +1,5 @@
+# Raw Leishen LIDAR packet.
+
+builtin_interfaces/Time stamp              # packet timestamp
+uint8[2000] data        # packet contents
+

src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg

@@ -0,0 +1,12 @@
+# Time when the point is captured
+float32 time
+
+# Converted distance in the sensor frame
+float64 x
+float64 y
+float64 z
+
+# Raw measurement from Leishen M10
+float64 azimuth
+float64 distance
+float64 intensity

src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg

@@ -0,0 +1,6 @@
+# Altitude of all the points within this scan
+float64 altitude
+
+# The valid points in this scan sorted by azimuth
+# from 0 to 359.99
+LslidarPoint[] points

src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+# The 0th scan is at the bottom
+LslidarScan[16] scans

src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>lslidar_msgs</name>
+  <version>1.2.0</version>
+  <description>ROS message definitions for Leishen LIDARs.</description>
+  <maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
+  <author>Nick Shu</author>
+  <license>GNU General Public License V3.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+
+  <depend>std_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/LSLIDAR_X_ROS2-20240228/src/version.txt

@@ -0,0 +1,20 @@
+版本变更
+/***************************************************************
+初始版本: LSLIDAR_M10_N10_V2.5.0_221104_ROS2
+变更内容: 
+	1.实现M10/M10_P/M10_PLUS/N10/M10_GPS网口和串口传输数据生成点云功能
+    	2.实现点云角度裁剪和距离过滤功能
+    	3.可以通过lslidar_order话题控制雷达启停
+    	4.支持读取pcap包
+	
+更改日期: 2022-11-04
+***************************************************************/
+
+/***************************************************************
+初始版本: LSLIDAR_M10_N10_V2.5.0_221111_ROS2
+变更内容: 
+	1.针对M10和M10_GPS雷达出货后发现的点云问题进行驱动补救
+	
+更改日期: 2022-11-11
+***************************************************************/
+

src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh

@@ -0,0 +1,33 @@
+#CP2102 串口号0002 设置别名为wheeltec_controller
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller.rules
+#CH9102，同时系统安装了对应驱动 串口号0002 设置别名为wheeltec_controller
+echo  'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller2.rules
+#CH9102，同时系统没有安装对应驱动 串口号0002 设置别名为wheeltec_controller
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_controller"' >/etc/udev/rules.d/wheeltec_controller3.rules
+
+#CP2102 串口号0001 设置别名为wheeltec_lidar
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar.rules
+#CH9102，同时系统安装了对应驱动 串口号0001 设置别名为wheeltec_lidar
+echo  'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="54B8001974", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar2.rules
+#CH9102，同时系统没有安装对应驱动 串口号0001 设置别名为wheeltec_lidar
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_lidar"' >/etc/udev/rules.d/wheeltec_lidar3.rules
+
+#CP2102 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss.rules
+#CH9102，同时系统安装了对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo  'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss2.rules
+#CH9102，同时系统没有安装对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss3.rules
+
+echo  'SUBSYSTEM=="video4linux",ATTR{name}=="GENERAL WEBCAM",ATTR{index}=="0",MODE:="0777",SYMLINK+="RgbCam"' >>/etc/udev/rules.d/camera.rules
+echo  'SUBSYSTEM=="video4linux",ATTR{name}=="GENERAL WEBCAM: GENERAL WEBCAM",ATTR{index}=="0",MODE:="0777",SYMLINK+="RgbCam"' >>/etc/udev/rules.d/camera.rules
+echo  'SUBSYSTEM=="video4linux",ATTR{name}=="Astra Pro HD Camera: Astra Pro ",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Pro"' >>/etc/udev/rules.d/camera.rules
+echo  'SUBSYSTEM=="video4linux",ATTR{name}=="USB 2.0 Camera: USB Camera",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Dabai"' >>/etc/udev/rules.d/camera.rules
+echo  'SUBSYSTEM=="video4linux",ATTR{name}=="USB 2.0 Camera",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Gemini"' >>/etc/udev/rules.d/camera.rules
+echo  'SUBSYSTEM=="video4linux",ATTR{name}=="Intel(R) RealSense(TM) Depth Ca",ATTR{index}=="0",MODE:="0777",SYMLINK+="realsense"' >>/etc/udev/rules.d/camera.rules
+
+service udev reload
+sleep 2
+service udev restart
+
+

src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py

@@ -0,0 +1,62 @@
+import os
+from pathlib import Path
+import launch_ros.actions
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    Lslidar_dir = get_package_share_directory('lslidar_driver')
+    Lslidar_launch_dir = os.path.join(Lslidar_dir, 'launch')
+
+       
+    Ld14_dir = get_package_share_directory('ldlidar_sl_ros2')
+    Ld14_launch_dir = os.path.join(Ld14_dir, 'launch')
+    
+    Ld06_dir = get_package_share_directory('ldlidar_stl_ros2')
+    Ld06_launch_dir = os.path.join(Ld14_dir, 'launch')
+           
+
+    Lsm10_m10_uart = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10_uart_launch.py')),)
+            
+    Lsm10_m10_net = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10_net_launch.py')),)
+            
+    Lsm10_m10p_uart = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10p_uart_launch.py')),)
+            
+    Lsm10_m10p_net = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsm10p_net_launch.py')),)
+             
+    Lsn10 = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10_launch.py')),)
+    Lsn10p = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10p_launch.py')),)     
+
+    Lsn10_net = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10_net_launch.py')),)
+    Lsn10p_net = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Lslidar_launch_dir, 'lsn10p_net_launch.py')),) 
+                   
+    Ld14 = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Ld14_launch_dir, 'ld14.launch.py')),)
+    Ld06 = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(Ld06_launch_dir, 'ld06.launch.py')),)
+                  
+    # Create the launch description and populate
+    ld = LaunchDescription()
+    '''
+    Please select your lidar here, options include:
+    Lsm10_m10p_uart、Lsm10_m10p_net、Lsm10_m10_uart、Lsm10_m10_net、Lsn10、Lsn10p,Lsn10_net、Lsn10p_net,ld14、Ld06.
+    1.If you are using LS* lidar (including lsn10, lsm10*), please don't forget to 
+    modify the tf conversion parameters of robot_mode_description.launch.py
+    according to the user guide file.
+    2.If you are using m10 lidar, please pay attention to distinguish whether it is m10p or not.
+    '''
+    ld.add_action(Lsn10)
+
+    return ld
+

src/gc_navigation2_real/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.8)
+project(gc_navigation2_real)
+
+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)
+
+install(
+  DIRECTORY launch config params maps behavior_tree
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml

@@ -0,0 +1,37 @@
+<!--
+  阿克曼底盘自定义行为树：移除了 Spin（原地旋转），
+  恢复行为改为：清除代价地图 → 后退 → 等待
+  实车版本 — 与仿真版相同逻辑
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <!-- 1. 清除代价地图，去掉短暂障碍 -->
+          <Sequence name="ClearingActions">
+            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </Sequence>
+          <!-- 2. 快速后退 0.8m，给重新规划留空间（阿克曼无法原地旋转） -->
+          <BackUp backup_dist="0.80" backup_speed="0.18"/>
+          <!-- 3. 短暂等待 0.3s，让新路径规划完成 -->
+          <Wait wait_duration="0.3"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

src/gc_navigation2_real/config/slam_toolbox_async_real.yaml

@@ -0,0 +1,78 @@
+slam_toolbox:
+  ros__parameters:
+
+    # ============================ Solver 插件参数 ======================================
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ============================ ROS 基础参数（实车）===================================
+    odom_frame: odom_combined                   # 实车：EKF 融合后的里程计坐标系
+    map_frame: map                             # 地图坐标系
+    base_frame: base_footprint                 # 机器人基座坐标系（与 URDF 根帧一致）
+    scan_topic: /scan                          # 激光雷达话题（实车镭神雷达）
+    use_map_saver: true                        # 启用地图保存功能
+    mode: mapping                              # mapping = 定位 + 实时建图
+
+    # ============================ 已有地图加载 ==========================================
+    # map_file_name: ""                        # 从零开始建图，留空
+    map_start_pose: [0.0, 0.0, 0.0]           # 初始位姿 [x, y, yaw]
+
+    # ============================ 调试与性能 ===========================================
+    debug_logging: false
+    throttle_scans: 1                          # 每 N 帧处理一次（实车每秒10帧左右）
+    transform_publish_period: 0.02             # TF 发布周期（秒）
+    map_update_interval: 3.0                   # /map 话题更新间隔（秒）
+    resolution: 0.05                           # 地图分辨率（米/像素）
+    restamp_tf: false
+    min_laser_range: 0.15                      # 实车雷达盲区约 0.12m，设 0.15 过滤近处噪点
+    max_laser_range: 12.0                      # 实车雷达最大有效距离（镭神 N10 ≈ 10-15m）
+    minimum_time_interval: 0.3                 # 最小处理时间间隔（实车可适当放宽）
+    transform_timeout: 0.2                     # TF 查找超时
+    tf_buffer_duration: 30.0                   # TF 缓冲区时长
+    stack_size_to_use: 40000000                # 栈大小（序列化大地图需要）
+    enable_interactive_mode: true              # 交互模式：允许外部工具编辑地图
+
+    # ============================ 通用建图参数 =========================================
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.3               # 实车最小移动距离（米），小于仿真以更频繁更新
+    minimum_travel_heading: 0.3                # 最小转向角度（弧度）
+    check_min_dist_and_heading_precisely: false
+    scan_buffer_size: 10                       # 扫描缓冲区大小
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+
+    # ============================ 回环检测参数 =========================================
+    do_loop_closing: true                      # 启用回环检测
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+    loop_search_maximum_distance: 3.0
+
+    # ============================ 相关性匹配参数 =======================================
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # ============================ 扫描匹配器参数 ========================================
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
+    min_pass_through: 2
+    occupancy_threshold: 0.1

src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml

@@ -0,0 +1,77 @@
+slam_toolbox:
+  ros__parameters:
+
+    # ============================ Solver 插件参数 ======================================
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ============================ ROS 基础参数（实车定位模式）============================
+    odom_frame: odom_combined                   # 实车：EKF 融合后的里程计坐标系
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: /scan
+    mode: localization                         # 纯定位模式，不更新地图
+
+    # ============================ 已有地图加载 ==========================================
+    # 由 launch 文件通过 map_file_name 参数传入 .posegraph 路径
+    # map_file_name: ""
+    map_start_pose: [0.0, 0.0, 0.0]
+
+    # ============================ 调试与性能 ===========================================
+    debug_logging: false
+    throttle_scans: 2                          # 每2帧处理一次（实车激光较慢时使用）
+    transform_publish_period: 0.02
+    map_update_interval: 5.0
+    resolution: 0.05
+    restamp_tf: false
+    min_laser_range: 0.15                      # 过滤雷达盲区噪点
+    max_laser_range: 12.0                      # 实车雷达最大有效距离
+    minimum_time_interval: 0.3
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    stack_size_to_use: 40000000
+
+    # ============================ 通用参数 =============================================
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.3
+    minimum_travel_heading: 0.3
+    check_min_dist_and_heading_precisely: false
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+
+    # 定位模式关闭回环检测
+    do_loop_closing: false
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+    loop_search_maximum_distance: 3.0
+
+    # ============================ 相关性匹配参数 =======================================
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # ============================ 扫描匹配器参数 ========================================
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
+    min_pass_through: 2
+    occupancy_threshold: 0.1

src/gc_navigation2_real/launch/real_bringup.launch.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+# ============================================================================
+# real_bringup.launch.py
+# 功能：实车基础 bringup — 底盘驱动 + 激光雷达 + TF 树
+#
+# 适用场景：
+#   - 单独调试底盘通信和传感器数据
+#   - 遥控测试（无导航）
+#   - 作为其他 launch 文件的基础模块
+#
+# 启动内容：
+#   origincar_base bringup   — 串口驱动 + EKF + IMU 滤波 + TF + robot_state_publisher
+#   lslidar_driver          — 镭神激光雷达 → /scan
+#
+# 使用方式：
+#   ros2 launch gc_navigation2_real real_bringup.launch.py
+# ============================================================================
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    """生成 LaunchDescription，启动实车底盘 + 雷达 + TF（无导航）"""
+
+    # ============================ 1. 包路径 =========================================
+    origincar_base_dir = get_package_share_directory('origincar_base')
+
+    # ============================ 2. 实车底盘 bringup ===============================
+    # 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF
+    origincar_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
+    )
+
+    # ============================ 3. 镭神激光雷达驱动 ===============================
+    lslidar_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory('lslidar_driver'),
+             '/launch', '/lsn10_launch.py']),
+    )
+
+    # ============================ 4. 组装 LaunchDescription =========================
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='使用仿真时间（实车必须为 false）'),
+
+        origincar_bringup,
+        lslidar_launch,
+    ])

src/gc_navigation2_real/launch/real_nav2_slam.launch.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+# ============================================================================
+# real_nav2_slam.launch.py
+# 功能：实车导航 — 已知静态地图 + SLAM 定位 + Nav2 导航
+#
+# 架构：
+#   origincar_base (bringup)    — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF
+#   lslidar_driver             — 镭神激光雷达驱动 → /scan
+#   map_server                 — 加载 yaml+pgm 全量静态地图 → /map
+#   slam_toolbox (localization) — 加载 .posegraph，激光扫描匹配定位
+#                                 发布 map→odom_combined TF（替代 AMCL）
+#                                 ⚠️ /map 话题重映射为 /slam_map，避免冲突
+#   navigation_launch          — Nav2 导航栈
+#                                 global_costmap 从 /map 获取全量静态地图
+#
+# 使用方式：
+#   ros2 launch gc_navigation2_real real_nav2_slam.launch.py
+# ============================================================================
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """生成 LaunchDescription，启动实车底盘 + 雷达 + SLAM定位 + Nav2 导航"""
+    ld = LaunchDescription()
+
+    # =============================1. 包路径===========================================
+    pkg_dir = get_package_share_directory('gc_navigation2_real')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    origincar_base_dir = get_package_share_directory('origincar_base')
+
+    # =============================2. 参数=============================================
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    map_yaml_path = LaunchConfiguration('map', default=os.path.join(
+        pkg_dir, 'maps', 'my_map.yaml'))
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        pkg_dir, 'params', 'gc_navigation_slam_real.yaml'))
+    slam_params_file = os.path.join(
+        pkg_dir, 'config', 'slam_toolbox_localization_real.yaml')
+    posegraph_path = os.path.join(
+        pkg_dir, 'maps', 'my_map.posegraph')
+
+    # =============================3. 实车底盘 bringup ================================
+    origincar_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
+    )
+
+    # =============================4. 镭神激光雷达驱动 ================================
+    lslidar_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory('lslidar_driver'),
+             '/launch', '/lsn10_launch.py']),
+    )
+
+    # =============================5. map_server：全量静态地图 ==========================
+    map_server_node = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': False}],
+    )
+    map_server_lifecycle = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager_map',
+        output='screen',
+        parameters=[{'use_sim_time': False, 'autostart': True,
+                     'node_names': ['map_server']}],
+    )
+
+    # =============================6. slam_toolbox 定位模式（替代 AMCL）==================
+    # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='localization_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[slam_params_file,
+                    {'use_sim_time': False,
+                     'map_file_name': posegraph_path}],
+        remappings=[('/map', '/slam_map')],
+    )
+
+    # =============================7. Nav2 导航栈 =====================================
+    navigation_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+            'params_file': nav2_param_path,
+        }.items(),
+    )
+
+    # =============================8. 组装=============================================
+    ld.add_action(origincar_bringup)
+    ld.add_action(lslidar_launch)
+    ld.add_action(map_server_node)
+    ld.add_action(map_server_lifecycle)
+    ld.add_action(slam_toolbox_node)
+    ld.add_action(navigation_launch)
+
+    return ld

src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+# ============================================================================
+# real_nav2_slam_online.launch.py
+# 功能：实车导航 — 已知全局地图 + SLAM 在线更新 + Nav2 导航
+#
+# 架构：
+#                        ┌─ /map（全量静态地图）
+#   map_server ──────────┤
+#                        └─ 来自 my_map.yaml + my_map.pgm
+#
+#                        ┌─ map→odom_combined TF（激光扫描匹配定位）
+#   slam_toolbox ────────┤
+#   (async mapping)      └─ /slam_map（实时建图结果，重映射避免冲突）
+#
+#   navigation           ┌─ global_costmap/static_layer ← /map（全量地图）
+#                        ├─ local_costmap/obstacle_layer ← /scan（实时避障）
+#                        └─ planner + controller + behavior
+#
+# 关键设计：
+#   1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局
+#   2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果
+#   3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新）
+#   4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图
+#
+# 与仿真版的区别：
+#   - 无 Gazebo，无 spawn_entity
+#   - 使用 origincar_base bringup（串口驱动 + EKF + IMU）
+#   - 使用 lslidar_driver（镭神实车雷达）
+#   - use_sim_time: False
+#   - TF 帧适配实车（odom_combined, base_footprint）
+#
+# 使用方式：
+#   ros2 launch gc_navigation2_real real_nav2_slam_online.launch.py
+# ============================================================================
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+
+
+def generate_launch_description():
+    """启动实车底盘 + 雷达 + 静态地图 + SLAM 在线定位建图 + Nav2 导航"""
+    ld = LaunchDescription()
+
+    # =============================1. 包路径===========================================
+    pkg_dir = get_package_share_directory('gc_navigation2_real')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    origincar_base_dir = get_package_share_directory('origincar_base')
+
+    # =============================2. 参数=============================================
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    map_yaml_path = os.path.join(pkg_dir, 'maps', 'my_map.yaml')
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        pkg_dir, 'params', 'gc_navigation_slam_real.yaml'))
+    slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml')
+    posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph')
+
+    # =============================3. 实车底盘 bringup ================================
+    origincar_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
+    )
+
+    # =============================4. 镭神激光雷达驱动 ================================
+    lslidar_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory('lslidar_driver'),
+             '/launch', '/lsn10_launch.py']),
+    )
+
+    # =============================5. map_server：全量静态地图 ==========================
+    map_server_node = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': False}],
+    )
+    map_server_lifecycle = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager_map',
+        output='screen',
+        parameters=[{'use_sim_time': False, 'autostart': True,
+                     'node_names': ['map_server']}],
+    )
+
+    # =============================6. slam_toolbox：定位 + 实时建图========================
+    # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[slam_params_file, {'use_sim_time': False}],
+        remappings=[('/map', '/slam_map')],
+    )
+
+    # =============================7. 加载已有序列化地图 =================================
+    load_map_cmd = ExecuteProcess(
+        cmd=[
+            'bash', '-c',
+            'sleep 8 && '
+            'ros2 service call /slam_toolbox/deserialize_map '
+            'slam_toolbox/srv/DeserializePoseGraph '
+            "'{filename: \"%s\", match_type: 1}'" % posegraph_path,
+        ],
+        output='screen',
+    )
+
+    # =============================8. Nav2 导航栈 =====================================
+    navigation_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+            'params_file': nav2_param_path,
+        }.items(),
+    )
+
+    # =============================9. 组装============================================
+    ld.add_action(origincar_bringup)
+    ld.add_action(lslidar_launch)
+    ld.add_action(map_server_node)
+    ld.add_action(map_server_lifecycle)
+    ld.add_action(slam_toolbox_node)
+    ld.add_action(load_map_cmd)
+    ld.add_action(navigation_launch)
+
+    return ld

src/gc_navigation2_real/launch/real_slam_mapping.launch.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+# ============================================================================
+# real_slam_mapping.launch.py
+# 功能：实车 SLAM 建图启动文件（无 Gazebo 仿真）
+#
+# 适用场景：
+#   - 在实际场地遥控机器人建立环境地图
+#   - 在已有地图基础上增量更新
+#   - 单独调试 slam_toolbox 建图参数
+#
+# 架构：
+#   origincar_base (bringup)    — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF
+#   lslidar_driver             — 镭神激光雷达驱动 → /scan
+#   async_slam_toolbox_node    — 激光 SLAM 建图（mapping 模式）
+#                                 发布 map→odom_combined TF
+#                                 发布 /map 话题（实时地图）
+#
+# 使用方式：
+#   ros2 launch gc_navigation2_real real_slam_mapping.launch.py
+#
+# 保存地图：
+#   ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
+#     "{filename: '/home/guoch/test_ws/src/gc_navigation2_real/maps/my_map'}"
+#   ros2 run nav2_map_server map_saver_cli -f <map_name>
+# ============================================================================
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    """生成 LaunchDescription，启动实车底盘 + 雷达 + slam_toolbox 建图"""
+
+    # ============================ 1. 包路径 =========================================
+    pkg_dir = get_package_share_directory('gc_navigation2_real')
+    origincar_base_dir = get_package_share_directory('origincar_base')
+
+    # ============================ 2. 声明启动参数 ====================================
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+
+    # slam_params_file: slam_toolbox 配置文件
+    slam_params_file = LaunchConfiguration(
+        'slam_params_file',
+        default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'))
+
+    # map_file_name: 可选，加载已有序列化地图继续建图
+    map_file_name = LaunchConfiguration('map_file_name', default='')
+
+    # ============================ 3. 实车底盘 bringup ===============================
+    # 启动 origincar_base：串口驱动 + EKF + IMU 滤波 + TF 发布
+    # 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF
+    origincar_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
+    )
+
+    # ============================ 4. 镭神激光雷达驱动 ===============================
+    # 启动 lslidar_driver → /scan 话题
+    lslidar_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory('lslidar_driver'),
+             '/launch', '/lsn10_launch.py']),
+    )
+
+    # ============================ 5. slam_toolbox 建图节点 ============================
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[
+            slam_params_file,
+            {
+                'use_sim_time': False,
+                'map_file_name': map_file_name,
+            },
+        ],
+    )
+
+    # ============================ 6. 组装 LaunchDescription =========================
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='使用仿真时间（实车必须为 false）'),
+        DeclareLaunchArgument(
+            'slam_params_file',
+            default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'),
+            description='slam_toolbox 参数配置文件路径'),
+        DeclareLaunchArgument(
+            'map_file_name',
+            default_value='',
+            description='已有序列化地图路径（.posegraph），留空则从零开始建图'),
+
+        origincar_bringup,
+        lslidar_launch,
+        slam_toolbox_node,
+    ])

src/gc_navigation2_real/package.xml

@@ -0,0 +1,31 @@
+<?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>gc_navigation2_real</name>
+  <version>0.0.0</version>
+  <description>Real-world navigation package: SLAM + Nav2 for OriginCar on physical robot (no Gazebo)</description>
+  <maintainer email="guoch@todo.todo">guoch</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>slam_toolbox</exec_depend>
+  <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>origincar_base</exec_depend>
+  <exec_depend>origincar_description</exec_depend>
+  <exec_depend>origincar_msg</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>imu_filter_madgwick</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>lslidar_driver</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/gc_navigation2_real/params/gc_navigation_slam_real.yaml

@@ -0,0 +1,353 @@
+# ============================================================================
+# gc_navigation_slam_real.yaml
+# 实车 Nav2 导航参数（SLAM 定位方案，无 AMCL）
+#
+# 与仿真版的区别：
+#   - use_sim_time: False（使用系统时钟）
+#   - TF 帧适配实车（odom_combined, base_footprint）
+#   - 速度/加速度参数适当降低以保证实车安全
+#   - 激光雷达参数匹配镭神 N10 实车性能
+# ============================================================================
+
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom_combined
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
+    default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_real/share/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 36
+      model_dt: 0.05
+      batch_size: 1000
+      vx_std: 0.2
+      vy_std: 0.0
+      wz_std: 0.4
+      vx_max: 0.4           # 实车最大线速度降低（安全）
+      vx_min: -0.25         # 实车最大倒车速度降低
+      vy_max: 0.0
+      wz_max: 1.5           # 实车最大角速度降低
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: true
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom_combined         # 实车：EKF 融合后的里程计坐标系
+      robot_base_frame: base_footprint    # 实车：URDF 根帧
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: False
+      transform_tolerance: 0.5
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint   # 实车：与 URDF 根帧一致
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: true
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.3
+      change_penalty: 0.0
+      non_straight_penalty: 1.2
+      cost_penalty: 2.0
+      retrospective_penalty: 0.015
+      lookup_table_size: 20.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: odom_combined              # 实车：EKF 融合后的里程计坐标系
+    robot_base_frame: base_footprint         # 实车：URDF 根帧
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

src/origincar_base/CMakeLists.txt

@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 3.5)
+project(origincar_base)
+
+# 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(actionlib REQUIRED)
+#find_package(actionlib_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+find_package(nav2_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)                         # CHANGE
+find_package(origincar_msg REQUIRED)                         # CHANGE
+find_package(serial REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(origincar_description REQUIRED)
+find_package(robot_localization REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Position.msg"
+ )
+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()
+
+include_directories(
+  include
+  ${robot_localization_INCLUDE_DIRS}
+)
+set(origincar_base_node_SRCS
+  src/origincar_base.cpp
+  src/Quaternion_Solution.cpp
+)
+
+add_executable(origincar_base_node src/origincar_base.cpp src/Quaternion_Solution.cpp)
+ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)
+
+#add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)
+#ament_target_dependencies(testNode rclcpp std_msgs nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg)
+
+install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME})
+
+install(TARGETS
+origincar_base_node
+
+DESTINATION lib/${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  DIRECTORY launch  config 
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_package()

src/origincar_base/config/ekf.yaml

@@ -0,0 +1,222 @@
+### ekf config file ###
+ekf_filter_node:
+    ros__parameters:
+# 启用通过话题设置初始位姿
+        use_pose_with_covariance_stamped: true  # 允许通过话题重置
+        pose0: /set_pose  # 指定话题名称
+        pose0_config: [true, true, false,  # 使用 x, y
+                      false, false, true]  # 使用 yaw
+        pose0_differential: false
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+        frequency: 30.0
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+        sensor_timeout: 2.0
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+        two_d_mode: true
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+        transform_time_offset: 0.0
+
+# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
+# Defaults to 0.0 if unspecified.
+        transform_timeout: 0.2
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+        print_diagnostics: false
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+        debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+        debug_out_file: /path/to/debug/file.txt
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+        publish_tf: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: false
+
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+#         odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+        map_frame: map              # Defaults to "map" if unspecified
+        odom_frame: odom_combined            # Defaults to "odom" if unspecified
+        base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
+        world_frame: odom_combined           # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+        odom0: odom
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+        odom0_config: [true, false, false,
+                       false, false, false,
+                       true,  true,  false,
+                       false, false, true,
+                       false, false, false]
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+        odom0_queue_size: 10
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+        odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+        odom0_differential: true
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+        odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+#         odom0_pose_rejection_threshold: 5.0
+#         odom0_twist_rejection_threshold: 1.0
+
+        imu0: /imu/data_raw
+        imu0_config: [false, false, false,
+                      false, false, true,
+                      false, false, false,
+                      false, false, true,
+                      false,  false, false]
+
+        imu0_nodelay: false
+        imu0_differential: false
+        imu0_relative: true
+        imu0_queue_size: 10
+        imu0_pose_rejection_threshold: 20.0                 # Note the difference in parameter names
+        imu0_twist_rejection_threshold: 1.542                #
+        imu0_linear_acceleration_rejection_threshold: 10.0  #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+        imu0_remove_gravitational_acceleration: true
+
+# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+        use_control: false
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+        stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+        control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+        control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+        process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.05, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02, 0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+        initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,  0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,  0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,  0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9, 0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9, 0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]
+

src/origincar_base/config/imu.yaml

@@ -0,0 +1,7 @@
+imu_filter_madgwick:
+    ros__parameters:
+        fixed_frame: "base_footprint"
+        use_mag: false
+        publish_tf: false
+        world_frame: "enu"
+        orientation_stddev: 0.05

src/origincar_base/include/origincar_base/Quaternion_Solution.h

@@ -0,0 +1,10 @@
+
+#ifndef __QUATERNION_SOLUTION_H_
+#define __QUATERNION_SOLUTION_H_
+#include "origincar_base.h"
+float InvSqrt(float number);
+void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az);
+extern volatile float q0, q1, q2, q3;    
+#endif
+
+

src/origincar_base/include/origincar_base/origincar_base.h

@@ -0,0 +1,228 @@
+
+#ifndef _ORIGINCAR_BASE_H_
+#define _ORIGINCAR_BASE_H_
+
+#include <memory>
+#include <inttypes.h>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include <csignal>
+#include <thread>
+
+#include <iostream>
+#include <string.h>
+#include <string>
+#include <iostream>
+#include <math.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <rcl/types.h>
+#include <sys/stat.h>
+
+#include <serial/serial.h>
+#include <fcntl.h>
+#include <stdbool.h>
+#include <std_msgs/msg/string.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include <tf2_ros/transform_broadcaster.h>
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
+#include "origincar_msg/msg/data.hpp"
+#include "origincar_msg/msg/sign.hpp"  // 匹配信号发送
+#include <sensor_msgs/msg/imu.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+using namespace std;
+
+
+#define SEND_DATA_CHECK   1          //Send data check flag bits //发送数据校验标志位
+#define READ_DATA_CHECK   0          //Receive data to check flag bits //接收数据校验标志位
+#define FRAME_HEADER      0X7B       //Frame head //帧头
+#define FRAME_TAIL        0X7D       //Frame tail //帧尾
+#define RECEIVE_DATA_SIZE 24         //The length of the data sent by the lower computer //下位机发送过来的数据的长度
+#define SEND_DATA_SIZE    11         //The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度
+#define PI 				  3.1415926f //PI //圆周率
+
+#define GYROSCOPE_RATIO   0.00026644f
+
+#define ACCEl_RATIO 	  1671.84f
+
+extern sensor_msgs::msg::Imu Mpu6050;
+
+const double odom_pose_covariance[36]   = {1e-3,    0,    0,   0,   0,    0,
+										      0, 1e-3,    0,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0,  1e3 };
+
+const double odom_pose_covariance2[36]  = {1e-9,    0,    0,   0,   0,    0,
+										      0, 1e-3, 1e-9,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0, 1e-9 };
+
+const double odom_twist_covariance[36]  = {1e-3,    0,    0,   0,   0,    0,
+										      0, 1e-3,    0,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0,  1e3 };
+
+const double odom_twist_covariance2[36] = {1e-9,    0,    0,   0,   0,    0,
+										      0, 1e-3, 1e-9,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0, 1e-9} ;
+
+typedef struct __Vel_Pos_Data_
+{
+	float X;
+	float Y;
+	float Z;
+
+} Vel_Pos_Data;
+
+typedef struct __MPU6050_DATA_
+{
+	short accele_x_data;
+	short accele_y_data;
+	short accele_z_data;
+    short gyros_x_data;
+	short gyros_y_data;
+	short gyros_z_data;
+
+} MPU6050_DATA;
+
+typedef struct _SEND_DATA_
+{
+	uint8_t tx[SEND_DATA_SIZE];
+	float X_speed;
+	float Y_speed;
+	float Z_speed;
+	unsigned char Frame_Tail;
+} SEND_DATA;
+
+typedef struct _RECEIVE_DATA_
+{
+	uint8_t rx[RECEIVE_DATA_SIZE];
+	uint8_t Flag_Stop;
+	unsigned char Frame_Header;
+	float X_speed;
+	float Y_speed;
+	float Z_speed;
+	float Power_Voltage;
+	unsigned char Frame_Tail;
+} RECEIVE_DATA;
+
+class origincar_base : public rclcpp::Node
+
+{
+public:
+	origincar_base();
+	~origincar_base();
+	void Control();
+	void Publish_Odom();
+
+public : 
+	serial::Serial Stm32_Serial;
+
+private:
+	void declare_parameters();
+	void get_parameters();
+
+
+	void Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux);
+	void Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl);
+
+	void Publish_ImuSensor();
+	void Publish_Voltage();
+	auto createQuaternionMsgFromYaw(double yaw);
+
+	bool Get_Sensor_Data();
+	unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode);
+	short IMU_Trans(uint8_t Data_High,uint8_t Data_Low);
+	float Odom_Trans(uint8_t Data_High,uint8_t Data_Low);
+
+	void Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch);
+
+private:
+  void reset_filter() 
+  {
+    auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
+    
+    // 设置头信息
+    msg->header.stamp = this->now();
+    msg->header.frame_id = "odom_combined";
+
+    // 重置位姿
+    msg->pose.pose.position.x = 0.54;
+    msg->pose.pose.position.y = 0.2;
+    msg->pose.pose.orientation.w = 1.0;
+
+    // 设置关键协方差
+    std::array<double, 36> cov = {0};
+    cov[0] = 0.5;  // x
+    cov[7] = 0.5;  // y
+    cov[35] = 0.5; // yaw
+    msg->pose.covariance = cov;
+
+    // 发布重置指令
+    pose_pub_->publish(*msg);
+    RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!");
+  }
+
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+	rclcpp::Time _Now, _Last_Time;
+	float Sampling_Time;
+	rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr Cmd_Vel_Sub;
+	rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr Akm_Cmd_Vel_Sub;
+
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher;
+	rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr voltage_publisher;
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher; 
+
+	rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr test_publisher;
+
+	rclcpp::Publisher<origincar_msg::msg::Data>::SharedPtr robotpose_publisher;
+	rclcpp::Publisher<origincar_msg::msg::Data>::SharedPtr robotvel_publisher;
+	// rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr tf_pub_;
+
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tf_bro;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf_pub_;
+	rclcpp::TimerBase::SharedPtr test_timer;
+
+	rclcpp::TimerBase::SharedPtr odom_timer;
+	rclcpp::TimerBase::SharedPtr imu_timer;
+	rclcpp::TimerBase::SharedPtr voltage_timer;
+
+	rclcpp::TimerBase::SharedPtr robotpose_timer;
+	rclcpp::TimerBase::SharedPtr robotvel_timer;
+
+	std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+	rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr Sign_Switch_Sub;
+
+	string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
+	std::string cmd_vel;
+	int serial_baud_rate;
+	RECEIVE_DATA Receive_Data;
+	SEND_DATA Send_Data;
+
+	Vel_Pos_Data Robot_Pos;
+	Vel_Pos_Data Robot_Vel;
+	MPU6050_DATA Mpu6050_Data;
+	float Power_voltage;
+    size_t count_;
+};
+
+
+#endif //_ORIGINCAR_BASE_H_

src/origincar_base/launch/base_serial.launch.py

@@ -0,0 +1,47 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+import launch_ros.actions
+
+def generate_launch_description():
+    akmcar = LaunchConfiguration('akmcar', default='false')
+
+    robot_parameters = [
+        {'usart_port_name': '/dev/ttyACM0',
+         'serial_baud_rate': 115200,
+         'robot_frame_id': 'base_link',
+         'odom_frame_id': 'odom',
+         'cmd_vel': 'cmd_vel',
+         'product_number': 0}
+    ]
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'akmcar',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'
+        ),
+
+        launch_ros.actions.Node(
+            condition=IfCondition(akmcar),
+            package='origincar_base',
+            executable='origincar_base_node',
+            parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}],
+            remappings=[('/cmd_vel', 'cmd_vel')],
+        ),
+
+        launch_ros.actions.Node(
+            condition=IfCondition(akmcar),
+            package='origincar_base',
+            executable='cmd_vel_to_ackermann_drive.py',
+            name='cmd_vel_to_ackermann_drive',
+        ),
+
+        launch_ros.actions.Node(
+            condition=UnlessCondition(akmcar),
+            package='origincar_base',
+            executable='origincar_base_node',
+            parameters=robot_parameters + [{'akm_cmd_vel': 'none'}],
+        )
+    ])

src/origincar_base/launch/ekf.launch.py

@@ -0,0 +1,33 @@
+#!/usr/bin/python3
+
+import os
+import launch
+import launch_ros
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    
+    package_name = 'origincar_base'
+    ld =  launch.LaunchDescription()
+    pkg_share = launch_ros.substitutions.FindPackageShare(package=package_name).find(package_name) 
+    robot_localization_file_path = os.path.join(pkg_share, 'config/ekf.yaml')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+      # Start robot localization using an Extended Kalman filter
+    robot_localization_node = Node(
+      package='robot_localization',
+      executable='ekf_node',
+      name='ekf_filter_node',
+      output='screen',
+      remappings=[("odometry/filtered", "odom_combined")],
+      parameters=[
+          robot_localization_file_path,
+          {'use_sim_time': use_sim_time}
+      ])
+
+    ld.add_action(launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='false',
+                                            description='Flag to disable use_sim_time'))
+    ld.add_action(robot_localization_node)
+    return ld

src/origincar_base/launch/origincar_bringup.launch.py

@@ -0,0 +1,95 @@
+import os
+from pathlib import Path
+import launch
+from launch.actions import SetEnvironmentVariable
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import PushRosNamespace
+import launch_ros.actions
+from launch.conditions import UnlessCondition
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('origincar_base')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+    ekf_config = Path(get_package_share_directory('origincar_base'), 'config', 'ekf.yaml')
+    imu_config = Path(get_package_share_directory('origincar_base'), 'config', 'imu.yaml')
+
+    
+    carto_slam = LaunchConfiguration('carto_slam', default='false')
+    carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false')
+
+    akmcar = LaunchConfiguration('akmcar', default='true')
+    akmcar_dec = DeclareLaunchArgument('akmcar', default_value='true',
+        description='阿克曼底盘模式 (true=阿克曼, false=差速)')
+            
+    origincar_base = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')),
+            launch_arguments={'akmcar': akmcar}.items(),
+    )
+
+    choose_car = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')),
+    )
+
+    
+    base_to_link = launch_ros.actions.Node(
+            package='tf2_ros', 
+            executable='static_transform_publisher', 
+            name='base_to_link',
+            arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'],#arguments=['0.41', '0.12', '0','0', '0','0','base_footprint','base_link'],
+    )
+    
+
+    base_to_gyro = launch_ros.actions.Node(
+            package='tf2_ros', 
+            executable='static_transform_publisher', 
+            name='base_to_gyro',
+            arguments=['0', '0', '0','0', '0','0','base_footprint','gyro_link'],
+    )
+    
+    link_to_laser = launch_ros.actions.Node(
+            package='tf2_ros', 
+            executable='static_transform_publisher', 
+            name='link_to_laser',
+            arguments=['0', '0', '0','0', '0','0','base_link','laser'],
+    )
+
+    imu_filter_node =  launch_ros.actions.Node(
+        package='imu_filter_madgwick',
+        executable='imu_filter_madgwick_node',
+        parameters=[imu_config]
+    )
+    
+    robot_ekf = launch_ros.actions.Node(
+            condition=UnlessCondition(carto_slam),
+            package='robot_localization', 
+            executable='ekf_node', 
+            parameters=[ekf_config],
+            remappings=[("odometry/filtered", "odom_combined")]
+            )
+                              
+    joint_state_publisher_node = launch_ros.actions.Node(
+            package='joint_state_publisher', 
+            executable='joint_state_publisher', 
+            name='joint_state_publisher',
+    )
+
+    ld = LaunchDescription()
+
+    ld.add_action(carto_slam_dec)
+    ld.add_action(akmcar_dec)
+    ld.add_action(origincar_base)
+    ld.add_action(base_to_link)
+    ld.add_action(base_to_gyro)
+    ld.add_action(joint_state_publisher_node)
+    ld.add_action(choose_car)
+    ld.add_action(imu_filter_node)
+    ld.add_action(robot_ekf)
+
+    return ld
+

src/origincar_base/launch/robot_mode_description.launch.py

@@ -0,0 +1,24 @@
+import os
+from pathlib import Path
+import launch_ros.actions
+import launch
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,LogInfo,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    origincar_description = GroupAction([
+        launch_ros.actions.Node(
+            package='robot_state_publisher', 
+            executable='robot_state_publisher', 
+            name='robot_state_publisher',
+            arguments=[os.path.join(get_package_share_directory('origincar_description'),'urdf','origincar.urdf')]
+        )
+    ])
+ 
+    ld = LaunchDescription()
+
+    ld.add_action(origincar_description)
+    return ld

src/origincar_base/msg/Position.msg

@@ -0,0 +1,3 @@
+float32 angle_x
+float32 angle_y
+float32 distance

src/origincar_base/package.xml

@@ -0,0 +1,44 @@
+<?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>origincar_base</name>
+  <version>0.0.0</version>
+  <description>ROS2 origincar_base for origincar_base</description>
+  <maintainer email="ps-micro@todo.todo">ps-micro</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <build_export_depend>tf2_geometry_msgs</build_export_depend>
+
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <build_depend>tf2_geometry_msgs</build_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <depend>ament_index_cpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>rclpy</depend>
+  <depend>serial</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>ackermann_msgs</depend>
+  <depend>origincar_msg</depend>
+  <depend>origincar_description</depend>
+  <depend>robot_localization</depend>
+  
+  <test_depend>boost</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+import math
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from ackermann_msgs.msg import AckermannDriveStamped
+from rclpy.qos import QoSProfile
+
+class CmdVel2AckermannDriveNode(Node):
+    def __init__(self):
+        super().__init__('cmd_vel_to_ackermann_drive')
+        self.publisher = self.create_publisher(AckermannDriveStamped, '/ackermann_cmd', QoSProfile(depth=10))
+        self.subscription = self.create_subscription(Twist, 'cmd_vel', self.cmd_callback, QoSProfile(depth=10))
+        self.wheelbase = 0.143  #轮距
+        self.frame_id = 'odom_combined'
+        self.cmd_angle_instead_rotvel = False
+
+    def convert_trans_rot_vel_to_steering_angle(self, vel, omega):
+        if omega == 0 or vel == 0:
+            return 0
+        radius = vel / omega
+        return math.atan(self.wheelbase / radius)
+
+    def cmd_callback(self, data):
+        vel = data.linear.x
+        if self.cmd_angle_instead_rotvel:
+            steering = data.angular.z
+        else:
+            steering = 1.0 * self.convert_trans_rot_vel_to_steering_angle(vel, data.angular.z)
+
+        msg = AckermannDriveStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self.frame_id
+        msg.drive.steering_angle = steering
+        msg.drive.speed = vel
+        self.publisher.publish(msg)
+
+def main():
+    rclpy.init()
+    node = CmdVel2AckermannDriveNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/origincar_base/src/Quaternion_Solution.cpp

@@ -0,0 +1,91 @@
+
+#include "origincar_base/Quaternion_Solution.h"
+#define SAMPLING_FREQ 20.0f // 采样频率
+/**************************************
+Date: May 31, 2020
+Function: 平方根倒数 求四元数用到
+***************************************/
+float InvSqrt(float number)
+{
+  volatile long i;
+    volatile float x, y;
+    volatile const float f = 1.5F;
+    x = number * 0.5F;
+    y = number;
+    i = * (( long * ) &y);
+    i = 0x5f375a86 - ( i >> 1 );
+    y = * (( float * ) &i);
+    y = y * ( f - ( x * y * y ) );
+
+  return y;
+}
+/**************************************
+Date: May 31, 2020
+Function: 四元数解算
+***************************************/
+volatile float twoKp = 1.0f;     // 2 * proportional gain (Kp)
+volatile float twoKi = 0.0f;     // 2 * integral gain (Ki)
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;          // quaternion of sensor frame relative to auxiliary frame
+volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
+void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az)
+{
+  float recipNorm;
+  float halfvx, halfvy, halfvz;
+  float halfex, halfey, halfez;
+  float qa, qb, qc;
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    // 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模
+    recipNorm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;      
+    // 把四元数换算成方向余弦中的第三行的三个元素
+    halfvx = q1 * q3 - q0 * q2;
+    halfvy = q0 * q1 + q2 * q3;
+    halfvz = q0 * q0 - 0.5f + q3 * q3;
+    //误差是估计的重力方向和测量的重力方向的交叉乘积之和
+    halfex = (ay * halfvz - az * halfvy);
+    halfey = (az * halfvx - ax * halfvz);
+    halfez = (ax * halfvy - ay * halfvx);
+    // 计算并应用积分反馈（如果启用）
+    if(twoKi > 0.0f) {
+      integralFBx += twoKi * halfex * (1.0f / SAMPLING_FREQ);  // integral error scaled by Ki
+      integralFBy += twoKi * halfey * (1.0f / SAMPLING_FREQ);
+      integralFBz += twoKi * halfez * (1.0f / SAMPLING_FREQ);
+      gx += integralFBx;        // apply integral feedback
+      gy += integralFBy;
+      gz += integralFBz;
+    }
+    else {
+      integralFBx = 0.0f;       // prevent integral windup
+      integralFBy = 0.0f;
+      integralFBz = 0.0f;
+    }
+    // Apply proportional feedback
+    gx += twoKp * halfex;
+    gy += twoKp * halfey;
+    gz += twoKp * halfez;
+  }
+  // Integrate rate of change of quaternion
+  gx *= (0.5f * (1.0f / SAMPLING_FREQ));   // pre-multiply common factors
+  gy *= (0.5f * (1.0f / SAMPLING_FREQ));
+  gz *= (0.5f * (1.0f / SAMPLING_FREQ));
+  qa = q0;
+  qb = q1;
+  qc = q2;
+  q0 += (-qb * gx - qc * gy - q3 * gz);
+  q1 += (qa * gx + qc * gz - q3 * gy);
+  q2 += (qa * gy - qb * gz + q3 * gx);
+  q3 += (qa * gz + qb * gy - qc * gx); 
+  // Normalise quaternion
+  recipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  Mpu6050.orientation.w = q0;
+  Mpu6050.orientation.x = q1;
+  Mpu6050.orientation.y = q2;
+  Mpu6050.orientation.z = q3;
+}

src/origincar_base/src/origincar_base.cpp

@@ -0,0 +1,469 @@
+#include "origincar_base/origincar_base.h"
+#include "rclcpp/rclcpp.hpp"
+#include "origincar_base/Quaternion_Solution.h"
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" 
+#include "origincar_msg/msg/data.hpp"
+#include "robot_localization/srv/set_pose.hpp"
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+using std::placeholders::_1;
+using namespace std;
+void sigintHandler(int sig);
+sensor_msgs::msg::Imu Mpu6050;
+rclcpp::Node::SharedPtr node_handle = nullptr;
+int Init_imu_num = 0;
+float gyro_z_sum=0;
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    signal(SIGINT, sigintHandler);
+    origincar_base Robot_Control;
+    Robot_Control.Control();
+    rclcpp::shutdown();
+    return 0;
+}
+
+
+
+
+short origincar_base::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
+{
+    short transition_16;
+    transition_16 = 0;
+    transition_16 |=  Data_High<<8;
+    transition_16 |=  Data_Low;
+    return transition_16;
+}
+
+float origincar_base::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
+{
+    float data_return;
+    short transition_16;
+    transition_16 = 0;
+    transition_16 |=  Data_High<<8;
+    transition_16 |=  Data_Low;
+    data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001;
+    return data_return;
+  }
+
+void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
+{
+    short  transition;
+    std::cout<<"linerx"<<akm_ctl->drive.speed<<std::endl;
+    std::cout<<"angular"<<akm_ctl->drive.steering_angle<<std::endl;
+  
+    Send_Data.tx[0]=FRAME_HEADER;
+    Send_Data.tx[1] = 0;
+    Send_Data.tx[2] = 0; 
+
+    transition=0;
+    transition = akm_ctl->drive.speed*1000;
+    Send_Data.tx[4] = transition;
+    Send_Data.tx[3] = transition>>8;
+
+    transition=0;
+    transition = akm_ctl->drive.steering_angle*1000/2;
+    Send_Data.tx[8] = transition;
+    Send_Data.tx[7] = transition>>8;
+
+    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); 
+    Send_Data.tx[10]=FRAME_TAIL;
+
+    try {
+      Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
+    } catch (serial::IOException& e) {
+        RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
+    }
+}
+
+void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
+{
+  RCLCPP_INFO(this->get_logger(),"linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z);
+  std::cout<<"linerx"<<twist_aux->linear.x<<std::endl;
+  std::cout<<"angular"<<twist_aux->angular.z<<std::endl;
+    short  transition;
+    Send_Data.tx[0]=FRAME_HEADER;
+    Send_Data.tx[1] = 0;
+    Send_Data.tx[2] = 0; 
+
+    transition=0;
+    transition = twist_aux->linear.x*1000;
+    Send_Data.tx[4] = transition;
+    Send_Data.tx[3] = transition>>8;
+
+    transition=0;
+    transition = twist_aux->linear.y*1000;
+    Send_Data.tx[6] = transition;
+    Send_Data.tx[5] = transition>>8;
+
+    transition=0;
+    transition = (twist_aux->angular.z)*1000;
+    Send_Data.tx[8] = transition;
+    Send_Data.tx[7] = transition>>8;
+
+    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
+    Send_Data.tx[10]=FRAME_TAIL;
+
+    try 
+    {
+      Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
+    } catch (serial::IOException& e) {
+        RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
+    }
+}
+
+void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
+{
+  (void)sign_switch;
+    if (sign_switch->data == -1) {
+         memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+         memset(&Mpu6050, 0, sizeof(Mpu6050));
+         q0 = 0;
+         q1 = 0;
+         q2 = 0;
+         q3 = 0;
+         Robot_Pos.X = 0.54;
+         Robot_Pos.Y = 0.2;
+         Robot_Pos.Z = 0;
+         Robot_Vel.X = 0;
+         Robot_Vel.Y = 0;
+         Robot_Vel.Z = 0;
+         memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+         reset_filter();
+     }
+    else if(sign_switch->data == -2) {
+         memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+         Robot_Pos.X = 2.54;
+         Robot_Pos.Y = 2.5;
+         Robot_Pos.Z = -1.5708;
+         Robot_Vel.X = 0;
+         Robot_Vel.Y = 0;
+         Robot_Vel.Z = 0;
+         memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+         reset_filter();
+     }
+     /* 
+     else if (sign_switch->data == 6) {
+         memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+         Robot_Pos.X = 2;
+         Robot_Pos.Y = 2;
+         memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+     }*/
+     
+}
+
+void origincar_base::Publish_ImuSensor()
+{
+    sensor_msgs::msg::Imu Imu_Data_Pub;
+    Imu_Data_Pub.header.stamp = rclcpp::Node::now();
+    Imu_Data_Pub.header.frame_id = gyro_frame_id; 
+                                                  
+    Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
+    Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
+    Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
+    Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
+    Imu_Data_Pub.orientation_covariance[0] = 1e6; 
+    Imu_Data_Pub.orientation_covariance[4] = 1e6;
+    Imu_Data_Pub.orientation_covariance[8] = 1e-6;
+    Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
+    Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
+    Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
+    Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
+    Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
+    Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
+    Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
+    Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
+    Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;
+
+    imu_publisher->publish(Imu_Data_Pub);
+
+}
+
+void origincar_base::Publish_Odom()
+{
+    tf2::Quaternion q;
+    q.setRPY(0,0,Robot_Pos.Z);
+    geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);
+    
+    origincar_msg::msg::Data robotpose;
+    origincar_msg::msg::Data robotvel;
+    nav_msgs::msg::Odometry odom;
+
+    odom.header.stamp = rclcpp::Node::now();
+    odom.header.frame_id = odom_frame_id;
+    odom.child_frame_id = robot_frame_id;
+
+    odom.pose.pose.position.x = Robot_Pos.X;
+    odom.pose.pose.position.y = Robot_Pos.Y;
+
+    odom.pose.pose.position.z = 0.0;
+    odom.pose.pose.orientation = odom_quat;
+    //odom_quat;
+
+
+    odom.twist.twist.linear.x =  Robot_Vel.X;
+    odom.twist.twist.linear.y =  Robot_Vel.Y;
+    odom.twist.twist.angular.z = Robot_Vel.Z; 
+
+    robotpose.x = Robot_Pos.X;
+    robotpose.y = Robot_Pos.Y;
+    robotpose.z = Robot_Pos.Z;
+
+    robotvel.x = Robot_Vel.X;
+    robotvel.y = Robot_Vel.Y;
+    robotvel.z = Robot_Vel.Z;
+
+    geometry_msgs::msg::TransformStamped t;
+
+    t.header.stamp = rclcpp::Node::now();
+    t.header.frame_id = odom_frame_id;
+    t.child_frame_id  = robot_frame_id;
+
+    t.transform.translation.x = Robot_Pos.X;
+    t.transform.translation.y = Robot_Pos.Y;
+    t.transform.translation.z = 0.0;
+
+    t.transform.rotation = odom_quat;
+    tf_broadcaster_->sendTransform(t);
+    odom_publisher->publish(odom);
+    robotpose_publisher->publish(robotpose);
+    robotvel_publisher->publish(robotvel); 
+}
+
+void origincar_base::Publish_Voltage()
+{
+    std_msgs::msg::Float32 voltage_msgs;
+    static float Count_Voltage_Pub = 0;
+
+    if (Count_Voltage_Pub++ > 10) {
+        Count_Voltage_Pub = 0;
+        voltage_msgs.data = Power_voltage;
+        voltage_publisher->publish(voltage_msgs);
+    }
+}
+
+unsigned char origincar_base::Check_Sum(unsigned char Count_Number,unsigned char mode)
+{
+    unsigned char check_sum = 0, k;
+
+    if (mode == 0) {
+      for(k=0; k < Count_Number; k++) {
+        check_sum = check_sum^Receive_Data.rx[k];
+      }
+    } else if (mode == 1) {
+      for (k=0; k < Count_Number; k++) {
+        check_sum = check_sum^Send_Data.tx[k];
+      }
+    }
+
+    return check_sum;
+}
+
+bool origincar_base::Get_Sensor_Data()
+{
+    short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
+    uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0}; 
+    Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); 
+    for (j = 0; j < 24; j++) {
+      if (Receive_Data_Pr[j] == FRAME_HEADER)
+      Header_Pos=j;
+      else if (Receive_Data_Pr[j] == FRAME_TAIL)
+      Tail_Pos = j;
+    }
+
+    if (Tail_Pos == (Header_Pos + 23)) {
+      memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
+    }  else if (Header_Pos == (1 + Tail_Pos)) {
+      for (j = 0;j < 24; j++)
+      Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24];
+    }  else {
+    return false;
+    }
+
+    Receive_Data.Frame_Header = Receive_Data.rx[0];
+    Receive_Data.Frame_Tail = Receive_Data.rx[23];
+    if (Receive_Data.Frame_Header == FRAME_HEADER) {
+      if (Receive_Data.Frame_Tail == FRAME_TAIL) {
+        if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) {
+          Receive_Data.Flag_Stop=Receive_Data.rx[1];
+          Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);
+        
+          Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);
+                                                                          
+          //Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); 
+
+          Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);
+          Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]);
+          Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]);
+          Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);
+          Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);
+          Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);
+
+          Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
+          Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
+          Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
+
+          Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
+          Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
+          
+          if(Init_imu_num<100)
+          {
+            Init_imu_num += 1;
+            gyro_z_sum +=  Mpu6050_Data.gyros_z_data ;
+            Mpu6050.angular_velocity.z = 0;
+          }
+          else
+          {
+            Mpu6050.angular_velocity.z = (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num))*GYROSCOPE_RATIO;
+            //RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z);
+          }
+          Robot_Vel.Z = Mpu6050.angular_velocity.z;
+          transition_16 = 0;
+          transition_16 |=  Receive_Data.rx[20]<<8;
+          transition_16 |=  Receive_Data.rx[21];
+          Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001;
+
+          return true;
+        }
+      }
+    }
+
+    return false;
+}
+
+void origincar_base::Control()
+{
+    rclcpp::Time current_time, last_time;
+    current_time = rclcpp::Node::now();
+    last_time = rclcpp::Node::now();
+    while(rclcpp::ok()) {
+      current_time = rclcpp::Node::now();
+      Sampling_Time = (current_time - last_time).seconds();
+      if (true == Get_Sensor_Data()) {
+        Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
+        Robot_Pos.Y+=1.01*(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;//1.125
+        Robot_Pos.Z+= Mpu6050.angular_velocity.z * Sampling_Time;
+
+        Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
+                  Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
+        Publish_ImuSensor();
+        Publish_Voltage();
+        Publish_Odom();
+        rclcpp::spin_some(this->get_node_base_interface());
+      }
+      last_time = current_time;  
+    }
+}
+
+origincar_base::origincar_base()
+: rclcpp::Node ("origincar_base")
+{
+  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+  memset(&Receive_Data, 0, sizeof(Receive_Data));
+  memset(&Send_Data, 0, sizeof(Send_Data));
+  memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));
+  Robot_Pos.X = 0.54;
+  Robot_Pos.Y = 0.2;
+
+  int serial_baud_rate = 115200;
+
+  this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
+  this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
+  this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
+  this->declare_parameter<std::string>("odom_frame_id", "odom");
+  this->declare_parameter<std::string>("robot_frame_id", "base_link");
+  this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
+
+  this->get_parameter("serial_baud_rate", serial_baud_rate);
+  this->get_parameter("usart_port_name", usart_port_name);
+  this->get_parameter("cmd_vel", cmd_vel);
+  this->get_parameter("akm_cmd_vel", akm_cmd_vel);
+  this->get_parameter("odom_frame_id", odom_frame_id);
+  this->get_parameter("robot_frame_id", robot_frame_id);
+  this->get_parameter("gyro_frame_id", gyro_frame_id);
+
+  odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 10);
+
+  imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 10);
+
+  voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);
+
+  robotpose_publisher = create_publisher<origincar_msg::msg::Data>("robotpose", 10);
+
+  robotvel_publisher = create_publisher<origincar_msg::msg::Data>("robotvel", 10);
+  
+  pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+            "/set_pose", 
+            rclcpp::SystemDefaultsQoS().reliable());
+  
+  tf_bro = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+  Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>(
+      cmd_vel, 1, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1));
+  Akm_Cmd_Vel_Sub = create_subscription<ackermann_msgs::msg::AckermannDriveStamped>(
+      akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1));
+
+  Sign_Switch_Sub = create_subscription<std_msgs::msg::Int32>(
+       "/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1));
+  try  {
+    Stm32_Serial.setPort("/dev/ttyACM0");
+    Stm32_Serial.setBaudrate(serial_baud_rate);
+    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
+    Stm32_Serial.setTimeout(_time);
+    Stm32_Serial.open();
+  } catch (serial::IOException& e) {
+    RCLCPP_ERROR(this->get_logger(),"origincar_base can not open serial port,Please check the serial port cable! ");
+  }
+  if(Stm32_Serial.isOpen()) {
+    RCLCPP_INFO(this->get_logger(),"origincar_base serial port opened");
+  }
+}
+
+
+void sigintHandler(int sig)
+{
+    sig = sig;
+      printf("OriginBot shutdown...\n");
+    serial::Serial Stm32_Serial;
+    Stm32_Serial.setPort("/dev/ttyACM0");
+    Stm32_Serial.setBaudrate(115200);
+    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
+    Stm32_Serial.setTimeout(_time);
+    Stm32_Serial.open();                                       
+    SEND_DATA Send_Data;
+    if (Stm32_Serial.isOpen()) {
+    Send_Data.tx[0]=FRAME_HEADER;
+    Send_Data.tx[1] = 0;
+    Send_Data.tx[2] = 0;
+
+    Send_Data.tx[4] = 0;
+    Send_Data.tx[3] = 0;
+
+    Send_Data.tx[6] = 0;
+    Send_Data.tx[5] = 0;
+
+    Send_Data.tx[7] = 0;
+    Send_Data.tx[8] = 0;
+    int check_sum = 0;
+    for (int k = 0; k < 9; k++) {
+        check_sum = check_sum^Send_Data.tx[k];
+      }
+    Send_Data.tx[9]=check_sum;
+    Send_Data.tx[10]=FRAME_TAIL;
+
+    try {
+        Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
+    } catch (serial::IOException& e) {
+    }
+
+  }
+    // 关闭ROS2接口，清除资源
+    rclcpp::shutdown();
+}
+
+origincar_base::~origincar_base()
+{
+  RCLCPP_INFO(this->get_logger(),"Shutting down");
+}

src/origincar_description/urdf/origincar.urdf

@@ -83,7 +83,7 @@
     </inertial>
   </link>
   <joint name="fr_left_steer_joint" type="revolute">
-    <origin xyz="0.0841 0.0945 -0.03" rpy="0 0 0" />
+    <origin xyz="0.0715 0.0945 -0.03" rpy="0 0 0" />
     <parent link="chassis_link" />
     <child link="fr_left_steer_link" />
     <axis xyz="0 0 1" />
@@ -136,7 +136,7 @@
     </inertial>
   </link>
   <joint name="fr_right_steer_joint" type="revolute">
-    <origin xyz="0.0841 -0.0945 -0.03" rpy="0 0 0" />
+    <origin xyz="0.0715 -0.0945 -0.03" rpy="0 0 0" />
     <parent link="chassis_link" />
     <child link="fr_right_steer_link" />
     <axis xyz="0 0 1" />
@@ -205,7 +205,7 @@
     </collision>
   </link>
   <joint name="re_left_wheel_joint" type="continuous">
-    <origin xyz="-0.0841 0.0945 -0.03" rpy="1.5708 0 0" />
+    <origin xyz="-0.0715 0.0945 -0.03" rpy="1.5708 0 0" />
     <parent link="chassis_link" />
     <child link="re_left_wheel_link" />
     <axis xyz="0 0 -1" />
@@ -239,7 +239,7 @@
     </collision>
   </link>
   <joint name="re_right_wheel_joint" type="continuous">
-    <origin xyz="-0.0841 -0.0945 -0.03" rpy="-1.5708 0 0" />
+    <origin xyz="-0.0715 -0.0945 -0.03" rpy="-1.5708 0 0" />
     <parent link="chassis_link" />
     <child link="re_right_wheel_link" />
     <axis xyz="0 0 1" />
@@ -265,7 +265,7 @@
     </inertial>
   </link>
   <joint name="virtual_steering_wheel_joint" type="revolute">
-    <origin xyz="0.0841 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <origin xyz="0.0715 0.0 0.0" rpy="0.0 0.0 0.0" />
     <parent link="chassis_link" />
     <child link="virtual_steer_link" />
     <axis xyz="-1 0 0" />

src/origincar_msg/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(origincar_msg)
+
+# Default to C++14
+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_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Data.msg"
+  "msg/Sign.msg"
+  DEPENDENCIES std_msgs
+  ADD_LINTER_TESTS
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

src/origincar_msg/msg/Data.msg

@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 z

src/origincar_msg/msg/Sign.msg

@@ -0,0 +1 @@
+int32 sign_data

src/origincar_msg/package.xml

@@ -0,0 +1,21 @@
+<?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>origincar_msg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ps-micro@todo.todo">ps-micro</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>